From ce9f6cd6d71513ac2393680aad9ae9c858a8b74a Mon Sep 17 00:00:00 2001 From: gfuse Date: Fri, 1 Oct 2021 22:10:57 +0200 Subject: [PATCH] add comments for tomorrow --- GPU_version/vio/include/vio/frame.h | 2 +- GPU_version/vio/src/math_utils.cpp | 18 +++++++++--------- GPU_version/vio/src/reprojector.cpp | 7 +++++-- 3 files changed, 15 insertions(+), 12 deletions(-) diff --git a/GPU_version/vio/include/vio/frame.h b/GPU_version/vio/include/vio/frame.h index a80c6fd..7b456d0 100644 --- a/GPU_version/vio/include/vio/frame.h +++ b/GPU_version/vio/include/vio/frame.h @@ -93,7 +93,7 @@ namespace vio { return cam_->world2cam( T_f_w_.se3().inverse()*xyz_w); } /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c). - inline Vector2d w2px(const Vector3d& xyz_w) const { return cam_->world2cam( xyz_w ); } + inline Vector2d w2px(const Vector3d& xyz_w) const { return cam_->world2cam( w2f(xyz_w) ); } /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f). inline Vector3d c2f(const Vector2d& px) const { return cam_->cam2world(px[0], px[1]); } diff --git a/GPU_version/vio/src/math_utils.cpp b/GPU_version/vio/src/math_utils.cpp index a849a01..e3ca59d 100644 --- a/GPU_version/vio/src/math_utils.cpp +++ b/GPU_version/vio/src/math_utils.cpp @@ -12,25 +12,25 @@ namespace vk { using namespace Eigen; /* - * R and T are the rotation and translation of the current frames with respect to the refrence frame, T from ref to current + * R and T are the rotation and translation of the current frames with respect to the reference frame, T from ref to current * */ - +///TODO we need to add the restriction of the movement here Vector3d triangulateFeatureNonLin(const Matrix3d& R, const Vector3d& t, const Vector3d& feature1, const Vector3d& feature2 ) { - Vector3d f2 = R * feature2; + Vector3d R_F2 = R * feature2; Vector2d b; b[0] = t.dot(feature1); - b[1] = t.dot(f2);//cross(t,R*f2) + b[1] = t.dot(R_F2);//cross(t,R*f2) Matrix2d A; A(0,0) = feature1.dot(feature1); - A(1,0) = feature1.dot(f2); + A(1,0) = feature1.dot(R_F2); A(0,1) = -A(1,0); - A(1,1) = -f2.dot(f2); + A(1,1) = -R_F2.dot(R_F2); Vector2d lambda = A.inverse() * b; Vector3d xm = lambda[0] * feature1; - Vector3d xn = t + lambda[1] * f2; + Vector3d xn = t + lambda[1] * R_F2; return ( xm + xn )/2; } @@ -73,7 +73,7 @@ computeInliers(const vector& features2, // c2 const Vector3d& t, // T_c1_to_c2 const double reproj_thresh, double error_multiplier2, - vector& xyz_vec, // 3d point: optimal triangulation is to minimally correct the feature rays + vector& xyz_vec, // 3d point: with respect to the reference frame vector& inliers, vector& outliers) { @@ -85,7 +85,7 @@ computeInliers(const vector& features2, // c2 for(size_t j=0; j0.0){ - xyz_vec.push_back(triangulateFeatureNonLin(R, t, features1[j]/*refrence*/, features2[j]/*current*/ )); + xyz_vec.push_back(triangulateFeatureNonLin(R, t, features1[j]/*reference*/, features2[j]/*current*/ )); double e1 = reprojError(features1[j], xyz_vec.back(), error_multiplier2); double e2 = reprojError(features2[j], R.transpose()*(xyz_vec.back()-t), error_multiplier2); if(e1 > reproj_thresh || e2 > reproj_thresh) diff --git a/GPU_version/vio/src/reprojector.cpp b/GPU_version/vio/src/reprojector.cpp index e1e46e6..66ad2fa 100644 --- a/GPU_version/vio/src/reprojector.cpp +++ b/GPU_version/vio/src/reprojector.cpp @@ -139,9 +139,10 @@ namespace vio { Vector2d px((int) (*it_cur)->px.x(), (int) (*it_cur)->px.y()); SE3 T_ref_cur=it_frame.item.first->se3().inverse()*frame->se3(); + // pose with respect to reference frame Vector3d pos=vk::triangulateFeatureNonLin(T_ref_cur.rotation_matrix(),T_ref_cur.translation(), frame->c2f(px),(*it_ref)->f); - if(pos.z()<0.0)continue; + // point in world frame std::shared_ptr new_point=std::make_shared(it_frame.item.first->se3()*pos,(*it_ref)); frame->addFeature(std::make_shared(frame, new_point, @@ -149,6 +150,7 @@ namespace vio { new_point->addFrameRef(frame->fts_.back()); added_keypoints.push_back(match.trainIdx); frame->fts_.back()->point->last_frame_overlap_id_=it_frame.item.first->id_; + ///TODO number of cell is not reflecting the number of point, check K value grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px)); overlap_kfs.back().second++; }else{ @@ -164,6 +166,7 @@ namespace vio { (*it_ref)->point, px,(*it_cur)->level,(*it_cur)->score,(*it_cur)->descriptor)); added_keypoints.push_back(match.trainIdx); + ///TODO number of cell is not reflecting the number of point, check K value grid_.cells.at(k)->push_back(Candidate((*it_ref)->point, px)); overlap_kfs.back().second++; } @@ -216,7 +219,7 @@ namespace vio { } it->pt->n_succeeded_reproj_++; - if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10) + if(it->pt->n_succeeded_reproj_ > 10) it->pt->type_ = Point::TYPE_GOOD; cell.erase(it);