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),
cam.intrinsics,
cam.distortion_model,
0);
cam.distortion_coeffs);
// 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 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
std::vector<cv::Point2f> und_v;
image_handler::undistortPoints(und_pix_v,
image_handler::undistortPoints(anchorPatch_real,
cam.intrinsics,
cam.distortion_model,
0,
und_v);
//create distorted pixel points
anchorPatch_real = image_handler::distortPoints(und_v,
cam.intrinsics,
cam.distortion_model,
cam.distortion_coeffs);
cam.distortion_coeffs,
anchorPatch_ideal);
// save anchor position for later visualisaztion
@ -770,19 +762,16 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
// save true pixel Patch position
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))
return false;
}
for(auto point : anchorPatch_real)
anchorPatch.push_back(PixelIrradiance(point, anchorImage));
// project patch pixel to 3D space in camera coordinate system
for(auto point : und_v)
{
anchorPatch_ideal.push_back(point);
for(auto point : anchorPatch_ideal)
anchorPatch_3d.push_back(projectPixelToPosition(point, cam));
}
is_anchored = true;
return true;
}