From 3bbf39b88945f1f2a8052384363eae058f4baad9 Mon Sep 17 00:00:00 2001 From: galh97 Date: Sun, 21 May 2023 14:32:26 +0300 Subject: [PATCH 1/4] fixed inliers threshold to function, removed redundant code. --- CMakeLists.txt | 70 +++++++-- Examples/Monocular/mono_euroc.cc | 2 +- Examples/euroc_eval_examples.sh | 19 +++ Examples/euroc_examples.sh | 182 ++++++++++++++++++++++ Examples_old/ROS/ORB_SLAM3/CMakeLists.txt | 10 +- build.sh | 8 +- euroc_examples.sh | 182 ++++++++++++++++++++++ include/Sim3Solver.h | 4 +- src/KeyFrame.cc | 6 + src/LoopClosing.cc | 63 +++++--- src/Sim3Solver.cc | 11 +- 11 files changed, 513 insertions(+), 44 deletions(-) create mode 100755 Examples/euroc_eval_examples.sh create mode 100755 Examples/euroc_examples.sh create mode 100755 euroc_examples.sh diff --git a/CMakeLists.txt b/CMakeLists.txt index 016e74354d..3cd1a80d41 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,31 +12,31 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3") set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -march=native") set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native") -# Check C++11 or C++0x support +# Check C++14 or C++0x support include(CheckCXXCompilerFlag) -CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) +CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX11) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) if(COMPILER_SUPPORTS_CXX11) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") add_definitions(-DCOMPILEDWITHC11) - message(STATUS "Using flag -std=c++11.") + message(STATUS "Using flag -std=c++14.") elseif(COMPILER_SUPPORTS_CXX0X) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") add_definitions(-DCOMPILEDWITHC0X) message(STATUS "Using flag -std=c++0x.") else() - message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") + message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.") endif() LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) -find_package(OpenCV 4.4) - if(NOT OpenCV_FOUND) - message(FATAL_ERROR "OpenCV > 4.4 not found.") - endif() +#find_package(OpenCV 4.4) +# if(NOT OpenCV_FOUND) +# message(FATAL_ERROR "OpenCV > 4.4 not found.") +# endif() -MESSAGE("OPENCV VERSION:") -MESSAGE(${OpenCV_VERSION}) +#MESSAGE("OPENCV VERSION:") +#MESSAGE(${OpenCV_VERSION}) find_package(Eigen3 3.1.0 REQUIRED) find_package(Pangolin REQUIRED) @@ -121,6 +121,54 @@ ${EIGEN3_LIBS} ${Pangolin_LIBRARIES} ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so +#/usr/local/lib/aarch64-linux-gnu/libopencv_core.so +/usr/local/lib/aarch64-linux-gnu/libopencv_aruco.so +/usr/local/lib/aarch64-linux-gnu/libopencv_bgsegm.so +/usr/local/lib/aarch64-linux-gnu/libopencv_bioinspired.so +/usr/local/lib/aarch64-linux-gnu/libopencv_calib3d.so +/usr/local/lib/aarch64-linux-gnu/libopencv_ccalib.so +/usr/local/lib/aarch64-linux-gnu/libopencv_core.so +/usr/local/lib/aarch64-linux-gnu/libopencv_datasets.so +/usr/local/lib/aarch64-linux-gnu/libopencv_dnn_objdetect.so +/usr/local/lib/aarch64-linux-gnu/libopencv_dnn.so +/usr/local/lib/aarch64-linux-gnu/libopencv_dpm.so +/usr/local/lib/aarch64-linux-gnu/libopencv_face.so +/usr/local/lib/aarch64-linux-gnu/libopencv_features2d.so +/usr/local/lib/aarch64-linux-gnu/libopencv_flann.so +/usr/local/lib/aarch64-linux-gnu/libopencv_freetype.so +/usr/local/lib/aarch64-linux-gnu/libopencv_fuzzy.so +/usr/local/lib/aarch64-linux-gnu/libopencv_hdf.so +/usr/local/lib/aarch64-linux-gnu/libopencv_hfs.so +/usr/local/lib/aarch64-linux-gnu/libopencv_highgui.so +/usr/local/lib/aarch64-linux-gnu/libopencv_imgcodecs.so +/usr/local/lib/aarch64-linux-gnu/libopencv_img_hash.so +/usr/local/lib/aarch64-linux-gnu/libopencv_imgproc.so +/usr/local/lib/aarch64-linux-gnu/libopencv_line_descriptor.so +/usr/local/lib/aarch64-linux-gnu/libopencv_ml.so +/usr/local/lib/aarch64-linux-gnu/libopencv_objdetect.so +/usr/local/lib/aarch64-linux-gnu/libopencv_optflow.so +/usr/local/lib/aarch64-linux-gnu/libopencv_phase_unwrapping.so +/usr/local/lib/aarch64-linux-gnu/libopencv_photo.so +/usr/local/lib/aarch64-linux-gnu/libopencv_plot.so +/usr/local/lib/aarch64-linux-gnu/libopencv_reg.so +/usr/local/lib/aarch64-linux-gnu/libopencv_rgbd.so +/usr/local/lib/aarch64-linux-gnu/libopencv_saliency.so +/usr/local/lib/aarch64-linux-gnu/libopencv_sfm.so +/usr/local/lib/aarch64-linux-gnu/libopencv_shape.so +/usr/local/lib/aarch64-linux-gnu/libopencv_stereo.so +/usr/local/lib/aarch64-linux-gnu/libopencv_stitching.so +/usr/local/lib/aarch64-linux-gnu/libopencv_structured_light.so +/usr/local/lib/aarch64-linux-gnu/libopencv_superres.so +/usr/local/lib/aarch64-linux-gnu/libopencv_surface_matching.so +/usr/local/lib/aarch64-linux-gnu/libopencv_text.so +/usr/local/lib/aarch64-linux-gnu/libopencv_tracking.so +/usr/local/lib/aarch64-linux-gnu/libopencv_videoio.so +/usr/local/lib/aarch64-linux-gnu/libopencv_video.so +/usr/local/lib/aarch64-linux-gnu/libopencv_videostab.so +/usr/local/lib/aarch64-linux-gnu/libopencv_xfeatures2d.so +/usr/local/lib/aarch64-linux-gnu/libopencv_ximgproc.so +/usr/local/lib/aarch64-linux-gnu/libopencv_xobjdetect.so +/usr/local/lib/aarch64-linux-gnu/libopencv_xphoto.so -lboost_serialization -lcrypto ) diff --git a/Examples/Monocular/mono_euroc.cc b/Examples/Monocular/mono_euroc.cc index 3a233129be..1ea8f95784 100644 --- a/Examples/Monocular/mono_euroc.cc +++ b/Examples/Monocular/mono_euroc.cc @@ -80,7 +80,7 @@ int main(int argc, char **argv) int fps = 20; float dT = 1.f/fps; // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::MONOCULAR, false); + ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::MONOCULAR, true); float imageScale = SLAM.GetImageScale(); double t_resize = 0.f; diff --git a/Examples/euroc_eval_examples.sh b/Examples/euroc_eval_examples.sh new file mode 100755 index 0000000000..908b2f2833 --- /dev/null +++ b/Examples/euroc_eval_examples.sh @@ -0,0 +1,19 @@ +#!/bin/bash +pathDatasetEuroc='~/Downloads/euroc_test' + +# Single Session Example (Visual-Inertial) +echo "Launching V102 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Monocular-Inertial/EuRoC_TimeStamps/V102.txt dataset-V102_monoi +echo "------------------------------------" +echo "Evaluation of V102 trajectory with Monocular-Inertial sensor" +python ../evaluation/evaluate_ate_scale.py "$pathDatasetEuroc"/V102/mav0/state_groundtruth_estimate0/data.csv f_dataset-V102_monoi.txt --plot V102_monoi.pdf + + +# MultiSession Monocular Examples + +echo "Launching Vicon Room 2 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Monocular-Inertial/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Monocular-Inertial/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Monocular-Inertial/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_monoi +echo "------------------------------------" +echo "Evaluation of Vicon Room 2 trajectory with Stereo sensor" +python ../evaluation/evaluate_ate_scale.py ../evaluation/Ground_truth/EuRoC_imu/V2_GT.txt f_dataset-V201_to_V203_monoi.txt --plot V201_to_V203_monoi.pdf + diff --git a/Examples/euroc_examples.sh b/Examples/euroc_examples.sh new file mode 100755 index 0000000000..8dc6d63ff5 --- /dev/null +++ b/Examples/euroc_examples.sh @@ -0,0 +1,182 @@ +#!/bin/bash +pathDatasetEuroc='~/Downloads' + +#------------------------------------ +# Monocular Examples +echo "Launching MH01 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Monocular/EuRoC_TimeStamps/MH01.txt dataset-MH01_mono + +echo "Launching MH02 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH02 ./Monocular/EuRoC_TimeStamps/MH02.txt dataset-MH02_mono + +echo "Launching MH03 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH03 ./Monocular/EuRoC_TimeStamps/MH03.txt dataset-MH03_mono + +echo "Launching MH04 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH04 ./Monocular/EuRoC_TimeStamps/MH04.txt dataset-MH04_mono + +echo "Launching MH05 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH05 ./Monocular/EuRoC_TimeStamps/MH05.txt dataset-MH05_mono + +echo "Launching V101 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Monocular/EuRoC_TimeStamps/V101.txt dataset-V101_mono + +echo "Launching V102 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Monocular/EuRoC_TimeStamps/V102.txt dataset-V102_mono + +echo "Launching V103 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V103 ./Monocular/EuRoC_TimeStamps/V103.txt dataset-V103_mono + +echo "Launching V201 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Monocular/EuRoC_TimeStamps/V201.txt dataset-V201_mono + +echo "Launching V202 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V202 ./Monocular/EuRoC_TimeStamps/V202.txt dataset-V202_mono + +echo "Launching V203 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V203 ./Monocular/EuRoC_TimeStamps/V203.txt dataset-V203_mono + +# MultiSession Monocular Examples +echo "Launching Machine Hall with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Monocular/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Monocular/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Monocular/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Monocular/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Monocular/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_mono + +echo "Launching Vicon Room 1 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Monocular/EuRoC_TimeStamps/V101.txt "$pathDatasetEuroc"/V102 ./Monocular/EuRoC_TimeStamps/V102.txt "$pathDatasetEuroc"/V103 ./Monocular/EuRoC_TimeStamps/V103.txt dataset-V101_to_V103_mono + +echo "Launching Vicon Room 2 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Monocular/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Monocular/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Monocular/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_mono + +#------------------------------------ +# Stereo Examples +echo "Launching MH01 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Stereo/EuRoC_TimeStamps/MH01.txt dataset-MH01_stereo + +echo "Launching MH02 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH02 ./Stereo/EuRoC_TimeStamps/MH02.txt dataset-MH02_stereo + +echo "Launching MH03 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH03 ./Stereo/EuRoC_TimeStamps/MH03.txt dataset-MH03_stereo + +echo "Launching MH04 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH04 ./Stereo/EuRoC_TimeStamps/MH04.txt dataset-MH04_stereo + +echo "Launching MH05 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH05 ./Stereo/EuRoC_TimeStamps/MH05.txt dataset-MH05_stereo + +echo "Launching V101 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Stereo/EuRoC_TimeStamps/V101.txt dataset-V101_stereo + +echo "Launching V102 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Stereo/EuRoC_TimeStamps/V102.txt dataset-V102_stereo + +echo "Launching V103 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V103 ./Stereo/EuRoC_TimeStamps/V103.txt dataset-V103_stereo + +echo "Launching V201 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Stereo/EuRoC_TimeStamps/V201.txt dataset-V201_stereo + +echo "Launching V202 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V202 ./Stereo/EuRoC_TimeStamps/V202.txt dataset-V202_stereo + +echo "Launching V203 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V203 ./Stereo/EuRoC_TimeStamps/V203.txt dataset-V203_stereo + +# MultiSession Stereo Examples +echo "Launching Machine Hall with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Stereo/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Stereo/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Stereo/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Stereo/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Stereo/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_stereo + +echo "Launching Vicon Room 1 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Stereo/EuRoC_TimeStamps/V101.txt "$pathDatasetEuroc"/V102 ./Stereo/EuRoC_TimeStamps/V102.txt "$pathDatasetEuroc"/V103 ./Stereo/EuRoC_TimeStamps/V103.txt dataset-V101_to_V103_stereo + +echo "Launching Vicon Room 2 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Stereo/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Stereo/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Stereo/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_stereo + +#------------------------------------ +# Monocular-Inertial Examples +echo "Launching MH01 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Monocular-Inertial/EuRoC_TimeStamps/MH01.txt dataset-MH01_monoi + +echo "Launching MH02 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH02 ./Monocular-Inertial/EuRoC_TimeStamps/MH02.txt dataset-MH02_monoi + +echo "Launching MH03 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH03 ./Monocular-Inertial/EuRoC_TimeStamps/MH03.txt dataset-MH03_monoi + +echo "Launching MH04 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH04 ./Monocular-Inertial/EuRoC_TimeStamps/MH04.txt dataset-MH04_monoi + +echo "Launching MH05 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH05 ./Monocular-Inertial/EuRoC_TimeStamps/MH05.txt dataset-MH05_monoi + +echo "Launching V101 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Monocular-Inertial/EuRoC_TimeStamps/V101.txt dataset-V101_monoi + +echo "Launching V102 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Monocular-Inertial/EuRoC_TimeStamps/V102.txt dataset-V102_monoi + +echo "Launching V103 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V103 ./Monocular-Inertial/EuRoC_TimeStamps/V103.txt dataset-V103_monoi + +echo "Launching V201 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Monocular-Inertial/EuRoC_TimeStamps/V201.txt dataset-V201_monoi + +echo "Launching V202 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V202 ./Monocular-Inertial/EuRoC_TimeStamps/V202.txt dataset-V202_monoi + +echo "Launching V203 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V203 ./Monocular-Inertial/EuRoC_TimeStamps/V203.txt dataset-V203_monoi + +# MultiSession Monocular Examples +echo "Launching Machine Hall with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Monocular-Inertial/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Monocular-Inertial/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Monocular-Inertial/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Monocular-Inertial/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Monocular-Inertial/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_monoi + +echo "Launching Vicon Room 1 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Monocular-Inertial/EuRoC_TimeStamps/V101.txt "$pathDatasetEuroc"/V102 ./Monocular-Inertial/EuRoC_TimeStamps/V102.txt "$pathDatasetEuroc"/V103 ./Monocular-Inertial/EuRoC_TimeStamps/V103.txt dataset-V101_to_V103_monoi + +echo "Launching Vicon Room 2 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Monocular-Inertial/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Monocular-Inertial/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Monocular-Inertial/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_monoi + +#------------------------------------ +# Stereo-Inertial Examples +echo "Launching MH01 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Stereo-Inertial/EuRoC_TimeStamps/MH01.txt dataset-MH01_stereoi + +echo "Launching MH02 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH02 ./Stereo-Inertial/EuRoC_TimeStamps/MH02.txt dataset-MH02_stereoi + +echo "Launching MH03 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH03 ./Stereo-Inertial/EuRoC_TimeStamps/MH03.txt dataset-MH03_stereoi + +echo "Launching MH04 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH04 ./Stereo-Inertial/EuRoC_TimeStamps/MH04.txt dataset-MH04_stereoi + +echo "Launching MH05 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH05 ./Stereo-Inertial/EuRoC_TimeStamps/MH05.txt dataset-MH05_stereoi + +echo "Launching V101 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Stereo-Inertial/EuRoC_TimeStamps/V101.txt dataset-V101_stereoi + +echo "Launching V102 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Stereo-Inertial/EuRoC_TimeStamps/V102.txt dataset-V102_stereoi + +echo "Launching V103 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V103 ./Stereo-Inertial/EuRoC_TimeStamps/V103.txt dataset-V103_stereoi + +echo "Launching V201 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Stereo-Inertial/EuRoC_TimeStamps/V201.txt dataset-V201_stereoi + +echo "Launching V202 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V202 ./Stereo-Inertial/EuRoC_TimeStamps/V202.txt dataset-V202_stereoi + +echo "Launching V203 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V203 ./Stereo-Inertial/EuRoC_TimeStamps/V203.txt dataset-V203_stereoi + +# MultiSession Stereo-Inertial Examples +echo "Launching Machine Hall with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Stereo-Inertial/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Stereo-Inertial/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Stereo-Inertial/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Stereo-Inertial/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Stereo-Inertial/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_stereoi + +echo "Launching Vicon Room 1 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Stereo-Inertial/EuRoC_TimeStamps/V101.txt "$pathDatasetEuroc"/V102 ./Stereo-Inertial/EuRoC_TimeStamps/V102.txt "$pathDatasetEuroc"/V103 ./Stereo-Inertial/EuRoC_TimeStamps/V103.txt dataset-V101_to_V103_stereoi + +echo "Launching Vicon Room 2 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Stereo-Inertial/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Stereo-Inertial/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Stereo-Inertial/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_stereoi diff --git a/Examples_old/ROS/ORB_SLAM3/CMakeLists.txt b/Examples_old/ROS/ORB_SLAM3/CMakeLists.txt index 12792fe44b..12e4ae8964 100644 --- a/Examples_old/ROS/ORB_SLAM3/CMakeLists.txt +++ b/Examples_old/ROS/ORB_SLAM3/CMakeLists.txt @@ -12,20 +12,20 @@ MESSAGE("Build type: " ${ROS_BUILD_TYPE}) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") -# Check C++11 or C++0x support +# Check C++14 or C++0x support include(CheckCXXCompilerFlag) -CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) +CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX11) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) if(COMPILER_SUPPORTS_CXX11) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") add_definitions(-DCOMPILEDWITHC11) - message(STATUS "Using flag -std=c++11.") + message(STATUS "Using flag -std=c++14.") elseif(COMPILER_SUPPORTS_CXX0X) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") add_definitions(-DCOMPILEDWITHC0X) message(STATUS "Using flag -std=c++0x.") else() - message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") + message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.") endif() LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../../../cmake_modules) diff --git a/build.sh b/build.sh index 96d1c0941c..12e735f22f 100755 --- a/build.sh +++ b/build.sh @@ -4,7 +4,7 @@ cd Thirdparty/DBoW2 mkdir build cd build cmake .. -DCMAKE_BUILD_TYPE=Release -make -j +make cd ../../g2o @@ -13,7 +13,7 @@ echo "Configuring and building Thirdparty/g2o ..." mkdir build cd build cmake .. -DCMAKE_BUILD_TYPE=Release -make -j +make cd ../../Sophus @@ -22,7 +22,7 @@ echo "Configuring and building Thirdparty/Sophus ..." mkdir build cd build cmake .. -DCMAKE_BUILD_TYPE=Release -make -j +make cd ../../../ @@ -37,4 +37,4 @@ echo "Configuring and building ORB_SLAM3 ..." mkdir build cd build cmake .. -DCMAKE_BUILD_TYPE=Release -make -j4 +make diff --git a/euroc_examples.sh b/euroc_examples.sh new file mode 100755 index 0000000000..8dc6d63ff5 --- /dev/null +++ b/euroc_examples.sh @@ -0,0 +1,182 @@ +#!/bin/bash +pathDatasetEuroc='~/Downloads' + +#------------------------------------ +# Monocular Examples +echo "Launching MH01 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Monocular/EuRoC_TimeStamps/MH01.txt dataset-MH01_mono + +echo "Launching MH02 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH02 ./Monocular/EuRoC_TimeStamps/MH02.txt dataset-MH02_mono + +echo "Launching MH03 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH03 ./Monocular/EuRoC_TimeStamps/MH03.txt dataset-MH03_mono + +echo "Launching MH04 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH04 ./Monocular/EuRoC_TimeStamps/MH04.txt dataset-MH04_mono + +echo "Launching MH05 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH05 ./Monocular/EuRoC_TimeStamps/MH05.txt dataset-MH05_mono + +echo "Launching V101 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Monocular/EuRoC_TimeStamps/V101.txt dataset-V101_mono + +echo "Launching V102 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Monocular/EuRoC_TimeStamps/V102.txt dataset-V102_mono + +echo "Launching V103 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V103 ./Monocular/EuRoC_TimeStamps/V103.txt dataset-V103_mono + +echo "Launching V201 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Monocular/EuRoC_TimeStamps/V201.txt dataset-V201_mono + +echo "Launching V202 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V202 ./Monocular/EuRoC_TimeStamps/V202.txt dataset-V202_mono + +echo "Launching V203 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V203 ./Monocular/EuRoC_TimeStamps/V203.txt dataset-V203_mono + +# MultiSession Monocular Examples +echo "Launching Machine Hall with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Monocular/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Monocular/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Monocular/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Monocular/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Monocular/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_mono + +echo "Launching Vicon Room 1 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Monocular/EuRoC_TimeStamps/V101.txt "$pathDatasetEuroc"/V102 ./Monocular/EuRoC_TimeStamps/V102.txt "$pathDatasetEuroc"/V103 ./Monocular/EuRoC_TimeStamps/V103.txt dataset-V101_to_V103_mono + +echo "Launching Vicon Room 2 with Monocular sensor" +./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Monocular/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Monocular/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Monocular/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_mono + +#------------------------------------ +# Stereo Examples +echo "Launching MH01 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Stereo/EuRoC_TimeStamps/MH01.txt dataset-MH01_stereo + +echo "Launching MH02 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH02 ./Stereo/EuRoC_TimeStamps/MH02.txt dataset-MH02_stereo + +echo "Launching MH03 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH03 ./Stereo/EuRoC_TimeStamps/MH03.txt dataset-MH03_stereo + +echo "Launching MH04 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH04 ./Stereo/EuRoC_TimeStamps/MH04.txt dataset-MH04_stereo + +echo "Launching MH05 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH05 ./Stereo/EuRoC_TimeStamps/MH05.txt dataset-MH05_stereo + +echo "Launching V101 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Stereo/EuRoC_TimeStamps/V101.txt dataset-V101_stereo + +echo "Launching V102 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Stereo/EuRoC_TimeStamps/V102.txt dataset-V102_stereo + +echo "Launching V103 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V103 ./Stereo/EuRoC_TimeStamps/V103.txt dataset-V103_stereo + +echo "Launching V201 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Stereo/EuRoC_TimeStamps/V201.txt dataset-V201_stereo + +echo "Launching V202 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V202 ./Stereo/EuRoC_TimeStamps/V202.txt dataset-V202_stereo + +echo "Launching V203 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V203 ./Stereo/EuRoC_TimeStamps/V203.txt dataset-V203_stereo + +# MultiSession Stereo Examples +echo "Launching Machine Hall with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Stereo/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Stereo/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Stereo/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Stereo/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Stereo/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_stereo + +echo "Launching Vicon Room 1 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Stereo/EuRoC_TimeStamps/V101.txt "$pathDatasetEuroc"/V102 ./Stereo/EuRoC_TimeStamps/V102.txt "$pathDatasetEuroc"/V103 ./Stereo/EuRoC_TimeStamps/V103.txt dataset-V101_to_V103_stereo + +echo "Launching Vicon Room 2 with Stereo sensor" +./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Stereo/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Stereo/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Stereo/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_stereo + +#------------------------------------ +# Monocular-Inertial Examples +echo "Launching MH01 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Monocular-Inertial/EuRoC_TimeStamps/MH01.txt dataset-MH01_monoi + +echo "Launching MH02 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH02 ./Monocular-Inertial/EuRoC_TimeStamps/MH02.txt dataset-MH02_monoi + +echo "Launching MH03 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH03 ./Monocular-Inertial/EuRoC_TimeStamps/MH03.txt dataset-MH03_monoi + +echo "Launching MH04 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH04 ./Monocular-Inertial/EuRoC_TimeStamps/MH04.txt dataset-MH04_monoi + +echo "Launching MH05 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH05 ./Monocular-Inertial/EuRoC_TimeStamps/MH05.txt dataset-MH05_monoi + +echo "Launching V101 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Monocular-Inertial/EuRoC_TimeStamps/V101.txt dataset-V101_monoi + +echo "Launching V102 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Monocular-Inertial/EuRoC_TimeStamps/V102.txt dataset-V102_monoi + +echo "Launching V103 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V103 ./Monocular-Inertial/EuRoC_TimeStamps/V103.txt dataset-V103_monoi + +echo "Launching V201 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Monocular-Inertial/EuRoC_TimeStamps/V201.txt dataset-V201_monoi + +echo "Launching V202 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V202 ./Monocular-Inertial/EuRoC_TimeStamps/V202.txt dataset-V202_monoi + +echo "Launching V203 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V203 ./Monocular-Inertial/EuRoC_TimeStamps/V203.txt dataset-V203_monoi + +# MultiSession Monocular Examples +echo "Launching Machine Hall with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Monocular-Inertial/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Monocular-Inertial/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Monocular-Inertial/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Monocular-Inertial/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Monocular-Inertial/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_monoi + +echo "Launching Vicon Room 1 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Monocular-Inertial/EuRoC_TimeStamps/V101.txt "$pathDatasetEuroc"/V102 ./Monocular-Inertial/EuRoC_TimeStamps/V102.txt "$pathDatasetEuroc"/V103 ./Monocular-Inertial/EuRoC_TimeStamps/V103.txt dataset-V101_to_V103_monoi + +echo "Launching Vicon Room 2 with Monocular-Inertial sensor" +./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Monocular-Inertial/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Monocular-Inertial/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Monocular-Inertial/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_monoi + +#------------------------------------ +# Stereo-Inertial Examples +echo "Launching MH01 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Stereo-Inertial/EuRoC_TimeStamps/MH01.txt dataset-MH01_stereoi + +echo "Launching MH02 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH02 ./Stereo-Inertial/EuRoC_TimeStamps/MH02.txt dataset-MH02_stereoi + +echo "Launching MH03 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH03 ./Stereo-Inertial/EuRoC_TimeStamps/MH03.txt dataset-MH03_stereoi + +echo "Launching MH04 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH04 ./Stereo-Inertial/EuRoC_TimeStamps/MH04.txt dataset-MH04_stereoi + +echo "Launching MH05 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH05 ./Stereo-Inertial/EuRoC_TimeStamps/MH05.txt dataset-MH05_stereoi + +echo "Launching V101 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Stereo-Inertial/EuRoC_TimeStamps/V101.txt dataset-V101_stereoi + +echo "Launching V102 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Stereo-Inertial/EuRoC_TimeStamps/V102.txt dataset-V102_stereoi + +echo "Launching V103 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V103 ./Stereo-Inertial/EuRoC_TimeStamps/V103.txt dataset-V103_stereoi + +echo "Launching V201 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Stereo-Inertial/EuRoC_TimeStamps/V201.txt dataset-V201_stereoi + +echo "Launching V202 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V202 ./Stereo-Inertial/EuRoC_TimeStamps/V202.txt dataset-V202_stereoi + +echo "Launching V203 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V203 ./Stereo-Inertial/EuRoC_TimeStamps/V203.txt dataset-V203_stereoi + +# MultiSession Stereo-Inertial Examples +echo "Launching Machine Hall with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Stereo-Inertial/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Stereo-Inertial/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Stereo-Inertial/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Stereo-Inertial/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Stereo-Inertial/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_stereoi + +echo "Launching Vicon Room 1 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Stereo-Inertial/EuRoC_TimeStamps/V101.txt "$pathDatasetEuroc"/V102 ./Stereo-Inertial/EuRoC_TimeStamps/V102.txt "$pathDatasetEuroc"/V103 ./Stereo-Inertial/EuRoC_TimeStamps/V103.txt dataset-V101_to_V103_stereoi + +echo "Launching Vicon Room 2 with Stereo-Inertial sensor" +./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Stereo-Inertial/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Stereo-Inertial/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Stereo-Inertial/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_stereoi diff --git a/include/Sim3Solver.h b/include/Sim3Solver.h index 5539987823..8f9ead7a5c 100644 --- a/include/Sim3Solver.h +++ b/include/Sim3Solver.h @@ -48,7 +48,7 @@ class Sim3Solver Eigen::Matrix3f GetEstimatedRotation(); Eigen::Vector3f GetEstimatedTranslation(); float GetEstimatedScale(); - + int N; protected: void ComputeCentroid(Eigen::Matrix3f &P, Eigen::Matrix3f &Pr, Eigen::Vector3f &C); @@ -78,7 +78,7 @@ class Sim3Solver std::vector mvnMaxError1; std::vector mvnMaxError2; - int N; + int mN1; // Current Estimation diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc index 293ab48182..ef91129752 100644 --- a/src/KeyFrame.cc +++ b/src/KeyFrame.cc @@ -376,6 +376,12 @@ MapPoint* KeyFrame::GetMapPoint(const size_t &idx) return mvpMapPoints[idx]; } +//this function loop on its map points, for each, it gets all the +//key frames that connected to it (called weight of each key frame). +//after counting all connected map points of key frames, +//we select the key frames that have more map points +//connected than the required threshold. if there are no kfs, +// choose the one with max weight. void KeyFrame::UpdateConnections(bool upParent) { map KFcounter; diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc index 319afb551b..0af0b42646 100644 --- a/src/LoopClosing.cc +++ b/src/LoopClosing.cc @@ -86,10 +86,19 @@ void LoopClosing::SetLocalMapper(LocalMapping *pLocalMapper) mpLocalMapper=pLocalMapper; } +// When a new frame is captured, its ORB descriptors are matched with the descriptors +// of all the keyframes in the map. If enough matches are found, the new +// frame is considered a loop closure candidate. + +// The next step is to verify whether the +// candidate frame is a true loop closure or a false positive. + +// This is done by comparing the geometric consistency between the candidate +// frame and the keyframes in the map. void LoopClosing::Run() { - mbFinished =false; + mbFinished =false; while(1) { @@ -578,16 +587,20 @@ bool LoopClosing::DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame* bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, KeyFrame* &pMatchedKF2, KeyFrame* &pLastCurrentKF, g2o::Sim3 &g2oScw, int &nNumCoincidences, std::vector &vpMPs, std::vector &vpMatchedMPs) { + std::chrono::steady_clock::time_point time_StartQuery = std::chrono::steady_clock::now(); + int nBoWMatches = 20; - int nBoWInliers = 15; + int nBoWInliers = 10; int nSim3Inliers = 20; int nProjMatches = 50; int nProjOptMatches = 80; + // gets the set of all kfs that have enough shared map points (above threshold=15) set spConnectedKeyFrames = mpCurrentKF->GetConnectedKeyFrames(); int nNumCovisibles = 10; + //dont touch, these are good matcher values ORBmatcher matcherBoW(0.9, true); ORBmatcher matcher(0.75, true); @@ -604,6 +617,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, vector vnMatchesStage(numCandidates, 0); int index = 0; + //Verbose::PrintMess("BoW candidates: There are " + to_string(vpBowCand.size()) + " possible candidates ", Verbose::VERBOSITY_DEBUG); for(KeyFrame* pKFi : vpBowCand) { @@ -612,7 +626,9 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, // std::cout << "KF candidate: " << pKFi->mnId << std::endl; // Current KF against KF with covisibles version + //find all covisible key frame of candidate, add the candidate to the list as well std::vector vpCovKFi = pKFi->GetBestCovisibilityKeyFrames(nNumCovisibles); + if(vpCovKFi.empty()) { std::cout << "Covisible list empty" << std::endl; @@ -621,7 +637,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, else { vpCovKFi.push_back(vpCovKFi[0]); - vpCovKFi[0] = pKFi; + vpCovKFi[0] = pKFi; } @@ -641,41 +657,42 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, } //std::cout << "Check BoW continue because is far to the matched one " << std::endl; - - std::vector > vvpMatchedMPs; + + std::vector > vvpMatchedMPs; //for every cov kf, all matched map points vvpMatchedMPs.resize(vpCovKFi.size()); std::set spMatchedMPi; int numBoWMatches = 0; - KeyFrame* pMostBoWMatchesKF = pKFi; + KeyFrame* pMostBoWMatchesKF = pKFi; int nMostBoWNumMatches = 0; std::vector vpMatchedPoints = std::vector(mpCurrentKF->GetMapPointMatches().size(), static_cast(NULL)); std::vector vpKeyFrameMatchedMP = std::vector(mpCurrentKF->GetMapPointMatches().size(), static_cast(NULL)); int nIndexMostBoWMatchesKF=0; - for(int j=0; jisBad()) continue; - int num = matcherBoW.SearchByBoW(mpCurrentKF, vpCovKFi[j], vvpMatchedMPs[j]); - if (num > nMostBoWNumMatches) + //get matches by bow for our current frame with one of the cov kf of the i'th Bow Candidates + int num = matcherBoW.SearchByBoW(mpCurrentKF, vpCovKFi[j], vvpMatchedMPs[j]); //fill the vvpMatchedMPs[j] with all matched map points of vpCovKFi[j] with mpCurrentKF + if (num > nMostBoWNumMatches) // pick the one with max matches { nMostBoWNumMatches = num; nIndexMostBoWMatchesKF = j; } } - for(int j=0; jisBad()) continue; - if(spMatchedMPi.find(pMPi_j) == spMatchedMPi.end()) + if(spMatchedMPi.find(pMPi_j) == spMatchedMPi.end()) //insert if not present in list { spMatchedMPi.insert(pMPi_j); numBoWMatches++; @@ -688,7 +705,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, //pMostBoWMatchesKF = vpCovKFi[pMostBoWMatchesKF]; - if(numBoWMatches >= nBoWMatches) // TODO pick a good threshold + if(numBoWMatches >= nBoWMatches) // TODO pick a good threshold, now nBoWMatches=20 { // Geometric validation bool bFixedScale = mbFixScale; @@ -696,7 +713,10 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, bFixedScale=false; Sim3Solver solver = Sim3Solver(mpCurrentKF, pMostBoWMatchesKF, vpMatchedPoints, bFixedScale, vpKeyFrameMatchedMP); - solver.SetRansacParameters(0.99, nBoWInliers, 300); // at least 15 inliers + std::cout << "nBoWInliers before " << nBoWInliers << std::endl; + nBoWInliers = std::max(nBoWInliers, solver.N / 4); + std::cout << "nBoWInliers after " << nBoWInliers << std::endl; + solver.SetRansacParameters(0.99, nBoWInliers, 300); // at least 15 inliers- todo - fix 15 to be in relation to the amount of matched points, distance from middle, etc.. bool bNoMore = false; vector vbInliers; @@ -778,7 +798,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, if(numProjOptMatches >= nProjOptMatches) { - int max_x = -1, min_x = 1000000; + /*int max_x = -1, min_x = 1000000; int max_y = -1, min_y = 1000000; for(MapPoint* pMPi : vpMatchedMP) { @@ -789,7 +809,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, tuple indexes = pMPi->GetIndexInKeyFrame(pKFi); int index = get<0>(indexes); - if(index >= 0) + if(index >= 0) //TODO { int coord_x = pKFi->mvKeysUn[index].pt.x; if(coord_x < min_x) @@ -810,7 +830,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, max_y = coord_y; } } - } + }*/ int nNumKFs = 0; //vpMatchedMPs = vpMatchedMP; @@ -866,6 +886,11 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, } index++; } + + std::chrono::steady_clock::time_point time_EndQuery = std::chrono::steady_clock::now(); + + double timeDataQuery = std::chrono::duration_cast >(time_EndQuery - time_StartQuery).count(); + std::cout << "time: " << timeDataQuery << std::endl; if(nBestMatchesReproj > 0) { @@ -879,7 +904,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, return nNumCoincidences >= 3; } - else + /* else { int maxStage = -1; int maxMatched; @@ -891,7 +916,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, maxMatched = vnMatchesStage[i]; } } - } + } */ return false; } diff --git a/src/Sim3Solver.cc b/src/Sim3Solver.cc index b8d3abfb14..02def677f6 100644 --- a/src/Sim3Solver.cc +++ b/src/Sim3Solver.cc @@ -222,6 +222,7 @@ Eigen::Matrix4f Sim3Solver::iterate(int nIterations, bool &bNoMore, vector vbInliers = vector(mN1,false); nInliers=0; + std::cout << "there are " << N << "map points" << std::endl; if(N } ComputeSim3(P3Dc1i,P3Dc2i); - + std::cout << "iteration number: " << nCurrentIterations << std::endl; CheckInliers(); if(mnInliersi>=mnBestInliers) @@ -270,13 +271,16 @@ Eigen::Matrix4f Sim3Solver::iterate(int nIterations, bool &bNoMore, vector mBestRotation = mR12i; mBestTranslation = mt12i; mBestScale = ms12i; + std::cout << "if(mnInliersi>mRansacMinInliers) - mnIterations: " << mnIterations << "mRansacMaxIts: " << mRansacMaxIts << std::endl; if(mnInliersi>mRansacMinInliers) { nInliers = mnInliersi; - for(int i=0; i=mRansacMaxIts) - mnIterations: " << mnIterations << "mRansacMaxIts: " << mRansacMaxIts << std::endl; if(mnIterations>=mRansacMaxIts) bNoMore=true; @@ -436,6 +442,7 @@ void Sim3Solver::CheckInliers() else mvbInliersi[i]=false; } + std::cout << "mnInliersi: " << mnInliersi << std::endl; } Eigen::Matrix4f Sim3Solver::GetEstimatedTransformation() From 2a01615ffce4e2a36e93785fdc1d3ba25a7b52b3 Mon Sep 17 00:00:00 2001 From: galh97 Date: Sun, 28 May 2023 18:23:46 +0300 Subject: [PATCH 2/4] fixed code --- include/LoopClosing.h | 4 +- include/ORBmatcher.h | 4 +- include/Sim3Solver.h | 4 +- src/LoopClosing.cc | 85 +++++++++++++++++++------------------------ src/ORBmatcher.cc | 43 ++++++++++------------ src/Sim3Solver.cc | 26 +++++++++---- 6 files changed, 83 insertions(+), 83 deletions(-) diff --git a/include/LoopClosing.h b/include/LoopClosing.h index c9ccb269ae..6e53d88335 100644 --- a/include/LoopClosing.h +++ b/include/LoopClosing.h @@ -143,7 +143,9 @@ class LoopClosing void MergeLocal2(); void CheckObservations(set &spKFsMap1, set &spKFsMap2); - + + static bool compareByValue(const std::pair& a, const std::pair& b); + void ResetIfRequested(); bool mbResetRequested; bool mbResetActiveMapRequested; diff --git a/include/ORBmatcher.h b/include/ORBmatcher.h index 92bedd7f9a..1ad46cb034 100644 --- a/include/ORBmatcher.h +++ b/include/ORBmatcher.h @@ -66,7 +66,7 @@ namespace ORB_SLAM3 // Brute force constrained to ORB that belong to the same vocabulary node (at a certain level) // Used in Relocalisation and Loop Detection int SearchByBoW(KeyFrame *pKF, Frame &F, std::vector &vpMapPointMatches); - int SearchByBoW(KeyFrame *pKF1, KeyFrame* pKF2, std::vector &vpMatches12); + int SearchByBoW(KeyFrame *pKF1, KeyFrame* pKF2, std::vector &vpMatches12, std::map mapPointToDistance, std::map mapPointToKeyFrame); // Matching for the Map Initialization (only used in the monocular case) int SearchForInitialization(Frame &F1, Frame &F2, std::vector &vbPrevMatched, std::vector &vnMatches12, int windowSize=10); @@ -104,4 +104,4 @@ namespace ORB_SLAM3 }// namespace ORB_SLAM -#endif // ORBMATCHER_H \ No newline at end of file +#endif // ORBMATCHER_H diff --git a/include/Sim3Solver.h b/include/Sim3Solver.h index 8f9ead7a5c..959fbfd98c 100644 --- a/include/Sim3Solver.h +++ b/include/Sim3Solver.h @@ -35,14 +35,14 @@ class Sim3Solver public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW Sim3Solver(KeyFrame* pKF1, KeyFrame* pKF2, const std::vector &vpMatched12, const bool bFixScale = true, - const vector vpKeyFrameMatchedMP = vector()); + std::map mapPointToKeyFrame = std::map()); void SetRansacParameters(double probability = 0.99, int minInliers = 6 , int maxIterations = 300); Eigen::Matrix4f find(std::vector &vbInliers12, int &nInliers); Eigen::Matrix4f iterate(int nIterations, bool &bNoMore, std::vector &vbInliers, int &nInliers); - Eigen::Matrix4f iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers, bool &bConverge); + Eigen::Matrix4f iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers, bool &bConverge, std::vector> pairsPointToDistance); Eigen::Matrix4f GetEstimatedTransformation(); Eigen::Matrix3f GetEstimatedRotation(); diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc index 0af0b42646..2355c124fc 100644 --- a/src/LoopClosing.cc +++ b/src/LoopClosing.cc @@ -670,38 +670,63 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, std::vector vpKeyFrameMatchedMP = std::vector(mpCurrentKF->GetMapPointMatches().size(), static_cast(NULL)); int nIndexMostBoWMatchesKF=0; + std::map mapPointToDistance; + std::map mapPointToKeyFrame; + for(int j=0; jisBad()) continue; //get matches by bow for our current frame with one of the cov kf of the i'th Bow Candidates - int num = matcherBoW.SearchByBoW(mpCurrentKF, vpCovKFi[j], vvpMatchedMPs[j]); //fill the vvpMatchedMPs[j] with all matched map points of vpCovKFi[j] with mpCurrentKF + int num = matcherBoW.SearchByBoW(mpCurrentKF, vpCovKFi[j], vvpMatchedMPs[j], mapPointToDistance, mapPointToKeyFrame); //fill the vvpMatchedMPs[j] with all matched map points of vpCovKFi[j] with mpCurrentKF if (num > nMostBoWNumMatches) // pick the one with max matches { nMostBoWNumMatches = num; nIndexMostBoWMatchesKF = j; } } + + // Create a vector of pairs from the map + std::vector> pairsPointToDistance(mapPointToDistance.begin(), mapPointToDistance.end()); + + // Sort the vector based on the values using the custom comparator function + std::sort(pairsPointToDistance.begin(), pairsPointToDistance.end(), LoopClosing::compareByValue); - for(int j=0; j sortedMapPointsByScore; + + for (const auto& pair : pairsPointToDistance) { + MapPoint* mapPoint = pair.first; + if(!mapPoint || mapPoint->isBad()){ + continue; + } + numBoWMatches++; + sortedMapPointsByScore.push_back(mapPoint); + } + + // todo maybe remove this for loop, we dont need vpMatchedPoints anymore + //because we created a sorted version - sortedMapPointsByScore. + // we will need to add numBoWMatches++; to the upper loop + //we will need to fix vpKeyFrameMatchedMP or remove it + /*for(int j=0; jisBad()) continue; - - if(spMatchedMPi.find(pMPi_j) == spMatchedMPi.end()) //insert if not present in list + + if(spMatchedMPi.insert(pMPi_j).second) //insert if not present in list { - spMatchedMPi.insert(pMPi_j); numBoWMatches++; - + std::cout << "k " << k << std::endl; vpMatchedPoints[k]= pMPi_j; vpKeyFrameMatchedMP[k] = vpCovKFi[j]; } } - } + }*/ + + std::cout<< "vpKeyFrameMatchedMP.size() " << vpKeyFrameMatchedMP.size() << std::endl; //pMostBoWMatchesKF = vpCovKFi[pMostBoWMatchesKF]; @@ -712,7 +737,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, if(mpTracker->mSensor==System::IMU_MONOCULAR && !mpCurrentKF->GetMap()->GetIniertialBA2()) bFixedScale=false; - Sim3Solver solver = Sim3Solver(mpCurrentKF, pMostBoWMatchesKF, vpMatchedPoints, bFixedScale, vpKeyFrameMatchedMP); + Sim3Solver solver = Sim3Solver(mpCurrentKF, pMostBoWMatchesKF, sortedMapPointsByScore, bFixedScale, mapPointToKeyFrame); std::cout << "nBoWInliers before " << nBoWInliers << std::endl; nBoWInliers = std::max(nBoWInliers, solver.N / 4); std::cout << "nBoWInliers after " << nBoWInliers << std::endl; @@ -725,7 +750,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, Eigen::Matrix4f mTcm; while(!bConverge && !bNoMore) { - mTcm = solver.iterate(20,bNoMore, vbInliers, nInliers, bConverge); + mTcm = solver.iterate(20,bNoMore, vbInliers, nInliers, bConverge, pairsPointToDistance); //Verbose::PrintMess("BoW guess: Solver achieve " + to_string(nInliers) + " geometrical inliers among " + to_string(nBoWInliers) + " BoW matches", Verbose::VERBOSITY_DEBUG); } @@ -798,40 +823,6 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, if(numProjOptMatches >= nProjOptMatches) { - /*int max_x = -1, min_x = 1000000; - int max_y = -1, min_y = 1000000; - for(MapPoint* pMPi : vpMatchedMP) - { - if(!pMPi || pMPi->isBad()) - { - continue; - } - - tuple indexes = pMPi->GetIndexInKeyFrame(pKFi); - int index = get<0>(indexes); - if(index >= 0) //TODO - { - int coord_x = pKFi->mvKeysUn[index].pt.x; - if(coord_x < min_x) - { - min_x = coord_x; - } - if(coord_x > max_x) - { - max_x = coord_x; - } - int coord_y = pKFi->mvKeysUn[index].pt.y; - if(coord_y < min_y) - { - min_y = coord_y; - } - if(coord_y > max_y) - { - max_y = coord_y; - } - } - }*/ - int nNumKFs = 0; //vpMatchedMPs = vpMatchedMP; //vpMPs = vpMapPoints; @@ -879,10 +870,6 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, } } } - /*else - { - Verbose::PrintMess("BoW candidate: it don't match with the current one", Verbose::VERBOSITY_DEBUG); - }*/ } index++; } @@ -920,6 +907,10 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, return false; } +bool LoopClosing::compareByValue(const std::pair& a, const std::pair& b) { + return a.second < b.second; +} + bool LoopClosing::DetectCommonRegionsFromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches, std::vector &vpMPs, std::vector &vpMatchedMPs) { diff --git a/src/ORBmatcher.cc b/src/ORBmatcher.cc index 9129683e4e..81db6dbb1f 100644 --- a/src/ORBmatcher.cc +++ b/src/ORBmatcher.cc @@ -762,7 +762,7 @@ namespace ORB_SLAM3 return nmatches; } - int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector &vpMatches12) + int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector &vpMatches12, std::map mapPointToDistance, std::map mapPointToKeyFrame) { const vector &vKeysUn1 = pKF1->mvKeysUn; const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec; @@ -823,10 +823,7 @@ namespace ORB_SLAM3 MapPoint* pMP2 = vpMapPoints2[idx2]; - if(vbMatched2[idx2] || !pMP2) - continue; - - if(pMP2->isBad()) + if(vbMatched2[idx2] || !pMP2 || pMP2->isBad()) continue; const cv::Mat &d2 = Descriptors2.row(idx2); @@ -845,26 +842,26 @@ namespace ORB_SLAM3 } } - if(bestDist1(bestDist1) < mfNNratio * static_cast(bestDist2))) { - if(static_cast(bestDist1)(bestDist2)) - { - vpMatches12[idx1]=vpMapPoints2[bestIdx2]; - vbMatched2[bestIdx2]=true; + mapPointToDistance[vpMapPoints2[bestIdx2]] = bestDist1; + mapPointToKeyFrame[vpMapPoints2[bestIdx2]] = pKF2; + vpMatches12[idx1]=vpMapPoints2[bestIdx2]; //todo why dont we need to save from vpMapPoints1 too? + vbMatched2[bestIdx2]=true; - if(mbCheckOrientation) - { - float rot = vKeysUn1[idx1].angle-vKeysUn2[bestIdx2].angle; - if(rot<0.0) - rot+=360.0f; - int bin = round(rot*factor); - if(bin==HISTO_LENGTH) - bin=0; - assert(bin>=0 && bin=0 && bin &vpMatched12, const bool bFixScale, - vector vpKeyFrameMatchedMP): + std::map mapPointToKeyFrame): mnIterations(0), mnBestInliers(0), mbFixScale(bFixScale), pCamera1(pKF1->mpCamera), pCamera2(pKF2->mpCamera) { - bool bDifferentKFs = false; - if(vpKeyFrameMatchedMP.empty()) + bool bDifferentKFs = false; //todo + if(mapPointToKeyFrame.empty()) { bDifferentKFs = true; - vpKeyFrameMatchedMP = vector(vpMatched12.size(), pKF2); + //vpKeyFrameMatchedMP = vector(vpMatched12.size(), pKF2); } mpKF1 = pKF1; @@ -81,8 +81,8 @@ Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector if(pMP1->isBad() || pMP2->isBad()) continue; - if(bDifferentKFs) - pKFm = vpKeyFrameMatchedMP[i1]; + if(!bDifferentKFs) + pKFm = mapPointToKeyFrame[pMP2]; int indexKF1 = get<0>(pMP1->GetIndexInKeyFrame(pKF1)); int indexKF2 = get<0>(pMP2->GetIndexInKeyFrame(pKFm)); @@ -215,7 +215,7 @@ Eigen::Matrix4f Sim3Solver::iterate(int nIterations, bool &bNoMore, vector return Eigen::Matrix4f::Identity(); } -Eigen::Matrix4f Sim3Solver::iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers, bool &bConverge) +Eigen::Matrix4f Sim3Solver::iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers, bool &bConverge, std::vector> pairsPointToDistance) { bNoMore = false; bConverge = false; @@ -243,7 +243,7 @@ Eigen::Matrix4f Sim3Solver::iterate(int nIterations, bool &bNoMore, vector nCurrentIterations++; mnIterations++; - vAvailableIndices = mvAllIndices; + /*vAvailableIndices = mvAllIndices; // Get min set of points for(short i = 0; i < 3; ++i) @@ -257,7 +257,17 @@ Eigen::Matrix4f Sim3Solver::iterate(int nIterations, bool &bNoMore, vector vAvailableIndices[randi] = vAvailableIndices.back(); vAvailableIndices.pop_back(); + }*/ + + + // Get min set of points + int startingIndex = (nCurrentIterations -1) * 3; + for(short i = startingIndex; i < nCurrentIterations * 3; ++i) + { + P3Dc1i.col(i - startingIndex) = mvX3Dc1[i]; + P3Dc2i.col(i - startingIndex) = mvX3Dc2[i]; } + ComputeSim3(P3Dc1i,P3Dc2i); std::cout << "iteration number: " << nCurrentIterations << std::endl; From 70c6d156e9d6e5c3403cf73366ef66ff671d1d62 Mon Sep 17 00:00:00 2001 From: galh97 Date: Sun, 11 Jun 2023 16:37:48 +0300 Subject: [PATCH 3/4] bug --- include/LoopClosing.h | 2 +- include/ORBmatcher.h | 2 +- include/Sim3Solver.h | 7 ++-- src/LoopClosing.cc | 64 ++++++++++++++--------------------- src/ORBmatcher.cc | 7 ++-- src/Sim3Solver.cc | 79 ++++++++++++++++++++++--------------------- 6 files changed, 76 insertions(+), 85 deletions(-) diff --git a/include/LoopClosing.h b/include/LoopClosing.h index 6e53d88335..91c5e2b11e 100644 --- a/include/LoopClosing.h +++ b/include/LoopClosing.h @@ -144,7 +144,7 @@ class LoopClosing void CheckObservations(set &spKFsMap1, set &spKFsMap2); - static bool compareByValue(const std::pair& a, const std::pair& b); + static bool compareByValue(const std::pair& a, const std::pair& b); void ResetIfRequested(); bool mbResetRequested; diff --git a/include/ORBmatcher.h b/include/ORBmatcher.h index 1ad46cb034..d7a1366a52 100644 --- a/include/ORBmatcher.h +++ b/include/ORBmatcher.h @@ -66,7 +66,7 @@ namespace ORB_SLAM3 // Brute force constrained to ORB that belong to the same vocabulary node (at a certain level) // Used in Relocalisation and Loop Detection int SearchByBoW(KeyFrame *pKF, Frame &F, std::vector &vpMapPointMatches); - int SearchByBoW(KeyFrame *pKF1, KeyFrame* pKF2, std::vector &vpMatches12, std::map mapPointToDistance, std::map mapPointToKeyFrame); + int SearchByBoW(KeyFrame *pKF1, KeyFrame* pKF2, std::vector &vpMatches12, std::map &mapPointsIndexToDistance, std::vector> &mapPoint1ToMapPoint2Pairs, std::map &mapPointToKeyFrame); // Matching for the Map Initialization (only used in the monocular case) int SearchForInitialization(Frame &F1, Frame &F2, std::vector &vbPrevMatched, std::vector &vnMatches12, int windowSize=10); diff --git a/include/Sim3Solver.h b/include/Sim3Solver.h index 959fbfd98c..56f40c7006 100644 --- a/include/Sim3Solver.h +++ b/include/Sim3Solver.h @@ -34,15 +34,16 @@ class Sim3Solver { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - Sim3Solver(KeyFrame* pKF1, KeyFrame* pKF2, const std::vector &vpMatched12, const bool bFixScale = true, - std::map mapPointToKeyFrame = std::map()); + Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector &mapPoints1, + const vector &vpMatched12, const bool bFixScale, + std::map mapPointToKeyFrame); void SetRansacParameters(double probability = 0.99, int minInliers = 6 , int maxIterations = 300); Eigen::Matrix4f find(std::vector &vbInliers12, int &nInliers); Eigen::Matrix4f iterate(int nIterations, bool &bNoMore, std::vector &vbInliers, int &nInliers); - Eigen::Matrix4f iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers, bool &bConverge, std::vector> pairsPointToDistance); + Eigen::Matrix4f iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers, bool &bConverge); Eigen::Matrix4f GetEstimatedTransformation(); Eigen::Matrix3f GetEstimatedRotation(); diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc index 2355c124fc..ed00372532 100644 --- a/src/LoopClosing.cc +++ b/src/LoopClosing.cc @@ -660,7 +660,6 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, std::vector > vvpMatchedMPs; //for every cov kf, all matched map points vvpMatchedMPs.resize(vpCovKFi.size()); - std::set spMatchedMPi; int numBoWMatches = 0; KeyFrame* pMostBoWMatchesKF = pKFi; @@ -670,7 +669,8 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, std::vector vpKeyFrameMatchedMP = std::vector(mpCurrentKF->GetMapPointMatches().size(), static_cast(NULL)); int nIndexMostBoWMatchesKF=0; - std::map mapPointToDistance; + std::map mapPointsIndexToDistance; + std::vector> mapPoint1ToMapPoint2Pairs; std::map mapPointToKeyFrame; for(int j=0; j &vpBowCand, continue; //get matches by bow for our current frame with one of the cov kf of the i'th Bow Candidates - int num = matcherBoW.SearchByBoW(mpCurrentKF, vpCovKFi[j], vvpMatchedMPs[j], mapPointToDistance, mapPointToKeyFrame); //fill the vvpMatchedMPs[j] with all matched map points of vpCovKFi[j] with mpCurrentKF + int num = matcherBoW.SearchByBoW(mpCurrentKF, vpCovKFi[j], vvpMatchedMPs[j], mapPointsIndexToDistance, mapPoint1ToMapPoint2Pairs, mapPointToKeyFrame); //fill the vvpMatchedMPs[j] with all matched map points of vpCovKFi[j] with mpCurrentKF if (num > nMostBoWNumMatches) // pick the one with max matches { nMostBoWNumMatches = num; @@ -687,45 +687,32 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, } } + std::cout << "mapPointsIndexToDistance.size() = " << mapPointsIndexToDistance.size() << std::endl; + // Create a vector of pairs from the map - std::vector> pairsPointToDistance(mapPointToDistance.begin(), mapPointToDistance.end()); + std::vector> pairsPointsIndexToDistance(mapPointsIndexToDistance.begin(), mapPointsIndexToDistance.end()); // Sort the vector based on the values using the custom comparator function - std::sort(pairsPointToDistance.begin(), pairsPointToDistance.end(), LoopClosing::compareByValue); - - std::vector sortedMapPointsByScore; - - for (const auto& pair : pairsPointToDistance) { - MapPoint* mapPoint = pair.first; - if(!mapPoint || mapPoint->isBad()){ + //std::sort(pairsPointsIndexToDistance.begin(), pairsPointsIndexToDistance.end(), LoopClosing::compareByValue); + + std::vector sortedMapPoints1ByScore; + std::vector sortedMapPoints2ByScore; + std::vector sortedScores; + std::cout << "pairsPointsIndexToDistance.size = " << pairsPointsIndexToDistance.size() << std::endl; + std::cout << "the scores: " << std::endl; + for (const auto& [index,score] : pairsPointsIndexToDistance) { + MapPoint* mapPoint1 = mapPoint1ToMapPoint2Pairs[index].first; + MapPoint* mapPoint2 = mapPoint1ToMapPoint2Pairs[index].second; + std::cout << score << ", "; + if(!mapPoint1 || mapPoint1->isBad() || !mapPoint2 || mapPoint2->isBad()){ continue; } + numBoWMatches++; - sortedMapPointsByScore.push_back(mapPoint); + sortedMapPoints1ByScore.push_back(mapPoint1); + sortedMapPoints2ByScore.push_back(mapPoint2); } - // todo maybe remove this for loop, we dont need vpMatchedPoints anymore - //because we created a sorted version - sortedMapPointsByScore. - // we will need to add numBoWMatches++; to the upper loop - //we will need to fix vpKeyFrameMatchedMP or remove it - /*for(int j=0; jisBad()) - continue; - - if(spMatchedMPi.insert(pMPi_j).second) //insert if not present in list - { - numBoWMatches++; - std::cout << "k " << k << std::endl; - vpMatchedPoints[k]= pMPi_j; - vpKeyFrameMatchedMP[k] = vpCovKFi[j]; - } - } - }*/ - std::cout<< "vpKeyFrameMatchedMP.size() " << vpKeyFrameMatchedMP.size() << std::endl; //pMostBoWMatchesKF = vpCovKFi[pMostBoWMatchesKF]; @@ -737,11 +724,11 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, if(mpTracker->mSensor==System::IMU_MONOCULAR && !mpCurrentKF->GetMap()->GetIniertialBA2()) bFixedScale=false; - Sim3Solver solver = Sim3Solver(mpCurrentKF, pMostBoWMatchesKF, sortedMapPointsByScore, bFixedScale, mapPointToKeyFrame); + Sim3Solver solver = Sim3Solver(mpCurrentKF, pMostBoWMatchesKF, sortedMapPoints1ByScore, sortedMapPoints2ByScore, bFixedScale, mapPointToKeyFrame); std::cout << "nBoWInliers before " << nBoWInliers << std::endl; nBoWInliers = std::max(nBoWInliers, solver.N / 4); std::cout << "nBoWInliers after " << nBoWInliers << std::endl; - solver.SetRansacParameters(0.99, nBoWInliers, 300); // at least 15 inliers- todo - fix 15 to be in relation to the amount of matched points, distance from middle, etc.. + solver.SetRansacParameters(0.99, nBoWInliers, 300); bool bNoMore = false; vector vbInliers; @@ -750,8 +737,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, Eigen::Matrix4f mTcm; while(!bConverge && !bNoMore) { - mTcm = solver.iterate(20,bNoMore, vbInliers, nInliers, bConverge, pairsPointToDistance); - //Verbose::PrintMess("BoW guess: Solver achieve " + to_string(nInliers) + " geometrical inliers among " + to_string(nBoWInliers) + " BoW matches", Verbose::VERBOSITY_DEBUG); + mTcm = solver.iterate(20,bNoMore, vbInliers, nInliers, bConverge); } if(bConverge) @@ -907,7 +893,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, return false; } -bool LoopClosing::compareByValue(const std::pair& a, const std::pair& b) { +bool LoopClosing::compareByValue(const std::pair& a, const std::pair& b) { return a.second < b.second; } diff --git a/src/ORBmatcher.cc b/src/ORBmatcher.cc index 81db6dbb1f..efd8361659 100644 --- a/src/ORBmatcher.cc +++ b/src/ORBmatcher.cc @@ -762,7 +762,7 @@ namespace ORB_SLAM3 return nmatches; } - int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector &vpMatches12, std::map mapPointToDistance, std::map mapPointToKeyFrame) + int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector &vpMatches12, std::map &mapPointsIndexToDistance, std::vector> &mapPoint1ToMapPoint2Pairs, std::map &mapPointToKeyFrame) { const vector &vKeysUn1 = pKF1->mvKeysUn; const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec; @@ -845,7 +845,10 @@ namespace ORB_SLAM3 if(bestDist1 < TH_LOW && (static_cast(bestDist1) < mfNNratio * static_cast(bestDist2))) { - mapPointToDistance[vpMapPoints2[bestIdx2]] = bestDist1; + std::cout << "bestDist1 < TH_LOW " << bestDist1 << std::endl; + mapPoint1ToMapPoint2Pairs.push_back(std::make_pair(vpMapPoints1[idx1], vpMapPoints2[bestIdx2])); + mapPointsIndexToDistance[mapPoint1ToMapPoint2Pairs.size() - 1] = bestDist1; + std::cout << "mapPointToDistance.size = " << mapPoint1ToMapPoint2Pairs.size() << std::endl; mapPointToKeyFrame[vpMapPoints2[bestIdx2]] = pKF2; vpMatches12[idx1]=vpMapPoints2[bestIdx2]; //todo why dont we need to save from vpMapPoints1 too? vbMatched2[bestIdx2]=true; diff --git a/src/Sim3Solver.cc b/src/Sim3Solver.cc index 69166a2544..94adb3f3be 100644 --- a/src/Sim3Solver.cc +++ b/src/Sim3Solver.cc @@ -32,7 +32,8 @@ namespace ORB_SLAM3 { -Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector &vpMatched12, const bool bFixScale, +Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector &vpKeyFrameMP1, + const vector &vpKeyFrameMP2, const bool bFixScale, std::map mapPointToKeyFrame): mnIterations(0), mnBestInliers(0), mbFixScale(bFixScale), pCamera1(pKF1->mpCamera), pCamera2(pKF2->mpCamera) @@ -47,13 +48,11 @@ Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector mpKF1 = pKF1; mpKF2 = pKF2; - vector vpKeyFrameMP1 = pKF1->GetMapPointMatches(); - - mN1 = vpMatched12.size(); + mN1 = vpKeyFrameMP2.size(); mvpMapPoints1.reserve(mN1); mvpMapPoints2.reserve(mN1); - mvpMatches12 = vpMatched12; + mvpMatches12 = vpKeyFrameMP2; mvnIndices1.reserve(mN1); mvX3Dc1.reserve(mN1); mvX3Dc2.reserve(mN1); @@ -70,10 +69,10 @@ Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector KeyFrame* pKFm = pKF2; //Default variable for(int i1=0; i1 if(pMP1->isBad() || pMP2->isBad()) continue; - if(!bDifferentKFs) + if(bDifferentKFs) pKFm = mapPointToKeyFrame[pMP2]; int indexKF1 = get<0>(pMP1->GetIndexInKeyFrame(pKF1)); @@ -108,6 +107,9 @@ Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector Eigen::Vector3f X3D2w = pMP2->GetWorldPos(); mvX3Dc2.push_back(Rcw2*X3D2w+tcw2); + + std::cout << "mvX3Dc1.size = " << mvX3Dc1.size() << std::endl; + std::cout << "mvX3Dc2.size = " << mvX3Dc2.size() << std::endl; mvAllIndices.push_back(idx); idx++; @@ -215,7 +217,7 @@ Eigen::Matrix4f Sim3Solver::iterate(int nIterations, bool &bNoMore, vector return Eigen::Matrix4f::Identity(); } -Eigen::Matrix4f Sim3Solver::iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers, bool &bConverge, std::vector> pairsPointToDistance) +Eigen::Matrix4f Sim3Solver::iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers, bool &bConverge) { bNoMore = false; bConverge = false; @@ -237,38 +239,34 @@ Eigen::Matrix4f Sim3Solver::iterate(int nIterations, bool &bNoMore, vector int nCurrentIterations = 0; Eigen::Matrix4f bestSim3; - - while(mnIterations mBestRotation = mR12i; mBestTranslation = mt12i; mBestScale = ms12i; - std::cout << "if(mnInliersi>mRansacMinInliers) - mnIterations: " << mnIterations << "mRansacMaxIts: " << mRansacMaxIts << std::endl; - + if(mnInliersi>mRansacMinInliers) { nInliers = mnInliersi; @@ -299,6 +296,9 @@ Eigen::Matrix4f Sim3Solver::iterate(int nIterations, bool &bNoMore, vector bestSim3 = mBestT12; } } + + nCurrentIterations++; + mnIterations++; } std::cout << "if(mnIterations>=mRansacMaxIts) - mnIterations: " << mnIterations << "mRansacMaxIts: " << mRansacMaxIts << std::endl; @@ -381,8 +381,9 @@ void Sim3Solver::ComputeSim3(Eigen::Matrix3f &P1, Eigen::Matrix3f &P2) double ang=atan2(vec.norm(),evec(0,maxIndex)); vec = 2*ang*vec/vec.norm(); //Angle-axis representation. quaternion angle is the half + std::cout<<"starts exp in sim3"< Date: Fri, 16 Jun 2023 12:35:58 +0300 Subject: [PATCH 4/4] added dynamic iteration number, and fixed all bugs --- include/ORBmatcher.h | 2 +- include/Sim3Solver.h | 6 ++-- src/LoopClosing.cc | 38 ++++++++++++++----------- src/ORBmatcher.cc | 5 +--- src/Sim3Solver.cc | 67 +++++++++++++++----------------------------- 5 files changed, 49 insertions(+), 69 deletions(-) diff --git a/include/ORBmatcher.h b/include/ORBmatcher.h index d7a1366a52..c907de7069 100644 --- a/include/ORBmatcher.h +++ b/include/ORBmatcher.h @@ -66,7 +66,7 @@ namespace ORB_SLAM3 // Brute force constrained to ORB that belong to the same vocabulary node (at a certain level) // Used in Relocalisation and Loop Detection int SearchByBoW(KeyFrame *pKF, Frame &F, std::vector &vpMapPointMatches); - int SearchByBoW(KeyFrame *pKF1, KeyFrame* pKF2, std::vector &vpMatches12, std::map &mapPointsIndexToDistance, std::vector> &mapPoint1ToMapPoint2Pairs, std::map &mapPointToKeyFrame); + int SearchByBoW(KeyFrame *pKF1, KeyFrame* pKF2, std::vector &vpMatches12, std::map &mapPointsIndexToDistance, std::vector> &mapPoint1ToMapPoint2Pairs, std::map &mapPointToKeyFrame, std::set sMatchedMapPoints1, std::set sMatchedMapPoints2); // Matching for the Map Initialization (only used in the monocular case) int SearchForInitialization(Frame &F1, Frame &F2, std::vector &vbPrevMatched, std::vector &vnMatches12, int windowSize=10); diff --git a/include/Sim3Solver.h b/include/Sim3Solver.h index 56f40c7006..abf1ea0bdb 100644 --- a/include/Sim3Solver.h +++ b/include/Sim3Solver.h @@ -35,9 +35,9 @@ class Sim3Solver public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector &mapPoints1, - const vector &vpMatched12, const bool bFixScale, - std::map mapPointToKeyFrame); - + const vector &vpMatched12, + std::map mapPointToKeyFrame, const bool bFixScale = true); + bool equalEigenVector(Eigen::Vector3f lastEigen, Eigen::Vector3f newEigen); void SetRansacParameters(double probability = 0.99, int minInliers = 6 , int maxIterations = 300); Eigen::Matrix4f find(std::vector &vbInliers12, int &nInliers); diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc index ed00372532..8f2e5fb0b6 100644 --- a/src/LoopClosing.cc +++ b/src/LoopClosing.cc @@ -673,13 +673,16 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, std::vector> mapPoint1ToMapPoint2Pairs; std::map mapPointToKeyFrame; + std::set sMatchedMapPoints1; + std::set sMatchedMapPoints2; + for(int j=0; jisBad()) continue; //get matches by bow for our current frame with one of the cov kf of the i'th Bow Candidates - int num = matcherBoW.SearchByBoW(mpCurrentKF, vpCovKFi[j], vvpMatchedMPs[j], mapPointsIndexToDistance, mapPoint1ToMapPoint2Pairs, mapPointToKeyFrame); //fill the vvpMatchedMPs[j] with all matched map points of vpCovKFi[j] with mpCurrentKF + int num = matcherBoW.SearchByBoW(mpCurrentKF, vpCovKFi[j], vvpMatchedMPs[j], mapPointsIndexToDistance, mapPoint1ToMapPoint2Pairs, mapPointToKeyFrame, sMatchedMapPoints1, sMatchedMapPoints2); //fill the vvpMatchedMPs[j] with all matched map points of vpCovKFi[j] with mpCurrentKF if (num > nMostBoWNumMatches) // pick the one with max matches { nMostBoWNumMatches = num; @@ -687,35 +690,41 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, } } - std::cout << "mapPointsIndexToDistance.size() = " << mapPointsIndexToDistance.size() << std::endl; - // Create a vector of pairs from the map std::vector> pairsPointsIndexToDistance(mapPointsIndexToDistance.begin(), mapPointsIndexToDistance.end()); // Sort the vector based on the values using the custom comparator function - //std::sort(pairsPointsIndexToDistance.begin(), pairsPointsIndexToDistance.end(), LoopClosing::compareByValue); + std::sort(pairsPointsIndexToDistance.begin(), pairsPointsIndexToDistance.end(), LoopClosing::compareByValue); std::vector sortedMapPoints1ByScore; std::vector sortedMapPoints2ByScore; - std::vector sortedScores; + int lastScore = -1; + int repetitions = 1; std::cout << "pairsPointsIndexToDistance.size = " << pairsPointsIndexToDistance.size() << std::endl; std::cout << "the scores: " << std::endl; for (const auto& [index,score] : pairsPointsIndexToDistance) { MapPoint* mapPoint1 = mapPoint1ToMapPoint2Pairs[index].first; MapPoint* mapPoint2 = mapPoint1ToMapPoint2Pairs[index].second; - std::cout << score << ", "; if(!mapPoint1 || mapPoint1->isBad() || !mapPoint2 || mapPoint2->isBad()){ continue; } + if(score == lastScore){ + repetitions++; + if(repetitions == 5){ + break; + } + } else { + repetitions = 1; + lastScore = score; + } + + std::cout << score << ", "; numBoWMatches++; sortedMapPoints1ByScore.push_back(mapPoint1); sortedMapPoints2ByScore.push_back(mapPoint2); } - - std::cout<< "vpKeyFrameMatchedMP.size() " << vpKeyFrameMatchedMP.size() << std::endl; - - //pMostBoWMatchesKF = vpCovKFi[pMostBoWMatchesKF]; + std::cout << std::endl; if(numBoWMatches >= nBoWMatches) // TODO pick a good threshold, now nBoWMatches=20 { @@ -724,10 +733,8 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, if(mpTracker->mSensor==System::IMU_MONOCULAR && !mpCurrentKF->GetMap()->GetIniertialBA2()) bFixedScale=false; - Sim3Solver solver = Sim3Solver(mpCurrentKF, pMostBoWMatchesKF, sortedMapPoints1ByScore, sortedMapPoints2ByScore, bFixedScale, mapPointToKeyFrame); - std::cout << "nBoWInliers before " << nBoWInliers << std::endl; + Sim3Solver solver = Sim3Solver(mpCurrentKF, pMostBoWMatchesKF, sortedMapPoints1ByScore, sortedMapPoints2ByScore, mapPointToKeyFrame, bFixedScale); nBoWInliers = std::max(nBoWInliers, solver.N / 4); - std::cout << "nBoWInliers after " << nBoWInliers << std::endl; solver.SetRansacParameters(0.99, nBoWInliers, 300); bool bNoMore = false; @@ -735,10 +742,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, int nInliers; bool bConverge = false; Eigen::Matrix4f mTcm; - while(!bConverge && !bNoMore) - { - mTcm = solver.iterate(20,bNoMore, vbInliers, nInliers, bConverge); - } + mTcm = solver.iterate(300, bNoMore, vbInliers, nInliers, bConverge); if(bConverge) { diff --git a/src/ORBmatcher.cc b/src/ORBmatcher.cc index efd8361659..72793a60dc 100644 --- a/src/ORBmatcher.cc +++ b/src/ORBmatcher.cc @@ -762,7 +762,7 @@ namespace ORB_SLAM3 return nmatches; } - int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector &vpMatches12, std::map &mapPointsIndexToDistance, std::vector> &mapPoint1ToMapPoint2Pairs, std::map &mapPointToKeyFrame) + int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector &vpMatches12, std::map &mapPointsIndexToDistance, std::vector> &mapPoint1ToMapPoint2Pairs, std::map &mapPointToKeyFrame, std::set sMatchedMapPoints1, std::set sMatchedMapPoints2) { const vector &vKeysUn1 = pKF1->mvKeysUn; const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec; @@ -845,10 +845,8 @@ namespace ORB_SLAM3 if(bestDist1 < TH_LOW && (static_cast(bestDist1) < mfNNratio * static_cast(bestDist2))) { - std::cout << "bestDist1 < TH_LOW " << bestDist1 << std::endl; mapPoint1ToMapPoint2Pairs.push_back(std::make_pair(vpMapPoints1[idx1], vpMapPoints2[bestIdx2])); mapPointsIndexToDistance[mapPoint1ToMapPoint2Pairs.size() - 1] = bestDist1; - std::cout << "mapPointToDistance.size = " << mapPoint1ToMapPoint2Pairs.size() << std::endl; mapPointToKeyFrame[vpMapPoints2[bestIdx2]] = pKF2; vpMatches12[idx1]=vpMapPoints2[bestIdx2]; //todo why dont we need to save from vpMapPoints1 too? vbMatched2[bestIdx2]=true; @@ -900,7 +898,6 @@ namespace ORB_SLAM3 } } } - std::cout<< "nmatches " << nmatches<< "mbCheckOrientation" << mbCheckOrientation << std::endl; return nmatches; } diff --git a/src/Sim3Solver.cc b/src/Sim3Solver.cc index 94adb3f3be..88b0990f39 100644 --- a/src/Sim3Solver.cc +++ b/src/Sim3Solver.cc @@ -33,15 +33,15 @@ namespace ORB_SLAM3 Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector &vpKeyFrameMP1, - const vector &vpKeyFrameMP2, const bool bFixScale, - std::map mapPointToKeyFrame): + const vector &vpKeyFrameMP2, + std::map mapPointToKeyFrame, const bool bFixScale): mnIterations(0), mnBestInliers(0), mbFixScale(bFixScale), pCamera1(pKF1->mpCamera), pCamera2(pKF2->mpCamera) { - bool bDifferentKFs = false; //todo + bool bDifferentKFs = true; //todo if(mapPointToKeyFrame.empty()) { - bDifferentKFs = true; + bDifferentKFs = false; //vpKeyFrameMatchedMP = vector(vpMatched12.size(), pKF2); } @@ -69,17 +69,17 @@ Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector & KeyFrame* pKFm = pKF2; //Default variable for(int i1=0; i1isBad() || pMP2->isBad()) continue; - + if(bDifferentKFs) pKFm = mapPointToKeyFrame[pMP2]; @@ -95,6 +95,13 @@ Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector & const float sigmaSquare1 = pKF1->mvLevelSigma2[kp1.octave]; const float sigmaSquare2 = pKFm->mvLevelSigma2[kp2.octave]; + Eigen::Vector3f X3D1w = pMP1->GetWorldPos(); + Eigen::Vector3f X3D2w = pMP2->GetWorldPos(); + if(equalEigenVector(mvX3Dc1[mvX3Dc1.size() - 1], Rcw1*X3D1w+tcw1) || + equalEigenVector(mvX3Dc2[mvX3Dc2.size() - 1], Rcw2*X3D2w+tcw2)){ + continue; + } + mvnMaxError1.push_back(9.210*sigmaSquare1); mvnMaxError2.push_back(9.210*sigmaSquare2); @@ -102,15 +109,9 @@ Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector & mvpMapPoints2.push_back(pMP2); mvnIndices1.push_back(i1); - Eigen::Vector3f X3D1w = pMP1->GetWorldPos(); mvX3Dc1.push_back(Rcw1*X3D1w+tcw1); - - Eigen::Vector3f X3D2w = pMP2->GetWorldPos(); mvX3Dc2.push_back(Rcw2*X3D2w+tcw2); - std::cout << "mvX3Dc1.size = " << mvX3Dc1.size() << std::endl; - std::cout << "mvX3Dc2.size = " << mvX3Dc2.size() << std::endl; - mvAllIndices.push_back(idx); idx++; } @@ -122,6 +123,10 @@ Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector & SetRansacParameters(); } +bool Sim3Solver::equalEigenVector(Eigen::Vector3f lastEigen, Eigen::Vector3f newEigen){ + return lastEigen.x() == newEigen.x() && lastEigen.y() == newEigen.y() && lastEigen.z() == newEigen.z(); +} + void Sim3Solver::SetRansacParameters(double probability, int minInliers, int maxIterations) { mRansacProb = probability; @@ -224,51 +229,31 @@ Eigen::Matrix4f Sim3Solver::iterate(int nIterations, bool &bNoMore, vector vbInliers = vector(mN1,false); nInliers=0; - std::cout << "there are " << N << "map points" << std::endl; if(N vAvailableIndices; - Eigen::Matrix3f P3Dc1i; Eigen::Matrix3f P3Dc2i; int nCurrentIterations = 0; + Eigen::Matrix4f bestSim3; - std::cout << "mvX3Dc1.size(): " << mvX3Dc1.size() <=mnBestInliers) @@ -284,7 +269,6 @@ Eigen::Matrix4f Sim3Solver::iterate(int nIterations, bool &bNoMore, vector { nInliers = mnInliersi; for(int i=0; i=mRansacMaxIts) - mnIterations: " << mnIterations << "mRansacMaxIts: " << mRansacMaxIts << std::endl; - - if(mnIterations>=mRansacMaxIts) + if(mnIterations>=mRansacMaxIts || (mnIterations + 1) * 3 >= mvX3Dc1.size() || (mnIterations + 1) * 3 >= mvX3Dc2.size()) bNoMore=true; return bestSim3; @@ -381,9 +362,7 @@ void Sim3Solver::ComputeSim3(Eigen::Matrix3f &P1, Eigen::Matrix3f &P2) double ang=atan2(vec.norm(),evec(0,maxIndex)); vec = 2*ang*vec/vec.norm(); //Angle-axis representation. quaternion angle is the half - std::cout<<"starts exp in sim3"<