Update printing result

.gitignore

@@ -5,6 +5,7 @@ tmp.cpp
 source_this.bash
 *.out
 README_*
+*.png
 
 .vscode/
 others/

include/my_slam/frame.h

@@ -37,17 +37,9 @@ class Frame
   vector<DMatch> inlier_matches_; // inliers matches index with respect to all the points
   vector<Point3f> inliers_pts3d_; // matches with the previous frame
 
-  // vector<int> inliers_of_matches_; // inliers index with respect to the matches
-  // vector<int> inliers_of_all_pts_; // inliers index with respect to all the points
-
-  // -- Other commonly used points
-  // vector<Point2f> pts_matched_;
-
   // pose
   my_geometry::Camera::Ptr camera_;
   Mat T_w_c_; // transform from camera to world
-  // Mat R_curr_to_prev_;
-  // Mat t_curr_to_prev_;
 
 public:
   Frame() {}

src/feature_match.cpp

@@ -169,7 +169,6 @@ void matchFeatures(
         printf("-- Max dist : %f \n", max_dis);
         printf("-- Min dist : %f \n", min_dis);
     }
-    printf("Number of matches: %d\n", (int)matches.size());
 }
 
 void removeDuplicatedMatches(vector<cv::DMatch> &matches)

src/map.cpp

@@ -14,7 +14,7 @@ void Map::insertKeyFrame(Frame::Ptr frame)
     {
         keyframes_[frame->id_] = frame;
     }
-    printf("Insert keyframe!!! frame_id = %ld, total keyframes = %d", frame->id_, (int)keyframes_.size());
+    printf("Insert keyframe!!! frame_id = %ld, total keyframes = %d\n", frame->id_, (int)keyframes_.size());
 }
 
 void Map::insertMapPoint(MapPoint::Ptr map_point)

src/motion_estimation.cpp

@@ -136,7 +136,9 @@ int helperEstimatePossibleRelativePosesByEpipolarGeometry(
     double score_E = checkEssentialScore(essential_matrix, K, pts_img1, pts_img2, inliers_index_e);
     double score_H = checkHomographyScore(homography_matrix, pts_img1, pts_img2, inliers_index_h);
     double ratio=score_H/(score_E+score_H);
-    printf("score_E = %.1f, score_H = %.1f, H/(E+H)=%.1f\n", score_E, score_H, ratio);
+    printf("Evaluate E/H score: E = %.1f, H = %.1f, H/(E+H)=%.1f\n", score_E, score_H, ratio);
+    printf("        Mean score: E = %.1f, H = %.1f\n", 
+        score_E/inliers_index_e.size(), score_H/inliers_index_h.size());
     int best_sol=0;
     if (ratio>0.45){
         best_sol=1;

src/vo.cpp

@@ -100,7 +100,7 @@ bool VisualOdometry::checkLargeMoveForAddKeyFrame(Frame::Ptr curr, Frame::Ptr re
     double moved_dist = calcMatNorm(t);
     double rotated_angle = calcMatNorm(R_vec);
 
-    printf("Movint dist = %.5f; Rotated angle = %.5f\n", moved_dist, rotated_angle);
+    printf("Wrt prev keyframe, relative dist = %.5f, angle = %.5f\n", moved_dist, rotated_angle);
 
     // Satisfy each one will be a good keyframe
     bool res = moved_dist > MIN_DIST_BETWEEN_KEYFRAME || rotated_angle > MIN_ROTATED_ANGLE;

src/vo_addFrame.cpp

@@ -33,6 +33,7 @@ void VisualOdometry::addFrame(Frame::Ptr frame)
     {
         // Match features
         my_geometry::matchFeatures(ref_->descriptors_, curr_->descriptors_, curr_->matches_);
+        printf("Number of matches with the 1st frame: %d\n", (int)curr_->matches_.size());
 
         // Estimae motion and triangulate points
         estimateMotionAnd3DPoints();
@@ -51,7 +52,7 @@ void VisualOdometry::addFrame(Frame::Ptr frame)
         {
             curr_->T_w_c_ = ref_->T_w_c_;
             cout << "Small movement. Not initialize..." << endl;
-        }        
+        }
     }
     else if (vo_state_ == OK)
     {
@@ -65,6 +66,7 @@ void VisualOdometry::addFrame(Frame::Ptr frame)
             // --------------------- Triangulate more points --------------------
             // - Triangulate new points
             my_geometry::matchFeatures(ref_->descriptors_, curr_->descriptors_, curr_->matches_);
+            printf("Number of matches with prev keyframe: %d\n", (int)curr_->matches_.size());
 
             // -- Use Essential matrix to find the inliers
             vector<DMatch> inlier_matches; // matches, that are inliers
@@ -102,6 +104,7 @@ void VisualOdometry::addFrame(Frame::Ptr frame)
         Mat T_prev_to_curr = T_w_to_prev.inv() * T_w_to_curr;
         Mat R, t;
         getRtFromT(T_prev_to_curr, R, t);
+        cout << "\nCamera motion:" << endl;
         cout << "R_prev_to_curr: " << R << endl;
         cout << "t_prev_to_curr: " << t.t() << endl;
     }

src_main/run_vo.cpp

@@ -111,7 +111,7 @@ int main(int argc, char **argv)
             waitPclKeyPress(pcl_displayer);
 
         // Return
-        cout << "Finished an image" << endl;
+        // cout << "Finished an image" << endl;
         cam_pose_history.push_back(vo->curr_->T_w_c_.clone());
         // if (img_id == 100 || vo->vo_state_ == VisualOdometry::OK)
         //     break;

test/CMakeLists.txt

@@ -1,6 +1,6 @@
 
-# add_executable( test_epipolor_geometry test_epipolor_geometry.cpp )
-# target_link_libraries( test_epipolor_geometry my_geometry)
+add_executable( test_epipolor_geometry test_epipolor_geometry.cpp )
+target_link_libraries( test_epipolor_geometry my_geometry)
 
 # add_executable( test_PnP test_PnP.cpp )
 # target_link_libraries( test_PnP my_geometry)

test/test_epipolor_geometry.cpp

@@ -39,7 +39,8 @@ int main(int argc, char **argv)
         // bin/test_epipolor_geometry rgb_00000.png rgb_00001.png # inliers = 90+
         // bin/test_epipolor_geometry rgb_00003.png rgb_00004.png # inliers = 35
         // bin/test_epipolor_geometry rgb_00004.png rgb_00005.png # inliers = 90+
-        // bin/test_epipolor_geometry image0001.jpg image0015.jpg # inliers = 90+
+        // bin/test_epipolor_geometry image0001.jpg image0015.jpg # Mean Score: E=11.0, H=9.1
+        // bin/test_epipolor_geometry image0001.jpg image0002.jpg # Mean Score: E=11.3, H=11.0
         IDX_TEST_CASE = -1;
         img_file1 = argv[1];
         img_file2 = argv[2];
@@ -94,6 +95,8 @@ int main(int argc, char **argv)
         computeDescriptors(img_1, keypoints_1, descriptors_1);
         computeDescriptors(img_2, keypoints_2, descriptors_2);
         matchFeatures(descriptors_1, descriptors_2, matches, PRINT_RES);
+        printf("Number of matches: %d\n", (int)matches.size());
+
     }
 
     // Estimation motion