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

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();