added 3d visualization and stepping through bag file - minor edits in jakobi

CMakeLists.txt

@@ -24,6 +24,7 @@ find_package(catkin REQUIRED COMPONENTS
   pcl_conversions
   pcl_ros
   std_srvs
+  visualization_msgs
 )
 
 ## System dependencies are found with CMake's conventions

include/msckf_vio/feature.hpp

@@ -16,12 +16,17 @@
 #include <Eigen/Geometry>
 #include <Eigen/StdVector>
 
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <geometry_msgs/Point.h>
+
 #include "image_handler.h"
 
 #include "math_utils.hpp"
 #include "imu_state.h"
 #include "cam_state.h"
 
+
 namespace msckf_vio {
 
 /*
@@ -168,6 +173,10 @@ struct Feature {
                   std::vector<double>& anchorPatch_estimate,
                   IlluminationParameter& estimatedIllumination) const;
 
+bool MarkerGeneration(
+                  ros::Publisher& marker_pub,
+                  const CamStateServer& cam_states) const;
+
   bool VisualizePatch(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
@@ -207,7 +216,7 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p,
   std::vector<cv::Point2f> anchorPatch_ideal;
   std::vector<cv::Point2f> anchorPatch_real;
 
-  // Position of NxN Patch in 3D space
+  // Position of NxN Patch in 3D space in anchor camera frame
   std::vector<Eigen::Vector3d> anchorPatch_3d;
 
   // Anchor Isometry
@@ -406,6 +415,118 @@ bool Feature::estimate_FrameIrradiance(
   }
 }
 
+// generates markers for every camera position/observation
+// and estimated feature/path position
+bool Feature::MarkerGeneration(
+                  ros::Publisher& marker_pub,
+                  const CamStateServer& cam_states) const
+{
+  visualization_msgs::MarkerArray ma;
+
+  // add all camera states used for estimation
+  int count = 0;
+
+  for(auto observation : observations)
+  {
+    visualization_msgs::Marker marker;
+    marker.header.frame_id = "world";
+    marker.header.stamp = ros::Time::now();
+
+    marker.ns = "cameras";
+    marker.id = count++;
+    marker.type =  visualization_msgs::Marker::ARROW;
+    marker.action = visualization_msgs::Marker::ADD;
+
+    marker.pose.position.x = cam_states.find(observation.first)->second.position(0);
+    marker.pose.position.y = cam_states.find(observation.first)->second.position(1);
+    marker.pose.position.z = cam_states.find(observation.first)->second.position(2);
+
+    // rotate form x to z axis
+    Eigen::Vector4d q = quaternionMultiplication(Eigen::Vector4d(0, -0.707, 0, 0.707), cam_states.find(observation.first)->second.orientation);
+
+    marker.pose.orientation.x = q(0);
+    marker.pose.orientation.y = q(1);
+    marker.pose.orientation.z = q(2);
+    marker.pose.orientation.w = q(3);
+
+    marker.scale.x = 0.15;
+    marker.scale.y = 0.05;
+    marker.scale.z = 0.05;
+
+    if(count == 1)
+    {
+        marker.color.r = 0.0f;
+        marker.color.g = 0.0f;
+        marker.color.b = 1.0f;
+    }
+    else
+    {
+        marker.color.r = 0.0f;
+        marker.color.g = 1.0f;
+        marker.color.b = 0.0f;
+    }
+    marker.color.a = 1.0;
+
+    marker.lifetime = ros::Duration(0);
+
+    ma.markers.push_back(marker);
+  }
+
+  // 'delete' any existing cameras (make invisible)
+  for(int i = count; i < 20; i++)
+  {
+    visualization_msgs::Marker marker;
+
+    marker.header.frame_id = "world";
+    marker.header.stamp = ros::Time::now();
+
+    marker.ns = "cameras";
+    marker.id = i;
+    marker.type =  visualization_msgs::Marker::ARROW;
+    marker.action = visualization_msgs::Marker::ADD;
+
+    marker.pose.orientation.w = 1;
+
+    marker.color.a = 0.0;
+
+    marker.lifetime = ros::Duration(1);
+
+    ma.markers.push_back(marker);
+  }
+
+  //generate feature patch points position
+  visualization_msgs::Marker marker;
+  
+  marker.header.frame_id = "world";
+  marker.header.stamp = ros::Time::now();
+
+  marker.ns = "patch";
+  marker.id = 0;
+  marker.type = visualization_msgs::Marker::POINTS;
+  marker.action = visualization_msgs::Marker::ADD;
+
+  marker.pose.orientation.w = 1;
+
+  marker.scale.x = 0.02;
+  marker.scale.y = 0.02;
+
+  marker.color.r = 1.0f;
+  marker.color.g = 0.0f;
+  marker.color.b = 0.0f;
+  marker.color.a = 1.0;
+
+  for(auto point : anchorPatch_3d)
+  {
+    geometry_msgs::Point p;
+    p.x = point(0);
+    p.y = point(1);
+    p.z = point(2);
+    marker.points.push_back(p);
+  }
+  ma.markers.push_back(marker);
+
+  marker_pub.publish(ma);
+}
 
 bool Feature::VisualizePatch(
                   const CAMState& cam_state,
@@ -489,12 +610,15 @@ bool Feature::VisualizePatch(
                       cv::Point(20+scale*(N+i), 15+scale*(N/2+j+1)), 
                       cv::Scalar(255, 255 + photo_r[i*N+j]*255, 255 + photo_r[i*N+j]*255), 
                       CV_FILLED);
-      
 
+      cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu);
+      
+/*
   // visualize position of used observations and resulting feature position
   cv::Mat positionFrame(anchorImage.size(), CV_8UC3, cv::Scalar(255, 240, 255));
   cv::resize(positionFrame, positionFrame, cv::Size(), rescale, rescale);
 
+
   // draw world zero
   cv::line(positionFrame,
           cv::Point(20,20),
@@ -510,20 +634,20 @@ bool Feature::VisualizePatch(
   // for every observation, get cam state
   for(auto obs : observations)
   {
-      cam_state.find(obs->first);
       cv::line(positionFrame,
         cv::Point(20,20),
         cv::Point(30,20),
         cv::Scalar(255,0,0),
         CV_FILLED);
   }
+
   // draw, x y position and arrow with direction - write z next to it
 
   cv::resize(cam0.featureVisu, cam0.featureVisu, cv::Size(), rescale, rescale);
 
-  cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu);
-  cv::hconcat(cam0.featureVisu, positionFrame, cam0.featureVisu);
 
+  cv::hconcat(cam0.featureVisu, positionFrame, cam0.featureVisu);
+*/
   // write feature position
   std::stringstream pos_s;
   pos_s << "u: " << observations.begin()->second(0) << " v: " << observations.begin()->second(1);
@@ -534,11 +658,11 @@ bool Feature::VisualizePatch(
   //save image
 
   std::stringstream loc;
-  loc << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg";
+ // loc << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg";
-  cv::imwrite(loc.str(), cam0.featureVisu);
+  //cv::imwrite(loc.str(), cam0.featureVisu);
 
-  //cv::imshow("patch", cam0.featureVisu);
+  cv::imshow("patch", cam0.featureVisu);
-  //cvWaitKey(1);
+  cvWaitKey(0);
 }
 
 float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
@@ -586,7 +710,7 @@ Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, const CameraCa
   // save resulting NxN positions for this feature
 
   Eigen::Vector3d PositionInCamera(in_p.x/anchor_rho, in_p.y/anchor_rho, 1/anchor_rho);
-  Eigen::Vector3d PositionInWorld= T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation();
+  Eigen::Vector3d PositionInWorld = T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation();
   return PositionInWorld;
   //printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]);
 }
@@ -653,7 +777,7 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
   for(auto point : anchorPatch_real)
     anchorPatch.push_back(PixelIrradiance(point, anchorImage));
 
-  // project patch pixel to 3D space
+  // project patch pixel to 3D space in camera  coordinate system
   for(auto point : und_v)
   {
     anchorPatch_ideal.push_back(point);

include/msckf_vio/math_utils.hpp

@@ -43,6 +43,21 @@ inline void quaternionNormalize(Eigen::Vector4d& q) {
   return;
 }
 
+/*
+ * @brief invert rotation of passed quaternion through conjugation
+ *        and return conjugation
+ */
+inline Eigen::Vector4d quaternionConjugate(Eigen::Vector4d& q)
+{
+  Eigen::Vector4d p;
+  p(0) = -q(0);
+  p(1) = -q(1);
+  p(2) = -q(2);
+  p(3) =  q(3);
+
+  return p;
+}
+
 /*
  * @brief Perform q1 * q2
  */

include/msckf_vio/msckf_vio.h

@@ -181,6 +181,9 @@ class MsckfVio {
         const Eigen::Vector3d& gyro,
         const Eigen::Vector3d& acc);
 
+    // groundtruth state augmentation
+    void truePhotometricStateAugmentation(const double& time);
+
     // Measurement update
     void stateAugmentation(const double& time);
     void PhotometricStateAugmentation(const double& time);
@@ -230,7 +233,7 @@ class MsckfVio {
     bool GROUNDTRUTH;
 
     bool nan_flag;
-
+    bool play;
     double last_time_bound;
 
     // Patch size for Photometry
@@ -241,6 +244,13 @@ class MsckfVio {
 
     // State vector
     StateServer state_server;
+
+    // Ground truth state vector 
+    StateServer true_state_server;
+
+    // error state based on ground truth
+    StateServer err_state_server;
+
     // Maximum number of camera states
     int max_cam_state_size;
 
@@ -297,6 +307,7 @@ class MsckfVio {
     ros::Subscriber imu_sub;
     ros::Subscriber truth_sub;
     ros::Publisher odom_pub;
+    ros::Publisher marker_pub;
     ros::Publisher feature_pub;
     tf::TransformBroadcaster tf_pub;
     ros::ServiceServer reset_srv;

launch/msckf_vio_debug_tum.launch

@@ -21,6 +21,12 @@
       <!-- Photometry Flag-->
       <param name="PHOTOMETRIC" value="true"/>
 
+      <!-- Debugging Flaggs -->
+      <param name="PrintImages" value="true"/>
+      <param name="GroundTruth" value="true"/>
+
+      <param name="patch_size_n" value="7"/>
+
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 

package.xml

@@ -18,6 +18,7 @@
   <depend>nav_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
   <depend>eigen_conversions</depend>
   <depend>tf_conversions</depend>
   <depend>random_numbers</depend>

src/msckf_vio.cpp

@@ -27,6 +27,10 @@
 #include <msckf_vio/math_utils.hpp>
 #include <msckf_vio/utils.h>
 
+#include <rosbag/bag.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/String.h>
+
 
 using namespace std;
 using namespace Eigen;
@@ -248,9 +252,14 @@ bool MsckfVio::loadParameters() {
 }
 
 bool MsckfVio::createRosIO() {
+
+  // activating bag playing parameter, for debugging
+  nh.setParam("/play_bag", true);
+
   odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);
   feature_pub = nh.advertise<sensor_msgs::PointCloud2>(
       "feature_point_cloud", 10);
+  marker_pub = nh.advertise<visualization_msgs::MarkerArray>("/visualization_marker_array", 10);
 
   reset_srv = nh.advertiseService("reset",
       &MsckfVio::resetCallback, this);
@@ -301,6 +310,19 @@ bool MsckfVio::initialize() {
   if (!createRosIO()) return false;
   ROS_INFO("Finish creating ROS IO...");
 
+  /*
+  rosbag::Bag bag;
+  bag.open("/home/raphael/dev/MSCKF_ws/bag/TUM/dataset-corridor1_1024_16.bag", rosbag::bagmode::Read);
+
+  for(rosbag::MessageInstance const m: rosbag::View(bag))
+  {
+    std_msgs::Int32::ConstPtr i = m.instantiate<std_msgs::Int32>();
+    if (i != NULL)
+      std::cout << i->data << std::endl;
+  }
+
+  bag.close();
+  */
   return true;
 }
 
@@ -349,10 +371,14 @@ void MsckfVio::imageCallback(
   // Propogate the IMU state.
   // that are received before the image feature_msg.
   ros::Time start_time = ros::Time::now();
-  if(!GROUNDTRUTH)
+  
-    batchImuProcessing(feature_msg->header.stamp.toSec());
+  
-  else
+  batchImuProcessing(feature_msg->header.stamp.toSec());
+  
+  // save true state in seperate state vector
+  if(GROUNDTRUTH)
     batchTruthProcessing(feature_msg->header.stamp.toSec());
+
   double imu_processing_time = (
       ros::Time::now()-start_time).toSec();
 
@@ -645,6 +671,19 @@ void MsckfVio::mocapOdomCallback(const nav_msgs::OdometryConstPtr& msg) {
   return;
 }
 
+void MsckfVio::calcErrorState()
+{
+  // true_state_server - state_server = err_state_server
+  StateServer errState;
+  errState.imu_state.id = state_server.imu_state.id;
+  errState.imu_state.time = state-server.imu_state.time;
+
+  errState.imu_state.orientation = quaternionMultiplication(true_state_server.imu_state.orientation, quaterionConjugate(state_server.imu_state.orientation));
+  errState.imu_state.position = true_state_server.imu_state.position - state_server.imu_state.position;
+  errState.imu_state.velocity = 
+
+}
+
 void MsckfVio::batchTruthProcessing(const double& time_bound) {
 
   // Counter how many IMU msgs in the buffer are used.
@@ -652,7 +691,7 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
 
   for (const auto& truth_msg : truth_msg_buffer) {
     double truth_time = truth_msg.header.stamp.toSec();
-    if (truth_time < state_server.imu_state.time) {
+    if (truth_time < true_state_server.imu_state.time) {
       ++used_truth_msg_cntr;
       continue;
     }
@@ -675,7 +714,7 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
   last_time_bound = time_bound;
 
   // Set the state ID for the new IMU state.
-  state_server.imu_state.id = IMUState::next_id++;
+  true_state_server.imu_state.id = IMUState::next_id++;
 
   // Remove all used Truth msgs.
   truth_msg_buffer.erase(truth_msg_buffer.begin(),
@@ -687,7 +726,7 @@ void MsckfVio::processTruthtoIMU(const double& time,
     const Vector4d& m_rot,
     const Vector3d& m_trans){
 
-  IMUState& imu_state = state_server.imu_state;
+  IMUState& imu_state = true_state_server.imu_state;
   double dtime = time - imu_state.time;
 
   Vector4d& q = imu_state.orientation;
@@ -706,7 +745,7 @@ void MsckfVio::processTruthtoIMU(const double& time,
   imu_state.velocity_null = imu_state.velocity;
 
   // Update the state info
-  state_server.imu_state.time = time;
+  true_state_server.imu_state.time = time;
 
 }
 
@@ -905,7 +944,6 @@ void MsckfVio::stateAugmentation(const double& time)
   cam_state.orientation_null = cam_state.orientation;
   cam_state.position_null = cam_state.position;
 
-
   // Update the covariance matrix of the state.
   // To simplify computation, the matrix J below is the nontrivial block
   // in Equation (16) in "A Multi-State Constraint Kalman Filter for Vision
@@ -945,6 +983,33 @@ void MsckfVio::stateAugmentation(const double& time)
   return;
 }
 
+void MsckfVio::truePhotometricStateAugmentation(const double& time)
+{
+  const Matrix3d& true_R_i_c = true_state_server.imu_state.R_imu_cam0;
+  const Vector3d& true_t_c_i = true_state_server.imu_state.t_cam0_imu;
+
+  // Add a new camera state to the state server.
+  Matrix3d true_R_w_i = quaternionToRotation(
+      true_state_server.imu_state.orientation);
+  Matrix3d true_R_w_c = R_i_c * R_w_i;
+  Vector3d true_t_c_w = true_state_server.imu_state.position +
+    R_w_i.transpose()*t_c_i;
+
+  true_state_server.cam_states[true_state_server.imu_state.id] =
+    CAMState(state_server.imu_state.id);
+  CAMState& true_cam_state = true_state_server.cam_states[
+    true_state_server.imu_state.id];
+
+  true_cam_state.time = time;
+  true_cam_state.orientation = rotationToQuaternion(true_R_w_c);
+  true_cam_state.position = t_c_w;
+
+  true_cam_state.orientation_null = true_cam_state.orientation;
+  true_cam_state.position_null = true_cam_state.position;
+
+  return;
+}
+
 void MsckfVio::PhotometricStateAugmentation(const double& time) 
 {
 
@@ -1068,9 +1133,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
   Matrix3d R_w_c1 = CAMState::T_cam0_cam1.linear() * R_w_c0;
   Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation();
 
-  // 3d feature position in the world frame.
-  // And its observation with the stereo cameras.
-  const Vector3d& p_w = feature.position;
 
   //photometric observation
   std::vector<double> photo_z;
@@ -1080,9 +1142,13 @@ void MsckfVio::PhotometricMeasurementJacobian(
   Matrix<double, 2, 3> dh_dCpij = Matrix<double, 2, 3>::Zero();
   Matrix<double, 2, 3> dh_dGpij = Matrix<double, 2, 3>::Zero();
   Matrix<double, 2, 6> dh_dXplj = Matrix<double, 2, 6>::Zero();
-  Matrix<double, 3, 1> dGpi_drhoj = Matrix<double, 3, 1>::Zero();
+  Matrix<double, 3, 1> dGpj_drhoj = Matrix<double, 3, 1>::Zero();
-  Matrix<double, 3, 6> dGpi_XpAj = Matrix<double, 3, 6>::Zero();
+  Matrix<double, 3, 6> dGpj_XpAj = Matrix<double, 3, 6>::Zero();
-  
+
+  Matrix<double, 3, 3> dCpij_dGpij = Matrix<double, 3, 3>::Zero();
+  Matrix<double, 3, 3> dCpij_dCGtheta = Matrix<double, 3, 3>::Zero();
+  Matrix<double, 3, 3> dCpij_dGpC = Matrix<double, 3, 3>::Zero();
+
   // one line of the NxN Jacobians
   Eigen::Matrix<double, 1, 1> H_rhoj;
   Eigen::Matrix<double, 1, 6> H_plj;
@@ -1099,7 +1165,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
   double dx, dy;
   for (auto point : feature.anchorPatch_3d)
   {
-    Eigen::Vector3d p_c0 = R_w_c0 * (p_w-t_c0_w);
+    Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w);
     cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point);
 
     //add observation
@@ -1118,27 +1184,31 @@ void MsckfVio::PhotometricMeasurementJacobian(
     dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2));
     dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2));
 
+    dCpij_dGpij = quaternionToRotation(cam_state.orientation);
+
     //orientation takes camera frame to world frame, we wa
-    dh_dGpij = dh_dCpij * quaternionToRotation(cam_state.orientation).transpose();
+    dh_dGpij = dh_dCpij * dCpij_dGpij;
 
     //dh / d X_{pl}
-    dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * skewSymmetric(point);
+    dCpij_dCGtheta = skewSymmetric(p_c0);
-    dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation).transpose();
+    dCpij_dGpC = -quaternionToRotation(cam_state.orientation);
+    dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * dCpij_dCGtheta;
+    dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * dCpij_dGpC;
 
     //d{}^Gp_P{ij} / \rho_i
     double rho = feature.anchor_rho;
-    dGpi_drhoj = feature.T_anchor_w.linear() * Eigen::Vector3d(-feature.anchorPatch_ideal[count].x/(rho*rho), -feature.anchorPatch_ideal[count].y/(rho*rho), -1/(rho*rho));
+    dGpj_drhoj = feature.T_anchor_w.linear() * Eigen::Vector3d(-feature.anchorPatch_ideal[count].x/(rho*rho), -feature.anchorPatch_ideal[count].y/(rho*rho), -1/(rho*rho));
 
-    dGpi_XpAj.block<3, 3>(0, 0) = - skewSymmetric(feature.T_anchor_w.linear()
+    dGpj_XpAj.block<3, 3>(0, 0) = - skewSymmetric(feature.T_anchor_w.linear()
                                  * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), 
                                                    feature.anchorPatch_ideal[count].y/(rho), 
                                                    1/(rho)));
-    dGpi_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
+    dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
 
     // Intermediate Jakobians
-    H_rhoj = dI_dhj * dh_dGpij * dGpi_drhoj; // 1 x 3
+    H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 3
     H_plj = dI_dhj * dh_dXplj; // 1 x 6
-    H_pAj = dI_dhj * dh_dGpij * dGpi_XpAj; // 1 x 6
+    H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6
 
     H_rho.block<1, 1>(count, 0) = H_rhoj;
     H_pl.block<1, 6>(count, 0) = H_plj;
@@ -1209,7 +1279,10 @@ void MsckfVio::PhotometricMeasurementJacobian(
   std::stringstream ss;
   ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << "  frame: " << cam_state_cntr;
   if(PRINTIMAGES)
+  {  
+    feature.MarkerGeneration(marker_pub, state_server.cam_states);
     feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss);
+  }
 
   return;
 }
@@ -1220,6 +1293,12 @@ void MsckfVio::PhotometricFeatureJacobian(
     MatrixXd& H_x, VectorXd& r) 
 {
 
+  // stop playing bagfile if printing images
+  if(PRINTIMAGES)
+  {
+    std::cout << "stopped playpack" << std::endl;
+    nh.setParam("/play_bag", false);
+  }
   const auto& feature = map_server[feature_id];
 
   // Check how many camera states in the provided camera
@@ -1278,6 +1357,11 @@ void MsckfVio::PhotometricFeatureJacobian(
   H_x = A.transpose() * H_xi;
   r = A.transpose() * r_i;
 
+  if(PRINTIMAGES)
+  {
+    std::cout << "resume playback" << std::endl;
+    nh.setParam("/play_bag", true);
+  }
   return;
 }