diff --git a/CMakeLists.txt b/CMakeLists.txt index 016e74354d6..1761ee77a44 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.24.1) project(ORB_SLAM3) IF(NOT CMAKE_BUILD_TYPE) @@ -7,37 +7,34 @@ ENDIF() MESSAGE("Build type: " ${CMAKE_BUILD_TYPE}) -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -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++17 or C++0x support include(CheckCXXCompilerFlag) -CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) +CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) -if(COMPILER_SUPPORTS_CXX11) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") - add_definitions(-DCOMPILEDWITHC11) - message(STATUS "Using flag -std=c++11.") +if(COMPILER_SUPPORTS_CXX17) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") + add_definitions(-DCOMPILEDWITHC17) + message(STATUS "Using flag -std=c++17.") 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++17 support. Please use a different C++ compiler.") endif() LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) -find_package(OpenCV 4.4) +find_package(OpenCV 3) if(NOT OpenCV_FOUND) - message(FATAL_ERROR "OpenCV > 4.4 not found.") + message(FATAL_ERROR "OpenCV > 3 not found.") endif() -MESSAGE("OPENCV VERSION:") -MESSAGE(${OpenCV_VERSION}) - find_package(Eigen3 3.1.0 REQUIRED) find_package(Pangolin REQUIRED) find_package(realsense2) @@ -53,6 +50,8 @@ ${Pangolin_INCLUDE_DIRS} set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) + + add_library(${PROJECT_NAME} SHARED src/System.cc src/Tracking.cc @@ -121,6 +120,7 @@ ${EIGEN3_LIBS} ${Pangolin_LIBRARIES} ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so +-lboost_filesystem -lboost_serialization -lcrypto ) @@ -135,256 +135,7 @@ if(realsense2_FOUND) ) endif() - -# Build examples - -# RGB-D examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D) - -add_executable(rgbd_tum - Examples/RGB-D/rgbd_tum.cc) -target_link_libraries(rgbd_tum ${PROJECT_NAME}) - -if(realsense2_FOUND) - add_executable(rgbd_realsense_D435i - Examples/RGB-D/rgbd_realsense_D435i.cc) - target_link_libraries(rgbd_realsense_D435i ${PROJECT_NAME}) -endif() - - -# RGB-D inertial examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D-Inertial) - -if(realsense2_FOUND) - add_executable(rgbd_inertial_realsense_D435i - Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc) - target_link_libraries(rgbd_inertial_realsense_D435i ${PROJECT_NAME}) -endif() - -#Stereo examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo) - -add_executable(stereo_kitti - Examples/Stereo/stereo_kitti.cc) -target_link_libraries(stereo_kitti ${PROJECT_NAME}) - -add_executable(stereo_euroc - Examples/Stereo/stereo_euroc.cc) -target_link_libraries(stereo_euroc ${PROJECT_NAME}) - -add_executable(stereo_tum_vi - Examples/Stereo/stereo_tum_vi.cc) -target_link_libraries(stereo_tum_vi ${PROJECT_NAME}) - -if(realsense2_FOUND) - add_executable(stereo_realsense_t265 - Examples/Stereo/stereo_realsense_t265.cc) - target_link_libraries(stereo_realsense_t265 ${PROJECT_NAME}) - - add_executable(stereo_realsense_D435i - Examples/Stereo/stereo_realsense_D435i.cc) - target_link_libraries(stereo_realsense_D435i ${PROJECT_NAME}) -endif() - -#Monocular examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular) - -add_executable(mono_tum - Examples/Monocular/mono_tum.cc) -target_link_libraries(mono_tum ${PROJECT_NAME}) - -add_executable(mono_kitti - Examples/Monocular/mono_kitti.cc) -target_link_libraries(mono_kitti ${PROJECT_NAME}) - -add_executable(mono_euroc - Examples/Monocular/mono_euroc.cc) -target_link_libraries(mono_euroc ${PROJECT_NAME}) - -add_executable(mono_tum_vi - Examples/Monocular/mono_tum_vi.cc) -target_link_libraries(mono_tum_vi ${PROJECT_NAME}) - -if(realsense2_FOUND) - add_executable(mono_realsense_t265 - Examples/Monocular/mono_realsense_t265.cc) - target_link_libraries(mono_realsense_t265 ${PROJECT_NAME}) - - add_executable(mono_realsense_D435i - Examples/Monocular/mono_realsense_D435i.cc) - target_link_libraries(mono_realsense_D435i ${PROJECT_NAME}) -endif() - -#Monocular inertial examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular-Inertial) - -add_executable(mono_inertial_euroc - Examples/Monocular-Inertial/mono_inertial_euroc.cc) -target_link_libraries(mono_inertial_euroc ${PROJECT_NAME}) - -add_executable(mono_inertial_tum_vi - Examples/Monocular-Inertial/mono_inertial_tum_vi.cc) -target_link_libraries(mono_inertial_tum_vi ${PROJECT_NAME}) - -if(realsense2_FOUND) - add_executable(mono_inertial_realsense_t265 - Examples/Monocular-Inertial/mono_inertial_realsense_t265.cc) - target_link_libraries(mono_inertial_realsense_t265 ${PROJECT_NAME}) - - add_executable(mono_inertial_realsense_D435i - Examples/Monocular-Inertial/mono_inertial_realsense_D435i.cc) - target_link_libraries(mono_inertial_realsense_D435i ${PROJECT_NAME}) -endif() - -#Stereo Inertial examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo-Inertial) - -add_executable(stereo_inertial_euroc - Examples/Stereo-Inertial/stereo_inertial_euroc.cc) -target_link_libraries(stereo_inertial_euroc ${PROJECT_NAME}) - -add_executable(stereo_inertial_tum_vi - Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc) -target_link_libraries(stereo_inertial_tum_vi ${PROJECT_NAME}) - -if(realsense2_FOUND) - add_executable(stereo_inertial_realsense_t265 - Examples/Stereo-Inertial/stereo_inertial_realsense_t265.cc) - target_link_libraries(stereo_inertial_realsense_t265 ${PROJECT_NAME}) - - add_executable(stereo_inertial_realsense_D435i - Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc) - target_link_libraries(stereo_inertial_realsense_D435i ${PROJECT_NAME}) -endif() - -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Calibration) -if(realsense2_FOUND) - add_executable(recorder_realsense_D435i - Examples/Calibration/recorder_realsense_D435i.cc) - target_link_libraries(recorder_realsense_D435i ${PROJECT_NAME}) - - add_executable(recorder_realsense_T265 - Examples/Calibration/recorder_realsense_T265.cc) - target_link_libraries(recorder_realsense_T265 ${PROJECT_NAME}) -endif() - -#Old examples - -# RGB-D examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/RGB-D) - -add_executable(rgbd_tum_old - Examples_old/RGB-D/rgbd_tum.cc) -target_link_libraries(rgbd_tum_old ${PROJECT_NAME}) - -if(realsense2_FOUND) - add_executable(rgbd_realsense_D435i_old - Examples_old/RGB-D/rgbd_realsense_D435i.cc) - target_link_libraries(rgbd_realsense_D435i_old ${PROJECT_NAME}) -endif() - - -# RGB-D inertial examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/RGB-D-Inertial) - -if(realsense2_FOUND) - add_executable(rgbd_inertial_realsense_D435i_old - Examples_old/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc) - target_link_libraries(rgbd_inertial_realsense_D435i_old ${PROJECT_NAME}) -endif() - -#Stereo examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/Stereo) - -add_executable(stereo_kitti_old - Examples_old/Stereo/stereo_kitti.cc) -target_link_libraries(stereo_kitti_old ${PROJECT_NAME}) - -add_executable(stereo_euroc_old - Examples_old/Stereo/stereo_euroc.cc) -target_link_libraries(stereo_euroc_old ${PROJECT_NAME}) - -add_executable(stereo_tum_vi_old - Examples_old/Stereo/stereo_tum_vi.cc) -target_link_libraries(stereo_tum_vi_old ${PROJECT_NAME}) - -if(realsense2_FOUND) - add_executable(stereo_realsense_t265_old - Examples_old/Stereo/stereo_realsense_t265.cc) - target_link_libraries(stereo_realsense_t265_old ${PROJECT_NAME}) - - add_executable(stereo_realsense_D435i_old - Examples_old/Stereo/stereo_realsense_D435i.cc) - target_link_libraries(stereo_realsense_D435i_old ${PROJECT_NAME}) -endif() - -#Monocular examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/Monocular) - -add_executable(mono_tum_old - Examples_old/Monocular/mono_tum.cc) -target_link_libraries(mono_tum_old ${PROJECT_NAME}) - -add_executable(mono_kitti_old - Examples_old/Monocular/mono_kitti.cc) -target_link_libraries(mono_kitti_old ${PROJECT_NAME}) - -add_executable(mono_euroc_old - Examples_old/Monocular/mono_euroc.cc) -target_link_libraries(mono_euroc_old ${PROJECT_NAME}) - -add_executable(mono_tum_vi_old - Examples_old/Monocular/mono_tum_vi.cc) -target_link_libraries(mono_tum_vi_old ${PROJECT_NAME}) - -if(realsense2_FOUND) - add_executable(mono_realsense_t265_old - Examples_old/Monocular/mono_realsense_t265.cc) - target_link_libraries(mono_realsense_t265_old ${PROJECT_NAME}) - - add_executable(mono_realsense_D435i_old - Examples_old/Monocular/mono_realsense_D435i.cc) - target_link_libraries(mono_realsense_D435i_old ${PROJECT_NAME}) -endif() - -#Monocular inertial examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/Monocular-Inertial) - -add_executable(mono_inertial_euroc_old - Examples_old/Monocular-Inertial/mono_inertial_euroc.cc) -target_link_libraries(mono_inertial_euroc_old ${PROJECT_NAME}) - -add_executable(mono_inertial_tum_vi_old - Examples_old/Monocular-Inertial/mono_inertial_tum_vi.cc) -target_link_libraries(mono_inertial_tum_vi_old ${PROJECT_NAME}) - -if(realsense2_FOUND) - add_executable(mono_inertial_realsense_t265_old - Examples_old/Monocular-Inertial/mono_inertial_realsense_t265.cc) - target_link_libraries(mono_inertial_realsense_t265_old ${PROJECT_NAME}) - - add_executable(mono_inertial_realsense_D435i_old - Examples_old/Monocular-Inertial/mono_inertial_realsense_D435i.cc) - target_link_libraries(mono_inertial_realsense_D435i_old ${PROJECT_NAME}) -endif() - -#Stereo Inertial examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/Stereo-Inertial) - -add_executable(stereo_inertial_euroc_old - Examples_old/Stereo-Inertial/stereo_inertial_euroc.cc) -target_link_libraries(stereo_inertial_euroc_old ${PROJECT_NAME}) - -add_executable(stereo_inertial_tum_vi_old - Examples_old/Stereo-Inertial/stereo_inertial_tum_vi.cc) -target_link_libraries(stereo_inertial_tum_vi_old ${PROJECT_NAME}) - -if(realsense2_FOUND) - add_executable(stereo_inertial_realsense_t265_old - Examples_old/Stereo-Inertial/stereo_inertial_realsense_t265.cc) - target_link_libraries(stereo_inertial_realsense_t265_old ${PROJECT_NAME}) - - add_executable(stereo_inertial_realsense_D435i_old - Examples_old/Stereo-Inertial/stereo_inertial_realsense_D435i.cc) - target_link_libraries(stereo_inertial_realsense_D435i_old ${PROJECT_NAME}) -endif() +# FOR ILLIXR +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/src) +add_library(plugin SHARED src/plugin.cpp) +target_link_libraries(plugin ${PROJECT_NAME}) \ No newline at end of file diff --git a/Examples/RGB-D/ASUS.yaml b/Examples/RGB-D/ASUS.yaml new file mode 100644 index 00000000000..228d1f477cf --- /dev/null +++ b/Examples/RGB-D/ASUS.yaml @@ -0,0 +1,69 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. Adjust them! +#-------------------------------------------------------------------------------------------- +File.version: "1.0" + +Camera.type: "PinHole" + + +# Camera calibration and distortion parameters (OpenCV) +Camera1.fx: 570.342163 +Camera1.fy: 570.342224 +Camera1.cx: 320.000000 +Camera1.cy: 240.000000 + +Camera1.k1: 0.0 +Camera1.k2: 0.0 +Camera1.p1: 0.0 +Camera1.p2: 0.0 + +Camera.width: 640 +Camera.height: 480 + +# Camera frames per second +Camera.fps: 30 + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 1 + +# Close/Far threshold. Baseline times. +Stereo.ThDepth: 40.0 +Stereo.b: 0.0745 + +# Depth map values factor +RGBD.DepthMapFactor: 1000.0 + +#-------------------------------------------------------------------------------------------- +# ORB Parameters +#-------------------------------------------------------------------------------------------- +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 1250 + +# ORB Extractor: Scale factor between levels in the scale pyramid +ORBextractor.scaleFactor: 1.2 + +# ORB Extractor: Number of levels in the scale pyramid +ORBextractor.nLevels: 8 + +# ORB Extractor: Fast threshold +# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. +# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST +# You can lower these values if your images have low contrast +ORBextractor.iniThFAST: 20 +ORBextractor.minThFAST: 7 + +#-------------------------------------------------------------------------------------------- +# Viewer Parameters +#-------------------------------------------------------------------------------------------- +Viewer.KeyFrameSize: 0.05 +Viewer.KeyFrameLineWidth: 1.0 +Viewer.GraphLineWidth: 0.9 +Viewer.PointSize: 2.0 +Viewer.CameraSize: 0.08 +Viewer.CameraLineWidth: 3.0 +Viewer.ViewpointX: 0.0 +Viewer.ViewpointY: -0.7 +Viewer.ViewpointZ: -3.5 +Viewer.ViewpointF: 500.0 diff --git a/Examples/RGB-D/ETH3D.yaml b/Examples/RGB-D/ETH3D.yaml new file mode 100644 index 00000000000..dcace2a513a --- /dev/null +++ b/Examples/RGB-D/ETH3D.yaml @@ -0,0 +1,69 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. Adjust them! +#-------------------------------------------------------------------------------------------- +File.version: "1.0" + +Camera.type: "PinHole" + +# Camera calibration and distortion parameters (OpenCV) +Camera1.fx: 726.28741455078 +Camera1.fy: 726.28741455078 +Camera1.cx: 354.6496887207 +Camera1.cy: 186.46566772461 + +Camera1.k1: 0.0 +Camera1.k2: 0.0 +Camera1.p1: 0.0 +Camera1.p2: 0.0 + +Camera.width: 739 +Camera.height: 458 + +# Camera frames per second +Camera.fps: 27 + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 1 + +# Close/Far threshold. Baseline times. +Stereo.ThDepth: 40.0 +Stereo.b: 0.0747 + +# Depth map values factor +RGBD.DepthMapFactor: 5000.0 + +#-------------------------------------------------------------------------------------------- +# ORB Parameters +#-------------------------------------------------------------------------------------------- + +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 1000 # 1000 # 1200 + +# ORB Extractor: Scale factor between levels in the scale pyramid +ORBextractor.scaleFactor: 1.2 + +# ORB Extractor: Number of levels in the scale pyramid +ORBextractor.nLevels: 8 + +# ORB Extractor: Fast threshold +# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. +# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST +# You can lower these values if your images have low contrast +ORBextractor.iniThFAST: 10 #15 # 20 +ORBextractor.minThFAST: 3 #5 # 7 + +#-------------------------------------------------------------------------------------------- +# Viewer Parameters +#-------------------------------------------------------------------------------------------- +Viewer.KeyFrameSize: 0.05 +Viewer.KeyFrameLineWidth: 1.0 +Viewer.GraphLineWidth: 0.9 +Viewer.PointSize: 2.0 +Viewer.CameraSize: 0.08 +Viewer.CameraLineWidth: 3.0 +Viewer.ViewpointX: 0.0 +Viewer.ViewpointY: -0.7 +Viewer.ViewpointZ: -1.8 +Viewer.ViewpointF: 500.0 diff --git a/Examples/Stereo-Inertial/ETH3D.yaml b/Examples/Stereo-Inertial/ETH3D.yaml new file mode 100644 index 00000000000..9f7ea92bf3b --- /dev/null +++ b/Examples/Stereo-Inertial/ETH3D.yaml @@ -0,0 +1,116 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# System config +#-------------------------------------------------------------------------------------------- + +# When the variables are commented, the system doesn't load a previous session or not store the current one + +# If the LoadFile doesn't exist, the system give a message and create a new Atlas from scratch +#System.LoadAtlasFromFile: "Session_MH01_MH02_MH03_Stereo60_Pseudo" + +# The store file is created from the current session, if a file with the same name exists it is deleted +#System.SaveAtlasToFile: "Session_MH01_MH02_MH03_Stereo60_Pseudo" + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. Adjust them! +#-------------------------------------------------------------------------------------------- +File.version: "1.0" + +Camera.type: "PinHole" + +# Camera calibration and distortion parameters (OpenCV) +# Left camera +Camera1.fx: 726.04388427734 +Camera1.fy: 726.04388427734 +Camera1.cx: 385.73248291016 +Camera1.cy: 262.19641113281 + +Camera1.k1: 0.0 +Camera1.k2: 0.0 +Camera1.p1: 0.0 +Camera1.p2: 0.0 + +# Right camera +Camera2.fx: 726.28741455078 +Camera2.fy: 726.28741455078 +Camera2.cx: 354.6496887207 +Camera2.cy: 186.46566772461 + +Camera2.k1: 0.0 +Camera2.k2: 0.0 +Camera2.p1: 0.0 +Camera2.p2: 0.0 + +Camera.width: 739 +Camera.height: 458 + +# Camera frames per second +Camera.fps: 27 + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 1 + +# Close/Far threshold. Baseline times. +Stereo.ThDepth: 40.0 + +Stereo.T_c1_c2: !!opencv-matrix + rows: 4 + cols: 4 + dt: f + data: [0.999988491139898, -0.000435223209842609, 0.00477697010206442, 0.0899049965085514, + 0.000453015132142526, 0.999992961938085, -0.00372467985798297, 0.0016838913559981, + -0.00477530547859887, 0.00372680560411671, 0.999981653528498, -0.000261055292093, + 0.0, 0.0, 0.0, 1.0] + +IMU.T_b_c1: !!opencv-matrix + rows: 4 + cols: 4 + dt: f + data: [-0.99987545, -0.01531016, 0.00383257, 0.09372005, + 0.01531, -0.99988279, -0.00007083, 0.00822336, + 0.0038332, -0.00001215, 0.99999265, 0.00122234, + 0.0, 0.0, 0.0, 1.0] + +IMU.NoiseGyro: 9.0e-04 #8.94e-05 # 1.6968e-04 +IMU.NoiseAcc: 2.0e-03 #2.24e-03 # 2.0000e-3 +IMU.GyroWalk: 2.0e-04 #2.66e-05 +IMU.AccWalk: 7.0e-04 #7.53e-05 # 3.0000e-3 + +IMU.Frequency: 460.0 + +#-------------------------------------------------------------------------------------------- +# ORB Parameters +#-------------------------------------------------------------------------------------------- + +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 1500 # 1000 # 1200 + +# ORB Extractor: Scale factor between levels in the scale pyramid +ORBextractor.scaleFactor: 1.2 + +# ORB Extractor: Number of levels in the scale pyramid +ORBextractor.nLevels: 8 + +# ORB Extractor: Fast threshold +# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. +# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST +# You can lower these values if your images have low contrast +ORBextractor.iniThFAST: 10 #15 # 20 +ORBextractor.minThFAST: 3 #5 # 7 + +#-------------------------------------------------------------------------------------------- +# Viewer Parameters +#-------------------------------------------------------------------------------------------- +Viewer.KeyFrameSize: 0.05 +Viewer.KeyFrameLineWidth: 1.0 +Viewer.GraphLineWidth: 0.9 +Viewer.PointSize: 2.0 +Viewer.CameraSize: 0.08 +Viewer.CameraLineWidth: 3.0 +Viewer.ViewpointX: 0.0 +Viewer.ViewpointY: -0.7 +Viewer.ViewpointZ: -1.8 +Viewer.ViewpointF: 500.0 +Viewer.imageViewScale: 1.0 + diff --git a/Makefile b/Makefile new file mode 100644 index 00000000000..4bc5bc2567e --- /dev/null +++ b/Makefile @@ -0,0 +1,60 @@ +nproc=$(shell python3 -c 'import multiprocessing; print( max(multiprocessing.cpu_count() - 1, 1))') + +CXX := clang++-10 +CC := clang-10 + +.PHONY: plugin.dbg.so +plugin.dbg.so: build/Debug/Makefile + make -C build/Debug "-j$(nproc)" && \ + rm -f $@ && \ + ln -s lib/libplugin.so plugin.dbg.so && \ + true + +.PHONY: plugin.opt.so +plugin.opt.so: build/Release/Makefile + make -C build/Release "-j$(nproc)" && \ + rm -f $@ && \ + ln -s lib/libplugin.so plugin.opt.so && \ + true + +build/Debug/Makefile: run_orbslam_script + mkdir -p build/Debug && \ + cd build/Debug && \ + cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_COMPILER=$(CXX) -DCMAKE_C_COMPILER=$(CC) ../.. && \ + true + +build/Release/Makefile: run_orbslam_script + mkdir -p build/Release && \ + cd build/Release && \ + cmake -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_CXX_COMPILER=$(CXX) -DCMAKE_C_COMPILER=$(CC) ../.. && \ + true + +.PHONY: run_orbslam_script +run_orbslam_script: + cd Thirdparty/DBoW2 && \ + mkdir build && \ + cd build && \ + cmake .. -DCMAKE_BUILD_TYPE=Release && \ + make -j && \ + cd ../../g2o && \ + mkdir build && \ + cd build && \ + cmake .. -DCMAKE_BUILD_TYPE=Release && \ + make -j && \ + cd ../../Sophus && \ + mkdir build && \ + cd build && \ + cmake .. -DCMAKE_BUILD_TYPE=Release && \ + make -j && \ + cd ../../../ && \ + cd Vocabulary && \ + tar -xf ORBvoc.txt.tar.gz && \ + cd .. && \ + true + +tests/run: +tests/gdb: + +.PHONY: clean +clean: + touch build && rm -rf build *.so diff --git a/Thirdparty/DBoW2/DBoW2/FORB.cpp b/Thirdparty/DBoW2/DBoW2/FORB.cpp index 1f1990c2f7a..80bf473f666 100644 --- a/Thirdparty/DBoW2/DBoW2/FORB.cpp +++ b/Thirdparty/DBoW2/DBoW2/FORB.cpp @@ -13,7 +13,7 @@ #include #include #include -#include +#include #include "FORB.h" diff --git a/build.sh b/build.sh index 96d1c0941cd..57f6c78690f 100755 --- a/build.sh +++ b/build.sh @@ -31,10 +31,3 @@ echo "Uncompress vocabulary ..." cd Vocabulary tar -xf ORBvoc.txt.tar.gz cd .. - -echo "Configuring and building ORB_SLAM3 ..." - -mkdir build -cd build -cmake .. -DCMAKE_BUILD_TYPE=Release -make -j4 diff --git a/include/System.h b/include/System.h index 872c86e734d..540bfa12235 100644 --- a/include/System.h +++ b/include/System.h @@ -120,7 +120,6 @@ class System // Returns the camera pose (empty if tracking fails). Sophus::SE3f TrackMonocular(const cv::Mat &im, const double ×tamp, const vector& vImuMeas = vector(), string filename=""); - // This stops local mapping thread (map building) and performs only camera tracking. void ActivateLocalizationMode(); // This resumes local mapping thread and performs SLAM again. @@ -186,6 +185,13 @@ class System float GetImageScale(); + // Tracker. It receives a frame and computes the associated camera pose. + // It also decides when to insert a new keyframe, create some new MapPoints and + // performs relocalization if tracking fails. + Tracking* mpTracker; + + Settings* settings_; + #ifdef REGISTER_TIMES void InsertRectTime(double& time); void InsertResizeTime(double& time); @@ -212,10 +218,7 @@ class System //Map* mpMap; Atlas* mpAtlas; - // Tracker. It receives a frame and computes the associated camera pose. - // It also decides when to insert a new keyframe, create some new MapPoints and - // performs relocalization if tracking fails. - Tracking* mpTracker; + // Local Mapper. It manages the local map and performs local bundle adjustment. LocalMapping* mpLocalMapper; @@ -260,8 +263,6 @@ class System string mStrSaveAtlasToFile; string mStrVocabularyFilePath; - - Settings* settings_; }; }// namespace ORB_SLAM diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc index 53df3324fae..433b3503bca 100644 --- a/src/LocalMapping.cc +++ b/src/LocalMapping.cc @@ -196,50 +196,51 @@ void LocalMapping::Run() timeKFCulling_ms = std::chrono::duration_cast >(time_EndKFCulling - time_EndLBA).count(); vdKFCulling_ms.push_back(timeKFCulling_ms); #endif - - if ((mTinit<50.0f) && mbInertial) - { - if(mpCurrentKeyFrame->GetMap()->isImuInitialized() && mpTracker->mState==Tracking::OK) // Enter here everytime local-mapping is called - { - if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA1()){ - if (mTinit>5.0f) - { - cout << "start VIBA 1" << endl; - mpCurrentKeyFrame->GetMap()->SetIniertialBA1(); - if (mbMonocular) - InitializeIMU(1.f, 1e5, true); - else - InitializeIMU(1.f, 1e5, true); - - cout << "end VIBA 1" << endl; - } - } - else if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA2()){ - if (mTinit>15.0f){ - cout << "start VIBA 2" << endl; - mpCurrentKeyFrame->GetMap()->SetIniertialBA2(); - if (mbMonocular) - InitializeIMU(0.f, 0.f, true); - else - InitializeIMU(0.f, 0.f, true); - - cout << "end VIBA 2" << endl; - } - } - - // scale refinement - if (((mpAtlas->KeyFramesInMap())<=200) && - ((mTinit>25.0f && mTinit<25.5f)|| - (mTinit>35.0f && mTinit<35.5f)|| - (mTinit>45.0f && mTinit<45.5f)|| - (mTinit>55.0f && mTinit<55.5f)|| - (mTinit>65.0f && mTinit<65.5f)|| - (mTinit>75.0f && mTinit<75.5f))){ - if (mbMonocular) - ScaleRefinement(); - } - } - } + // TODO: Need to verify if removing these lines could affect the algorithm + + // if ((mTinit<50.0f) && mbInertial) + // { + // if(mpCurrentKeyFrame->GetMap()->isImuInitialized() && mpTracker->mState==Tracking::OK) // Enter here everytime local-mapping is called + // { + // if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA1()){ + // if (mTinit>5.0f) + // { + // cout << "start VIBA 1" << endl; + // mpCurrentKeyFrame->GetMap()->SetIniertialBA1(); + // if (mbMonocular) + // InitializeIMU(1.f, 1e5, true); + // else + // InitializeIMU(1.f, 1e5, true); + + // cout << "end VIBA 1" << endl; + // } + // } + // else if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA2()){ + // if (mTinit>15.0f){ + // cout << "start VIBA 2" << endl; + // mpCurrentKeyFrame->GetMap()->SetIniertialBA2(); + // if (mbMonocular) + // InitializeIMU(0.f, 0.f, true); + // else + // InitializeIMU(0.f, 0.f, true); + + // cout << "end VIBA 2" << endl; + // } + // } + + // // scale refinement + // if (((mpAtlas->KeyFramesInMap())<=200) && + // ((mTinit>25.0f && mTinit<25.5f)|| + // (mTinit>35.0f && mTinit<35.5f)|| + // (mTinit>45.0f && mTinit<45.5f)|| + // (mTinit>55.0f && mTinit<55.5f)|| + // (mTinit>65.0f && mTinit<65.5f)|| + // (mTinit>75.0f && mTinit<75.5f))){ + // if (mbMonocular) + // ScaleRefinement(); + // } + // } + // } } #ifdef REGISTER_TIMES diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc index 319afb551b4..f127613233b 100644 --- a/src/LoopClosing.cc +++ b/src/LoopClosing.cc @@ -982,7 +982,7 @@ void LoopClosing::CorrectLoop() unique_lock lock(mMutexGBA); mbStopGBA = true; - mnFullBAIdx++; + mnFullBAIdx = true; if(mpThreadGBA) { @@ -1233,7 +1233,7 @@ void LoopClosing::MergeLocal() unique_lock lock(mMutexGBA); mbStopGBA = true; - mnFullBAIdx++; + mnFullBAIdx = true; if(mpThreadGBA) { @@ -1806,7 +1806,7 @@ void LoopClosing::MergeLocal2() unique_lock lock(mMutexGBA); mbStopGBA = true; - mnFullBAIdx++; + mnFullBAIdx = true; if(mpThreadGBA) { diff --git a/src/ORBmatcher.cc b/src/ORBmatcher.cc index 9129683e4e4..e26c678241c 100644 --- a/src/ORBmatcher.cc +++ b/src/ORBmatcher.cc @@ -25,7 +25,7 @@ #include "Thirdparty/DBoW2/DBoW2/FeatureVector.h" -#include +#include using namespace std; diff --git a/src/Optimizer.cc b/src/Optimizer.cc index b785be38299..74b5a272ebb 100644 --- a/src/Optimizer.cc +++ b/src/Optimizer.cc @@ -1014,6 +1014,9 @@ int Optimizer::PoseOptimization(Frame *pFrame) nBad=0; for(size_t i=0, iend=vpEdgesMono.size(); i +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include "ImuTypes.h" +#include "Optimizer.h" +#include "Tracking.h" + +#include "../common/plugin.hpp" +#include "../common/phonebook.hpp" +#include "../common/switchboard.hpp" +#include "../common/data_format.hpp" +#include "../common/relative_clock.hpp" + +#define STEREO_IMU + +using namespace ILLIXR; + +class orb_slam3 : public plugin { +public: + orb_slam3(std::string name_, phonebook* pb_) + : plugin{name_, pb_} + , sb{pb->lookup_impl()} + , _m_pose{sb->get_writer("slow_pose")} + , root_path{getenv("ORB_SLAM_ROOT")} + , vocab_path{root_path / "Vocabulary" / "ORBvoc.txt"} + , _m_imu_integrator_input{sb->get_writer("imu_integrator_input")} + , _m_cam{sb->get_buffered_reader("cam")} + , cam_buffer{nullptr} + { + + // set initial slow pose + _m_pose.put(_m_pose.allocate( + time_point{}, + Eigen::Vector3f{0, 0, 0}, + Eigen::Quaternionf{1, 0, 0, 0} + )); + +#ifdef CV_HAS_METRICS + cv::metrics::setAccount(new std::string{"-1"}); +#endif + + // set setting path and initialize ORB_SLAM3 +#ifdef STEREO_IMU + setting_path = root_path / "Examples" / "Stereo-Inertial" / "EuRoC.yaml"; + SLAM = std::make_unique(vocab_path.string(), setting_path.string(), ORB_SLAM3::System::IMU_STEREO, false); + + +#else + setting_path = root_path / "Examples" / "RGB-D" / "ETH3D.yaml"; + SLAM = std::make_unique(vocab_path.string(), setting_path.string(), ORB_SLAM3::System::RGBD, false); +#endif + } + + virtual void start() override { + plugin::start(); +#ifdef STEREO_IMU + sb->schedule(id, "imu", [&](switchboard::ptr datum, std::size_t iteration_no) { + this->feed_imu_cam(datum, iteration_no); + }); +#else + sb->schedule(id, "rgb_depth", [&](switchboard::ptr datum, std::size_t iteration_no) { + this->feed_rgbd(datum, iteration_no); + }); +#endif + } + + void feed_imu_cam(switchboard::ptr datum, std::size_t iteration_no){ + // Ensures that slam doesnt start before valid IMU readings come in + if (datum == NULL) { + return; + } + + // Get current IMU data + cv::Point3f acc(datum->linear_a.x(), datum->linear_a.y(), datum->linear_a.z()); + cv::Point3f gyro(datum->angular_v.x(), datum->angular_v.y(), datum->angular_v.z()); + ORB_SLAM3::IMU::Point input_imu(acc.x, acc.y, acc.z, gyro.x, gyro.y, gyro.z, duration2double(datum->time.time_since_epoch())); + current_input.push_back(input_imu); + + if (cam_buffer) { + if (cam_buffer->time <= datum->time) { + cam = cam_buffer; + cam_buffer = nullptr; + } else { + return; + } + } else { + cam = _m_cam.size() == 0 ? nullptr : _m_cam.dequeue(); + // If there is not cam data this func call, break early + if (!cam) { + return; + } + if (cam->time > datum->time) { + cam_buffer = cam; + return; + } + } + + // If there is cam data, load IMU data from the last cam data up until now + prev_input.clear(); + for (int i = 0; i < current_input.size(); i++){ + ORB_SLAM3::IMU::Point imu_point = current_input[i]; + prev_input.push_back(imu_point); + } + current_input.clear(); + +#ifdef CV_HAS_METRICS + cv::metrics::setAccount(new std::string{std::to_string(iteration_no)}); + if (iteration_no % 20 == 0) { + cv::metrics::dump(); + } +#else +#warning "No OpenCV metrics available. Please recompile OpenCV from git clone --branch 3.4.6-instrumented https://github.com/ILLIXR/opencv/. (see install_deps.sh)" +#endif + + cv::Mat cam0{cam->img0}; + cv::Mat cam1{cam->img1}; + + auto start = std::chrono::steady_clock::now(); + Sophus::SE3f mat_pose = SLAM->TrackStereo(cam0, cam1, duration2double(cam->time.time_since_epoch()), prev_input).inverse(); + auto end = std::chrono::steady_clock::now(); + +#ifndef NDEBUG + // print duration to compute pose + double duration = std::chrono::duration(end-start).count(); + total_runtime += duration; + min_runtime = std::min(min_runtime, duration); + max_runtime = std::max(max_runtime, duration); + num_runtime++; + printf("timestamp: %f current: %f min: %f max: %f average: %f", duration2double(cam->time.time_since_epoch()), duration, + min_runtime, max_runtime, total_runtime / num_runtime); +#endif + + slam_tracker = SLAM->mpTracker; + if (slam_tracker->mState != ORB_SLAM3::Tracking::eTrackingState::OK && slam_tracker->mState != ORB_SLAM3::Tracking::eTrackingState::OK_KLT) { + return; + } + output_frame = slam_tracker->mCurrentFrame; + + // get translation matrix + Eigen::Vector3f trans = mat_pose.translation(); + Eigen::Vector3d posd = Eigen::Vector3d{double(trans.x()), double(trans.y()), double(trans.z())}; + + //get rotation matrix + Eigen::Quaternionf quat = mat_pose.unit_quaternion(); + Eigen::Quaterniond rotd = Eigen::Quaterniond{double(quat.w()),double(quat.x()),double(quat.y()),double(quat.z())}; + + // get velocity vector + Eigen::Vector3f velf = output_frame.GetVelocity(); + Eigen::Vector3d vel = Eigen::Vector3d{double(velf.x()), double(velf.y()), double(velf.z())}; + std::cout << "VEL: " << velf.x() << " " << velf.y() << " " << velf.z() << std::endl; + + // get bias + ORB_SLAM3::IMU::Bias imu_bias = output_frame.mImuBias; + Eigen::Vector3d gyro_bias(double(imu_bias.bwx), double(imu_bias.bwy), double(imu_bias.bwz)); + Eigen::Vector3d acc_bias(double(imu_bias.bax), double(imu_bias.bay), double(imu_bias.baz)); + Eigen::Vector3d zeroVector(0,0,0); + + // break early if there is no bias + if (gyro_bias == zeroVector && acc_bias == zeroVector) { + return; + } + + assert(isfinite(posf[0])); + assert(isfinite(posf[1])); + assert(isfinite(posf[2])); + assert(isfinite(rotf.w())); + assert(isfinite(rotf.x())); + assert(isfinite(rotf.y())); + assert(isfinite(rotf.z())); + + _m_pose.put(_m_pose.allocate( + cam->time, + trans, + quat + )); + + _m_imu_integrator_input.put(_m_imu_integrator_input.allocate( + cam->time, + ILLIXR::duration{0L}, + imu_params{ + SLAM->settings_->noiseGyro(), + SLAM->settings_->noiseAcc(), + SLAM->settings_->gyroWalk(), + SLAM->settings_->accWalk(), + .n_gravity = Eigen::Matrix(0.0, 0.0, -9.81), + .imu_integration_sigma = 1.0, + SLAM->settings_->imuFrequency() + }, + acc_bias, + gyro_bias, + posd, + vel, + rotd + )); + // clear imu vector if there are images + prev_input.clear(); + + } + +#ifndef STEREO_IMU + void feed_rgbd(switchboard::ptr datum, std::size_t iteration_no){ + // Ensures that slam doesnt start before valid IMU readings come in + if (datum == NULL) { + return; + } + +#ifdef CV_HAS_METRICS + cv::metrics::setAccount(new std::string{std::to_string(iteration_no)}); + if (iteration_no % 20 == 0) { + cv::metrics::dump(); + } +#else +#warning "No OpenCV metrics available. Please recompile OpenCV from git clone --branch 3.4.6-instrumented https://github.com/ILLIXR/opencv/. (see install_deps.sh)" +#endif + + // get and clone the images + cv::Mat img{datum->rgb}; + cv::Mat depth{datum->depth}; + cv::Mat input_cam = img.clone(); + cv::Mat input_depth = depth.clone(); + + auto start = std::chrono::steady_clock::now(); + Sophus::SE3f mat_pose = SLAM->TrackRGBD(input_cam,input_depth,duration2double(datum->time.time_since_epoch())).inverse(); + auto end = std::chrono::steady_clock::now(); + +#ifndef NDEBUG + // print duration to compute pose + double duration = std::chrono::duration(end-start).count(); + total_runtime += duration; + min_runtime = std::min(min_runtime, duration); + max_runtime = std::max(max_runtime, duration); + num_runtime++; + printf("timestamp: %f current: %f min: %f max: %f average: %f", duration2double(datum->time.time_since_epoch()), duration, + min_runtime, max_runtime, total_runtime / num_runtime); +#endif + + slam_tracker = SLAM->mpTracker; + if (slam_tracker->mState != ORB_SLAM3::Tracking::eTrackingState::OK) { + return; + } + output_frame = slam_tracker->mCurrentFrame; + + // get translation matrix + Eigen::Vector3f trans = mat_pose.translation(); + Eigen::Vector3d posd = Eigen::Vector3d{double(trans.x()), double(trans.y()), double(trans.z())}; + + //get rotation matrix + Eigen::Quaternionf quat = mat_pose.unit_quaternion(); + Eigen::Quaterniond rotd = Eigen::Quaterniond{double(quat.w()),double(quat.x()),double(quat.y()),double(quat.z())}; + + // get velocity vector + Eigen::Vector3f velf = output_frame.GetVelocity(); + Eigen::Vector3d vel = Eigen::Vector3d{double(velf.x()), double(velf.y()), double(velf.z())}; + + assert(isfinite(posf[0])); + assert(isfinite(posf[1])); + assert(isfinite(posf[2])); + assert(isfinite(rotf.w())); + assert(isfinite(rotf.x())); + assert(isfinite(rotf.y())); + assert(isfinite(rotf.z())); + + _m_pose.put(_m_pose.allocate( + datum->time, + trans, + quat + )); + + // clear imu vector if there are images + prev_input.clear(); + + } +#endif + + virtual ~orb_slam3() override { + SLAM->Shutdown(); + } + +private: + const std::shared_ptr sb; + switchboard::writer _m_pose; + std::shared_ptr _m_rtc; + switchboard::buffered_reader _m_cam; + switchboard::ptr cam; + switchboard::ptr cam_buffer; + switchboard::writer _m_imu_integrator_input; + int cam_count; + + double min_runtime = 1000000000; + double max_runtime = -10; + double total_runtime = 0; + int num_runtime = 0; + + std::unique_ptr SLAM; + ORB_SLAM3::Tracking * slam_tracker; + ORB_SLAM3::Frame output_frame; + + vector current_input; + vector prev_input; + boost::filesystem::path root_path; + boost::filesystem::path vocab_path; + boost::filesystem::path setting_path; +}; + +PLUGIN_MAIN(orb_slam3);