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

33
src/image_handler.cpp
View File

@ -81,5 +81,38 @@ std::vector<cv::Point2f> distortPoints(
return pts_out;
}
cv::Point2f distortPoint(
const cv::Point2f& pt_in,
const cv::Vec4d& intrinsics,
const std::string& distortion_model,
const cv::Vec4d& distortion_coeffs) {
std::vector<cv::Point2f> pts_in;
pts_in.push_back(pt_in);
const cv::Matx33d K(intrinsics[0], 0.0, intrinsics[2],
0.0, intrinsics[1], intrinsics[3],
0.0, 0.0, 1.0);
std::vector<cv::Point2f> pts_out;
if (distortion_model == "radtan") {
std::vector<cv::Point3f> homogenous_pts;
cv::convertPointsToHomogeneous(pts_in, homogenous_pts);
cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K,
distortion_coeffs, pts_out);
} else if (distortion_model == "equidistant") {
cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs);
} else {
ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
distortion_model.c_str());
std::vector<cv::Point3f> homogenous_pts;
cv::convertPointsToHomogeneous(pts_in, homogenous_pts);
cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K,
distortion_coeffs, pts_out);
}
return pts_out[0];
}
} // namespace image_handler
} // namespace msckf_vio

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();