add comments for tomorrow

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]); }

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<Vector3d>& features2, // c2
                const Vector3d& t,                 // T_c1_to_c2
                const double reproj_thresh,
                double error_multiplier2,
-               vector<Vector3d>& xyz_vec,         // 3d point: optimal triangulation is to minimally correct the feature rays
+               vector<Vector3d>& xyz_vec,         // 3d point: with respect to the reference frame
                vector<int>& inliers,
                vector<int>& outliers)
 {
@@ -85,7 +85,7 @@ computeInliers(const vector<Vector3d>& features2, // c2
   for(size_t j=0; j<features1.size(); ++j)
   {
     if(t.z()>0.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)

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<Point> new_point=std::make_shared<Point>(it_frame.item.first->se3()*pos,(*it_ref));
                         frame->addFeature(std::make_shared<Feature>(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);