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

tested bug: filter converges longer without distortion effects; reason probably general formulation inconsistencies

Browse Source
This commit is contained in:
Raphael Maenle
2019-05-22 10:57:31 +02:00
parent 5e9149eacc
commit cab56d9494
2 changed files with 14 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
include/msckf_vio/feature.hpp
View File

@@ -742,27 +742,19 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v), cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v),
cam.intrinsics, cam.intrinsics,
cam.distortion_model, cam.distortion_model,
0); cam.distortion_coeffs);
// create vector of patch in pixel plane // create vector of patch in pixel plane
std::vector<cv::Point2f>und_pix_v;
for(double u_run = -n; u_run <= n; u_run++) for(double u_run = -n; u_run <= n; u_run++)
for(double v_run = -n; v_run <= n; v_run++) for(double v_run = -n; v_run <= n; v_run++)
und_pix_v.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run)); anchorPatch_real.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run));
//create undistorted pure points //create undistorted pure points
std::vector<cv::Point2f> und_v; image_handler::undistortPoints(anchorPatch_real,
image_handler::undistortPoints(und_pix_v,
cam.intrinsics, cam.intrinsics,
cam.distortion_model, cam.distortion_model,
0, cam.distortion_coeffs,
und_v); anchorPatch_ideal);
//create distorted pixel points
anchorPatch_real = image_handler::distortPoints(und_v,
cam.intrinsics,
cam.distortion_model,
cam.distortion_coeffs);
// save anchor position for later visualisaztion // save anchor position for later visualisaztion
@@ -770,19 +762,16 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
// save true pixel Patch position // save true pixel Patch position
for(auto point : anchorPatch_real) for(auto point : anchorPatch_real)
{
if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1)) if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1))
return false; return false;
}
for(auto point : anchorPatch_real) for(auto point : anchorPatch_real)
anchorPatch.push_back(PixelIrradiance(point, anchorImage)); anchorPatch.push_back(PixelIrradiance(point, anchorImage));
// project patch pixel to 3D space in camera coordinate system // project patch pixel to 3D space in camera coordinate system
for(auto point : und_v) for(auto point : anchorPatch_ideal)
{
anchorPatch_ideal.push_back(point);
anchorPatch_3d.push_back(projectPixelToPosition(point, cam)); anchorPatch_3d.push_back(projectPixelToPosition(point, cam));
}
is_anchored = true; is_anchored = true;
return true; return true;
} }

14
src/msckf_vio.cpp
View File

@@ -1232,12 +1232,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
const Vector3d& t_c0_w = cam_state.position; const Vector3d& t_c0_w = cam_state.position;
// Cam1 pose.
Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear();
Matrix3d R_w_c1 = CAMState::T_cam0_cam1.linear() * R_w_c0;
Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation();
//photometric observation //photometric observation
std::vector<double> photo_z; std::vector<double> photo_z;
@@ -1316,16 +1310,21 @@ void MsckfVio::PhotometricMeasurementJacobian(
//calculate residual //calculate residual
//project pixel point to 'pure' position
cv::Point2f und_p_in_c0; cv::Point2f und_p_in_c0;
image_handler::undistortPoint(p_in_c0, image_handler::undistortPoint(p_in_c0,
cam0.intrinsics, cam0.intrinsics,
cam0.distortion_model, cam0.distortion_model,
0, cam0.distortion_coeffs,
und_p_in_c0); und_p_in_c0);
r_i[0] = z[0] - und_p_in_c0.x; r_i[0] = z[0] - und_p_in_c0.x;
r_i[1] = z[1] - und_p_in_c0.y; r_i[1] = z[1] - und_p_in_c0.y;
//cout << "comp\n" << p_c0(0)/p_c0(2) << ":" << und_p_in_c0.x << endl;
//cout << p_c0(1)/p_c0(2) << ":" << und_p_in_c0.y << endl;
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);
// set anchor Jakobi // set anchor Jakobi
@@ -1346,7 +1345,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
H_x = H_xl; H_x = H_xl;
H_y = H_rho; H_y = H_rho;
r = r_i; r = r_i;
cout << "h for patch done" << endl;
//TODO make this more fluent as well //TODO make this more fluent as well
if(PRINTIMAGES) if(PRINTIMAGES)