constructed jacobian based on residual: r = z(measurement pix. pos) - h(rho, x_l, x_a) - based on the projection of the feature point into space based on the anchor frame - followed by projection onto H_rho nullspace; works well enough

include/msckf_vio/image_handler.h

@@ -36,6 +36,15 @@ cv::Point2f distortPoint(
     const cv::Vec4d& intrinsics,
     const std::string& distortion_model,
     const cv::Vec4d& distortion_coeffs);
+
+void undistortPoint(
+    const cv::Point2f& pt_in,
+    const cv::Vec4d& intrinsics,
+    const std::string& distortion_model,
+    const cv::Vec4d& distortion_coeffs,
+    cv::Point2f& pt_out,
+    const cv::Matx33d &rectification_matrix = cv::Matx33d::eye(),
+    const cv::Vec4d &new_intrinsics = cv::Vec4d(1,1,0,0));
 }
 }
 #endif