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

src/image_handler.cpp

@@ -14,6 +14,48 @@ namespace msckf_vio {
 namespace image_handler {
 
 
+
+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,
+    const cv::Vec4d &new_intrinsics) {
+
+
+  std::vector<cv::Point2f> pts_in;
+  std::vector<cv::Point2f> pts_out;
+  pts_in.push_back(pt_in);
+  if (pts_in.size() == 0) return;
+
+  const cv::Matx33d K(
+      intrinsics[0], 0.0, intrinsics[2],
+      0.0, intrinsics[1], intrinsics[3],
+      0.0, 0.0, 1.0);
+
+  const cv::Matx33d K_new(
+      new_intrinsics[0], 0.0, new_intrinsics[2],
+      0.0, new_intrinsics[1], new_intrinsics[3],
+      0.0, 0.0, 1.0);
+
+  if (distortion_model == "radtan") {
+    cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
+                        rectification_matrix, K_new);
+  } else if (distortion_model == "equidistant") {
+    cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
+                                 rectification_matrix, K_new);
+  } else {
+    ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...",
+                  distortion_model.c_str());
+    cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
+                        rectification_matrix, K_new);
+  }
+  pt_out = pts_out[0];
+  return;
+}
+
 void undistortPoints(
     const std::vector<cv::Point2f>& pts_in,
     const cv::Vec4d& intrinsics,

src/msckf_vio.cpp

@@ -1253,7 +1253,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
   Matrix<double, 3, 3> dCpij_dGpC = Matrix<double, 3, 3>::Zero();
 
   // one line of the NxN Jacobians
-  Eigen::Matrix<double, 2, 3> H_f;
+  Eigen::Matrix<double, 2, 1> H_rho;
   Eigen::Matrix<double, 2, 6> H_plj;
   Eigen::Matrix<double, 2, 6> H_pAj;
 
@@ -1288,6 +1288,10 @@ void MsckfVio::PhotometricMeasurementJacobian(
 
   //d{}^Gp_P{ij} / \rho_i
   double rho = feature.anchor_rho;
+  // Isometry T_anchor_w takes a vector in anchor frame to world frame
+  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));
+
+
 
   // alternative derivation towards feature
   Matrix3d dCpc0_dpg = R_w_c0;
@@ -1298,7 +1302,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
   dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
 
   // Intermediate Jakobians
-  H_f   = dh_dCpij * dCpc0_dpg; // 1 x 1
+  H_rho = dh_dGpij * dGpj_drhoj; // 1 x 1
   H_plj = dh_dXplj; // 1 x 6
   H_pAj = dh_dGpij * dGpj_XpAj; // 1 x 6
 
@@ -1313,7 +1317,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
   //calculate residual
 
   cv::Point2f und_p_in_c0;
-  image_handler::undistortPoints(p_in_c0,
+  image_handler::undistortPoint(p_in_c0,
                                 cam0.intrinsics,
                                 cam0.distortion_model,
                                 0,
@@ -1322,8 +1326,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
   r_i[0] = z[0] - und_p_in_c0.x;
   r_i[1] = z[1] - und_p_in_c0.y;
 
-  cout << "r:\n" << r_i << endl;
-
   MatrixXd H_xl = MatrixXd::Zero(2, 21+state_server.cam_states.size()*7);
  
   // set anchor Jakobi
@@ -1342,7 +1344,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
   H_xl.block(0, 21+cam_state_cntr*7, 2, 6) = H_plj;
 
   H_x = H_xl;
-  H_y = H_f;
+  H_y = H_rho;
   r = r_i;
    cout << "h for patch done" << endl;
 
@@ -1406,7 +1408,7 @@ void MsckfVio::PhotometricFeatureJacobian(
 
   MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size,
       21+state_server.cam_states.size()*7);
-  MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 3);
+  MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 1);
   VectorXd r_i = VectorXd::Zero(jacobian_row_size);
   int stack_cntr = 0;