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

changed anchor feature position calculation

Browse Source
This commit is contained in:
Raphael Maenle 2019-05-22 11:24:53 +02:00
parent 05d277c4f4
commit 976108bffe
1 changed files with 32 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
include/msckf_vio/feature.hpp
View File

@ -121,6 +121,15 @@ struct Feature {
inline bool checkMotion(
const CamStateServer& cam_states) const;
/*
* @brief AnchorPixelToPosition projects an undistorted point in the
* anchor frame back into the real world using the rho calculated
* based on featur position
*/
inline Eigen::Vector3d AnchorPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam);
/*
* @brief InitializeAnchor generates the NxN patch around the
* feature in the Anchor image
@ -745,6 +754,20 @@ Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, const CameraCa
//printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]);
}
Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam)
{
// use undistorted position of point of interest
// project it back into 3D space using pinhole model
// save resulting NxN positions for this feature
Eigen::Vector3d PositionInCamera(in_p.x/anchor_rho, in_p.y/anchor_rho, 1/anchor_rho);
Eigen::Vector3d PositionInWorld = T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation();
return PositionInWorld;
//printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]);
}
//@test center projection must always be initial feature projection
bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
{
@ -772,27 +795,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
@ -800,19 +815,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);
anchorPatch_3d.push_back(projectPixelToPosition(point, cam));
}
for(auto point : anchorPatch_ideal)
anchorPatch_3d.push_back(AnchorPixelToPosition(point, cam));
is_anchored = true;
return true;
}