diff --git a/.gitignore b/.gitignore index d9dca20..341b0d7 100644 --- a/.gitignore +++ b/.gitignore @@ -5,6 +5,7 @@ tmp.cpp source_this.bash *.out README_* +*.png .vscode/ others/ diff --git a/include/my_slam/frame.h b/include/my_slam/frame.h index cb98051..8bb1262 100644 --- a/include/my_slam/frame.h +++ b/include/my_slam/frame.h @@ -37,17 +37,9 @@ class Frame vector inlier_matches_; // inliers matches index with respect to all the points vector inliers_pts3d_; // matches with the previous frame - // vector inliers_of_matches_; // inliers index with respect to the matches - // vector inliers_of_all_pts_; // inliers index with respect to all the points - - // -- Other commonly used points - // vector 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() {} diff --git a/src/feature_match.cpp b/src/feature_match.cpp index d0832e7..0e7cd4c 100644 --- a/src/feature_match.cpp +++ b/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 &matches) diff --git a/src/map.cpp b/src/map.cpp index 8d35db5..c2a3877 100644 --- a/src/map.cpp +++ b/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) diff --git a/src/motion_estimation.cpp b/src/motion_estimation.cpp index 9235b4a..a6b85ae 100644 --- a/src/motion_estimation.cpp +++ b/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; diff --git a/src/vo.cpp b/src/vo.cpp index 40768b2..833d3dc 100644 --- a/src/vo.cpp +++ b/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; diff --git a/src/vo_addFrame.cpp b/src/vo_addFrame.cpp index a4e6367..d28991c 100644 --- a/src/vo_addFrame.cpp +++ b/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 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; } diff --git a/src_main/run_vo.cpp b/src_main/run_vo.cpp index d0a9027..bc16aab 100644 --- a/src_main/run_vo.cpp +++ b/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; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 4091683..b7e0a8d 100644 --- a/test/CMakeLists.txt +++ b/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) diff --git a/test/test_epipolor_geometry.cpp b/test/test_epipolor_geometry.cpp index bc12dbc..432cef5 100644 --- a/test/test_epipolor_geometry.cpp +++ b/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