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

corrected position calculation for NxN points

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-12 19:04:45 +02:00
parent 8227a8e48d
commit abd343f576
4 changed files with 26 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
include/msckf_vio/feature.hpp
View File

@ -177,6 +177,9 @@ struct Feature {
// Position of NxN Patch in 3D space
std::vector<Eigen::Vector3d> anchorPatch_3d;
// Anchor Isometry
Eigen::Isometry3d T_anchor_w;
// 3d postion of the feature in the world frame.
Eigen::Vector3d position;
@ -335,8 +338,11 @@ bool Feature::projectPixelToPosition(cv::Point2f in_p,
// use undistorted position of point of interest
// project it back into 3D space using pinhole model
// save resulting NxN positions for this feature
anchorPatch_3d.push_back(Eigen::Vector3d(in_p.x/rho, in_p.y/rho, 1/rho));
printf("%f, %f, %f\n",in_p.x/rho, in_p.y/rho, 1/rho);
Eigen::Vector3d PositionInCamera(in_p.x/rho, in_p.y/rho, 1/rho);
Eigen::Vector3d PositionInWorld= T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation();
anchorPatch_3d.push_back(PositionInWorld);
printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]);
}
bool Feature::initializeAnchor(
@ -412,6 +418,7 @@ bool Feature::initializePosition(
// vector from the first camera frame in the buffer to this
// camera frame.
Eigen::Isometry3d T_c0_w = cam_poses[0];
T_anchor_w = T_c0_w;
for (auto& pose : cam_poses)
pose = pose.inverse() * T_c0_w;