From 5e9149eacc46a690fa5c962374bebf6275cb1a45 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Tue, 21 May 2019 14:50:53 +0200 Subject: [PATCH] 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 | 9 +++++++ src/image_handler.cpp | 42 +++++++++++++++++++++++++++++++ src/msckf_vio.cpp | 16 ++++++------ 3 files changed, 60 insertions(+), 7 deletions(-) diff --git a/include/msckf_vio/image_handler.h b/include/msckf_vio/image_handler.h index 6e8286f..2ba33a7 100644 --- a/include/msckf_vio/image_handler.h +++ b/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 diff --git a/src/image_handler.cpp b/src/image_handler.cpp index 5d868cc..d48793f 100644 --- a/src/image_handler.cpp +++ b/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 pts_in; + std::vector 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& pts_in, const cv::Vec4d& intrinsics, diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index a76c802..ed4ebbc 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1253,7 +1253,7 @@ void MsckfVio::PhotometricMeasurementJacobian( Matrix dCpij_dGpC = Matrix::Zero(); // one line of the NxN Jacobians - Eigen::Matrix H_f; + Eigen::Matrix H_rho; Eigen::Matrix H_plj; Eigen::Matrix 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::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;