From b3df5250600388d33cea2180b57b7d42aa25953c Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Wed, 12 Jun 2019 09:26:34 +0200 Subject: [PATCH] made residual change more direct. starts diverging but then sometimes appriximates correctly after some camera movement --- src/msckf_vio.cpp | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 57f1fb7..acafb60 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1258,8 +1258,6 @@ void MsckfVio::PhotometricMeasurementJacobian( auto point = feature.anchorPatch_3d[0]; Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); - cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); - // add jacobian //dh / d{}^Cp_{ij} @@ -1309,16 +1307,8 @@ void MsckfVio::PhotometricMeasurementJacobian( //calculate residual - //project pixel point to 'pure' position - cv::Point2f und_p_in_c0; - image_handler::undistortPoint(p_in_c0, - cam0.intrinsics, - cam0.distortion_model, - cam0.distortion_coeffs, - und_p_in_c0); - - r_i[0] = z[0] - und_p_in_c0.x; - r_i[1] = z[1] - und_p_in_c0.y; + r_i[0] = z[0] - p_c0(0)/p_c0(2); + r_i[1] = z[1] - p_c0(1)/p_c0(2); MatrixXd H_xl = MatrixXd::Zero(2, 21+state_server.cam_states.size()*7); @@ -1376,6 +1366,8 @@ void MsckfVio::PhotometricFeatureJacobian( if (feature.observations.find(cam_id) == feature.observations.end()) continue; + if (feature.observations.find(cam_id) == + feature.observations.begin()) continue; valid_cam_state_ids.push_back(cam_id); }