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

fixed incorrect undistortion/distortion. residual should now be calculated correctly

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-18 16:22:41 +02:00
parent d91ff7ca9d
commit 1fa2518215
4 changed files with 120 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/msckf_vio.cpp
View File

@ -929,15 +929,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
std::vector<cv::Point2f> out_v;
out_v.push_back(out_p);
/*
//prints the feature projection in pixel space
std::vector<cv::Point2f> my_p = image_handler::distortPoints( out_v,
cam0.intrinsics,
cam0.distortion_model,
cam0.distortion_coeffs);
printf("projection: %f, %f\n", my_p[0].x, my_p[0].y);
*/
// Compute the Jacobians.
Matrix<double, 4, 3> dz_dpc0 = Matrix<double, 4, 3>::Zero();
dz_dpc0(0, 0) = 1 / p_c0(2);
@ -1025,7 +1016,7 @@ void MsckfVio::PhotometricFeatureJacobian(
VectorXd r_j = VectorXd::Zero(jacobian_row_size);
int stack_cntr = 0;
printf("_____FEATURE:_____\n");
for (const auto& cam_id : valid_cam_state_ids) {
Matrix<double, 4, 6> H_xi = Matrix<double, 4, 6>::Zero();