raphael/msckf_vio
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

made residual change more direct. starts diverging but then sometimes appriximates correctly after some camera movement

Browse Source
This commit is contained in:
Raphael Maenle 2019-06-12 09:26:34 +02:00
parent a8d4580812
commit b3df525060
1 changed files with 4 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/msckf_vio.cpp
View File

@ -1258,8 +1258,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
auto point = feature.anchorPatch_3d[0]; auto point = feature.anchorPatch_3d[0];
Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); 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 // add jacobian
//dh / d{}^Cp_{ij} //dh / d{}^Cp_{ij}
@ -1309,16 +1307,8 @@ void MsckfVio::PhotometricMeasurementJacobian(
//calculate residual //calculate residual
//project pixel point to 'pure' position r_i[0] = z[0] - p_c0(0)/p_c0(2);
cv::Point2f und_p_in_c0; r_i[1] = z[1] - p_c0(1)/p_c0(2);
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;
MatrixXd H_xl = MatrixXd::Zero(2, 21+state_server.cam_states.size()*7); 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) == if (feature.observations.find(cam_id) ==
feature.observations.end()) continue; feature.observations.end()) continue;
if (feature.observations.find(cam_id) ==
feature.observations.begin()) continue;
valid_cam_state_ids.push_back(cam_id); valid_cam_state_ids.push_back(cam_id);
} }