fixed incorrect undistortion/distortion. residual should now be calculated correctly

include/msckf_vio/feature.hpp

@@ -144,7 +144,12 @@ struct Feature {
   inline bool initializePosition(
       const CamStateServer& cam_states);
 
-
+/*
+*  @brief project PositionToCamera Takes a 3d position in a world frame
+*         and projects it into the passed camera frame using pinhole projection
+*         then distorts it using camera information to get 
+*         the resulting distorted pixel position  
+*/
   inline cv::Point2f projectPositionToCamera(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
@@ -212,12 +217,15 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p,
   Eigen::Vector3d position;
 
   // inverse depth representation
-  double rho;
+  double anchor_rho;
 
   // A indicator to show if the 3d postion of the feature
   // has been initialized or not.
   bool is_initialized;
   bool is_anchored;
+
+  cv::Point2f anchor_center_pos;
+  cv::Point2f undist_anchor_center_pos;
   // Noise for a normalized feature measurement.
   static double observation_noise;
 
@@ -397,14 +405,24 @@ bool Feature::FrameIrradiance(
                   const movingWindow& cam0_moving_window,
                   std::vector<float>& anchorPatch_measurement) const
 {
-  //project every point in anchorPatch_3d.
+  // project every point in anchorPatch_3d.
-  if(!is_anchored)
+  // int count = 0;
-    printf("not anchored!\n");
   for (auto point : anchorPatch_3d)
   {
+    // testing
+    //if(cam_state_id == observations.begin()->first)
+      //if(count == 4)
+        //printf("\n\ncenter:\n");
+
     cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point); 
     float irradiance = PixelIrradiance(p_in_c0, cam0_moving_window.find(cam_state_id)->second.image);
     anchorPatch_measurement.push_back(irradiance);
+    
+    // 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);
+    
   }
 }
 
@@ -429,18 +447,20 @@ cv::Point2f Feature::projectPositionToCamera(
   Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w);
 
   out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2));
-  std::vector<cv::Point2f> out_v;
+
-  out_v.push_back(out_p);
+ // if(cam_state_id == observations.begin()->first)
-  std::vector<cv::Point2f> my_p = image_handler::distortPoints(out_v, 
+      //printf("undist:\n  \tproj pos: %f, %f\n\ttrue pos: %f, %f\n", out_p.x, out_p.y, undist_anchor_center_pos.x, undist_anchor_center_pos.y);
-                                                              cam.intrinsics, 
+ 
-                                                              cam.distortion_model, 
+  cv::Point2f my_p = image_handler::distortPoint(out_p, 
-                                                              cam.distortion_coeffs);
+                                                 cam.intrinsics, 
-  
+                                                 cam.distortion_model, 
+                                                 cam.distortion_coeffs);
+
   // printf("truPosition: %f, %f, %f\n", position.x(), position.y(), position.z());
   // printf("camPosition: %f, %f, %f\n", p_c0(0), p_c0(1), p_c0(2));
   // printf("Photo projection: %f, %f\n", my_p[0].x,  my_p[0].y);
   
-  return my_p[0];
+  return my_p;
 }
 
 Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p,
@@ -450,7 +470,7 @@ Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p,
   // project it back into 3D space using pinhole model
   // save resulting NxN positions for this feature
 
-  Eigen::Vector3d PositionInCamera(in_p.x/rho, in_p.y/rho, 1/rho);
+  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();
   return PositionInWorld;
   //printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]);
@@ -473,34 +493,56 @@ bool Feature::initializeAnchor(
     return false;
 
   cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second.image;
-  auto u = anchor->second(0)*cam.intrinsics[0] + cam.intrinsics[2];
+  auto u = anchor->second(0);//*cam.intrinsics[0] + cam.intrinsics[2];
-  auto v = anchor->second(1)*cam.intrinsics[1] + cam.intrinsics[3];
+  auto v = anchor->second(1);//*cam.intrinsics[1] + cam.intrinsics[3];
-  printf("initializing anchor\n");
+  
-  if(u - n < 0 || u + n >= cam.resolution(0) || v - n < 0 || v + n >= cam.resolution(1))
+  //testing
-  {
+  undist_anchor_center_pos = cv::Point2f(u,v);
-    printf("no good: \n");
+
-    printf("%f, %f\n", u, v);
-    return false;
-  }
   //for NxN patch pixels around feature
-  for(double u_run = u - n; u_run <= u + n; u_run = u_run + 1)
+  int count = 0;
+
+  // get feature in undistorted pixel space
+  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;
+  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));
+  
+  //create undistorted pure points    
+  std::vector<cv::Point2f> und_v;
+  image_handler::undistortPoints(und_pix_v,
+                                cam.intrinsics,
+                                cam.distortion_model,
+                                0,
+                                und_v);
+//create distorted pixel points
+  std::vector<cv::Point2f> vec = image_handler::distortPoints(und_v,
+                                                             cam.intrinsics,
+                                                             cam.distortion_model,
+                                                             cam.distortion_coeffs);
+
+  anchor_center_pos = vec[4];
+
+  for(auto point : vec)
   {
-    for(double v_run = v - n; v_run <= v + n; v_run = v_run + 1)
+    if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1))
     {
-      printf("ADDING\n");
+      printf("no good\n");
-      // add irradiance information
+      return false;
-      anchorPatch.push_back((double)anchorImage.at<uint8_t>((int)u_run,(int)v_run));
-
-      // project patch pixel to 3D space
-      auto intr = cam.intrinsics; 
-      cv::Point2f currentPoint((u_run-intr[2])/intr[0], (v_run-intr[3])/intr[1]);
-      Eigen::Vector3d Npose = projectPixelToPosition(currentPoint, cam);
-
-      //save position
-      anchorPatch_3d.push_back(Npose);
     }
   }
-  printf("set to true!!!\n");
+  for(auto point : vec)
+    anchorPatch.push_back((double)anchorImage.at<uint8_t>((int)point.x,(int)point.y));
+
+  // project patch pixel to 3D space
+  for(auto point : und_v)
+    anchorPatch_3d.push_back(projectPixelToPosition(point, cam));
+
   is_anchored = true;
   return true;
 }
@@ -645,7 +687,7 @@ bool Feature::initializePosition(
   }
 
   //save inverse depth distance from camera
-  rho = solution(2);
+  anchor_rho = solution(2);
 
   // Convert the feature position to the world frame.
   position = T_c0_w.linear()*final_position + T_c0_w.translation();

include/msckf_vio/image_handler.h

@@ -30,6 +30,12 @@ std::vector<cv::Point2f> distortPoints(
     const cv::Vec4d& intrinsics,
     const std::string& distortion_model,
     const cv::Vec4d& distortion_coeffs);
+
+cv::Point2f distortPoint(
+    const cv::Point2f& pt_in,
+    const cv::Vec4d& intrinsics,
+    const std::string& distortion_model,
+    const cv::Vec4d& distortion_coeffs);
 }
 }
 #endif

src/image_handler.cpp

@@ -81,5 +81,38 @@ std::vector<cv::Point2f> distortPoints(
   return pts_out;
 }
 
+cv::Point2f distortPoint(
+    const cv::Point2f& pt_in,
+    const cv::Vec4d& intrinsics,
+    const std::string& distortion_model,
+    const cv::Vec4d& distortion_coeffs) {
+
+  std::vector<cv::Point2f> pts_in;
+  pts_in.push_back(pt_in);
+
+  const cv::Matx33d K(intrinsics[0], 0.0, intrinsics[2],
+                      0.0, intrinsics[1], intrinsics[3],
+                      0.0, 0.0, 1.0);
+
+  std::vector<cv::Point2f> pts_out;
+  if (distortion_model == "radtan") {
+    std::vector<cv::Point3f> homogenous_pts;
+    cv::convertPointsToHomogeneous(pts_in, homogenous_pts);
+    cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K,
+                      distortion_coeffs, pts_out);
+  } else if (distortion_model == "equidistant") {
+    cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs);
+  } else {
+    ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
+                  distortion_model.c_str());
+    std::vector<cv::Point3f> homogenous_pts;
+    cv::convertPointsToHomogeneous(pts_in, homogenous_pts);
+    cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K,
+                      distortion_coeffs, pts_out);
+  }
+
+  return pts_out[0];
+}
+
 	} // namespace image_handler
 } // namespace msckf_vio

src/msckf_vio.cpp

@@ -929,15 +929,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
   std::vector<cv::Point2f> out_v;
   out_v.push_back(out_p);
 
-  /*
-  //prints the feature projection in pixel space
-  std::vector<cv::Point2f> my_p = image_handler::distortPoints( out_v,
-    cam0.intrinsics,
-    cam0.distortion_model,
-    cam0.distortion_coeffs);
-  printf("projection: %f, %f\n", my_p[0].x,  my_p[0].y);
-  */
-
   // Compute the Jacobians.
   Matrix<double, 4, 3> dz_dpc0 = Matrix<double, 4, 3>::Zero();
   dz_dpc0(0, 0) = 1 / p_c0(2);
@@ -1025,7 +1016,7 @@ void MsckfVio::PhotometricFeatureJacobian(
   VectorXd r_j = VectorXd::Zero(jacobian_row_size);
   int stack_cntr = 0;
 
-
+  printf("_____FEATURE:_____\n");
   for (const auto& cam_id : valid_cam_state_ids) {
 
     Matrix<double, 4, 6> H_xi = Matrix<double, 4, 6>::Zero();