added visualization with a ros flag, which shows feature with projection and residual (the features apparent movement)

include/msckf_vio/feature.hpp

@@ -171,8 +171,8 @@ struct Feature {
   bool VisualizePatch(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
-                  CameraCalibration& cam0) const;
+                  CameraCalibration& cam0,
-
+                  const std::vector<double> photo_r) const;
   /*
   * @brief projectPixelToPosition uses the calcualted pixels
   *     of the anchor patch to generate 3D positions of all of em
@@ -204,6 +204,7 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p,
   // NxN Patch of Anchor Image
   std::vector<double> anchorPatch;
   std::vector<cv::Point2f> anchorPatch_ideal;
+  std::vector<cv::Point2f> anchorPatch_real;
 
   // Position of NxN Patch in 3D space
   std::vector<Eigen::Vector3d> anchorPatch_3d;
@@ -408,43 +409,104 @@ bool Feature::estimate_FrameIrradiance(
 bool Feature::VisualizePatch(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
-                  CameraCalibration& cam0) const
+                  CameraCalibration& cam0,
+                  const std::vector<double> photo_r) const
 {
 
+  double rescale = 1;
+
+  //visu - anchor
+  auto anchor = observations.begin();
+  cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image;
+  cv::Mat dottedFrame(anchorImage.size(), CV_8UC3);
+  cv::cvtColor(anchorImage, dottedFrame, CV_GRAY2RGB);
+
+  for(auto point : anchorPatch_real)
+  {
+    // visu - feature
+    cv::Point xs(point.x, point.y);
+    cv::Point ys(point.x, point.y);
+    cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0));
+  }  
+  cam0.featureVisu = dottedFrame.clone();
+
   // visu - feature
   cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
-  cv::Mat dottedFrame(current_image.size(), CV_8UC3);
   cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB);
-
-  // project every point in anchorPatch_3d.
-  auto frame = cam0.moving_window.find(cam_state_id)->second.image;
   
+  // set position in frame 
+  // save irradiance of projection
+  std::vector<double> projectionPatch;
   for(auto point : anchorPatch_3d)
   {
     cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point);
-
+    projectionPatch.push_back(PixelIrradiance(p_in_c0, current_image));
     // visu - feature
     cv::Point xs(p_in_c0.x, p_in_c0.y);
     cv::Point ys(p_in_c0.x, p_in_c0.y);
     cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0));
   }  
-  // testing
-  //if(cam_state_id == observations.begin()->first)
-    //if(count++ == 4)
-      //printf("dist:\n \tpos: %f, %f\n\ttrue pos: %f, %f\n\n", p_in_c0.x, p_in_c0.y, anchor_center_pos.x, anchor_center_pos.y);
 
-  // visu - feature
+  cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu);
-  cv::resize(dottedFrame, dottedFrame, cv::Size(dottedFrame.cols*0.2, dottedFrame.rows*0.2));
+
-  if(cam0.featureVisu.empty())
+  // irradiance grid anchor
-    cam0.featureVisu = dottedFrame.clone();
+  int N = sqrt(anchorPatch_3d.size());
-  else
+  int scale = 20;
-    cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu);
+  cv::Mat irradianceFrame(anchorImage.size(), CV_8UC3, cv::Scalar(255, 240, 255));
+  cv::resize(irradianceFrame, irradianceFrame, cv::Size(), rescale, rescale);
+  for(int i = 0; i<N; i++)
+    for(int j = 0; j<N; j++)
+      cv::rectangle(irradianceFrame, 
+                    cv::Point(10+scale*(i+1), 10+scale*j), 
+                    cv::Point(10+scale*i, 10+scale*(j+1)), 
+                    cv::Scalar(anchorPatch[i*N+j]*255, anchorPatch[i*N+j]*255, anchorPatch[i*N+j]*255),  
+                    CV_FILLED);
+
+  // irradiance grid projection
+  for(int i = 0; i<N; i++)
+    for(int j = 0; j<N ; j++)
+      cv::rectangle(irradianceFrame, 
+                    cv::Point(10+scale*(i+1), 20+scale*(N+j)), 
+                    cv::Point(10+scale*(i), 20+scale*(N+j+1)), 
+                    cv::Scalar(projectionPatch[i*N+j]*255, projectionPatch[i*N+j]*255, projectionPatch[i*N+j]*255),  
+                    CV_FILLED);
+
+  // residual grid projection, positive - red, negative - blue colored 
+  for(int i = 0; i<N; i++)
+    for(int j = 0; j<N; j++)
+      if(photo_r[i*N+j]>0)
+        cv::rectangle(irradianceFrame, 
+                      cv::Point(20+scale*(N+i+1), 15+scale*(N/2+j)), 
+                      cv::Point(20+scale*(N+i), 15+scale*(N/2+j+1)), 
+                      cv::Scalar(255 - photo_r[i*N+j]*255, 255 - photo_r[i*N+j]*255, 255),  
+                      CV_FILLED);
+      else
+        cv::rectangle(irradianceFrame, 
+                      cv::Point(20+scale*(N+i+1), 15+scale*(N/2+j)), 
+                      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::resize(cam0.featureVisu, cam0.featureVisu, cv::Size(), rescale, rescale);
+
+  cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu);
+
+  // create line?
+
+  //save image
+
+  std::stringstream ss;
+  ss << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg";
+  cv::imwrite(ss.str(), cam0.featureVisu);
+
+  //cv::imshow("patch", cam0.featureVisu);
+  //cvWaitKey(1);
 }
 
 float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
 {
-  
+
-  return ((float)image.at<uint8_t>(pose.x, pose.y))/256;
+  return ((float)image.at<uint8_t>(pose.y, pose.x))/255;
 }
 
 cv::Point2f Feature::projectPositionToCamera(
@@ -514,16 +576,16 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
   int count = 0;
 
   // get feature in undistorted pixel space
+  // this only reverts from 'pure' space into undistorted pixel space using camera matrix
   cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v), 
                                                     cam.intrinsics, 
                                                     cam.distortion_model, 
                                                     0);
   // create vector of patch in pixel plane
-  std::vector<cv::Point2f> und_pix_v;
+  std::vector<cv::Point2f>und_pix_v;
   for(double u_run = -n; u_run <= n; u_run++)
     for(double v_run = -n; v_run <= n; v_run++)
-      und_pix_v.push_back(cv::Point2f(und_pix_p.x-u_run, und_pix_p.y-v_run));
+      und_pix_v.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run));
-  
 
 
   //create undistorted pure points    
@@ -533,23 +595,24 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
                                 cam.distortion_model,
                                 0,
                                 und_v);
+
   //create distorted pixel points
-  std::vector<cv::Point2f> vec = image_handler::distortPoints(und_v,
+  anchorPatch_real = image_handler::distortPoints(und_v,
-                                                             cam.intrinsics,
+                                                   cam.intrinsics,
-                                                             cam.distortion_model,
+                                                   cam.distortion_model,
-                                                             cam.distortion_coeffs);
+                                                   cam.distortion_coeffs);
 
 
   // save anchor position for later visualisaztion
-  anchor_center_pos = vec[4];
+  anchor_center_pos = anchorPatch_real[(N*N-1)/2];
 
   // save true pixel Patch position
-  for(auto point : vec)
+  for(auto point : anchorPatch_real)
   {
     if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1))
       return false;
   }
-  for(auto point : vec)
+  for(auto point : anchorPatch_real)
     anchorPatch.push_back(PixelIrradiance(point, anchorImage));
 
   // project patch pixel to 3D space

include/msckf_vio/msckf_vio.h

@@ -202,7 +202,9 @@ class MsckfVio {
     void onlineReset();
 
     // Photometry flag
+    // visualization flag
     bool PHOTOMETRIC;
+    bool PRINTIMAGES;
 
     bool nan_flag;
 

launch/msckf_vio_tum.launch

@@ -19,8 +19,9 @@
 
       <!-- Photometry Flag-->
       <param name="PHOTOMETRIC" value="true"/>
+      <param name="PrintImages" value="false"/>
 
-      <param name="patch_size_n" value="5"/>
+      <param name="patch_size_n" value="7"/>
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 

src/msckf_vio.cpp

@@ -63,7 +63,7 @@ MsckfVio::MsckfVio(ros::NodeHandle& pnh):
 bool MsckfVio::loadParameters() {
   //Photometry Flag
   nh.param<bool>("PHOTOMETRIC", PHOTOMETRIC, false);
-  
+  nh.param<bool>("PrintImages", PRINTIMAGES, false);
 
   // Frame id
   nh.param<string>("fixed_frame_id", fixed_frame_id, "world");
@@ -1027,11 +1027,11 @@ void MsckfVio::PhotometricMeasurementJacobian(
     //add observation
     photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame));
 
-    //add jacobian
+    // add jacobian
 
     // frame derivative calculated convoluting with kernel [-1, 0, 1]
-    dx = feature.PixelIrradiance(cv::Point2f(p_in_c0.x+1, p_in_c0.y), frame)- feature.PixelIrradiance(cv::Point2f(p_in_c0.x-1, p_in_c0.y), frame);
+    dx = feature.PixelIrradiance(cv::Point2f(p_in_c0.x+1, p_in_c0.y), frame) - feature.PixelIrradiance(cv::Point2f(p_in_c0.x-1, p_in_c0.y), frame);
-    dy = feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y+1), frame)- feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y-1), frame);
+    dy = feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y+1), frame) - feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y-1), frame);
     dI_dhj(0, 0) = dx;
     dI_dhj(0, 1) = dy;
     
@@ -1043,7 +1043,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
 
     //dh / d X_{pl}
     dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * skewSymmetric(point);
-    dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation).transpose();
+    dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation);//.transpose();
 
     //d{}^Gp_P{ij} / \rho_i
     double rho = feature.anchor_rho;
@@ -1074,6 +1074,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
   std::vector<double> estimate_photo_z;
   IlluminationParameter estimated_illumination;
   feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination);
+  
   for (auto& estimate_irradiance_j : estimate_irradiance)
             estimate_photo_z.push_back (estimate_irradiance_j * 
                     estimated_illumination.frame_gain * estimated_illumination.feature_gain +
@@ -1121,6 +1122,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
   for(auto data : photo_r)
     r[count++] = data;
 
+  if(PRINTIMAGES)
+    feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r);
+
   return;
 }
 
@@ -1151,15 +1155,8 @@ void MsckfVio::PhotometricFeatureJacobian(
   VectorXd r_i = VectorXd::Zero(jacobian_row_size);
   int stack_cntr = 0;
 
-  // visu - residual
-  //printf("_____FEATURE:_____\n");
-  // visu - feature
-  //cam0.featureVisu.release();
-
   for (const auto& cam_id : valid_cam_state_ids) {
 
-    //Matrix<double, 4, 6> H_xi = Matrix<double, 4, 6>::Zero();
-    //Matrix<double, 4, 3> H_fi = Matrix<double, 4, 3>::Zero();
     MatrixXd H_xl;
     MatrixXd H_yl;
     Eigen::VectorXd r_l = VectorXd::Zero(N*N);
@@ -1176,27 +1173,11 @@ void MsckfVio::PhotometricFeatureJacobian(
     r_i.segment(stack_cntr, N*N) = r_l;
     stack_cntr += N*N;
   }
-  // visu - feature
-  /*
-  if(!cam0.featureVisu.empty() && cam0.featureVisu.size().width > 3000)
-    imshow("feature", cam0.featureVisu);
-  cvWaitKey(1);
-
-  if((ros::Time::now() - image_save_time).toSec() > 1)
-  {
-    std::stringstream ss;
-    ss << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg";
-    imwrite(ss.str(), cam0.featureVisu);
-    image_save_time = ros::Time::now();
-  }
-  */
 
   // Project the residual and Jacobians onto the nullspace
   // of H_yj.
 
   // get Nullspace
-
-  
   JacobiSVD<MatrixXd> svd_helper(H_yi, ComputeFullU | ComputeThinV);
   int sv_size = 0;
   Eigen::VectorXd singularValues = svd_helper.singularValues();
@@ -1339,8 +1320,6 @@ void MsckfVio::featureJacobian(
    H_x = A.transpose() * H_xj;
    r = A.transpose() * r_j;
 
-   cout << "A: \n" << A.transpose() << endl;
-
   return;
 }
 
@@ -1348,8 +1327,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
 
   if (H.rows() == 0 || r.rows() == 0) return;
 
-
-  cout << "Updating...";
   // Decompose the final Jacobian matrix to reduce computational
   // complexity as in Equation (28), (29).
   MatrixXd H_thin;