diff --git a/.gitignore b/.gitignore index 7227515c0a..73f4da9bca 100644 --- a/.gitignore +++ b/.gitignore @@ -71,6 +71,7 @@ lib/ cmake_modules/ cmake-build-debug/ +cmake-build-release/ ExecMean.txt SessionInfo.txt diff --git a/CMakeLists.txt b/CMakeLists.txt index 016e74354d..47978de46c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8) project(ORB_SLAM3) IF(NOT CMAKE_BUILD_TYPE) - SET(CMAKE_BUILD_TYPE Release) + SET(CMAKE_BUILD_TYPE Release) ENDIF() MESSAGE("Build type: " ${CMAKE_BUILD_TYPE}) @@ -12,28 +12,30 @@ 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 -include(CheckCXXCompilerFlag) -CHECK_CXX_COMPILER_FLAG("-std=c++11" 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") - add_definitions(-DCOMPILEDWITHC11) - message(STATUS "Using flag -std=c++11.") -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.") -endif() +set(CMAKE_CXX_STANDARD 14) +add_definitions(-DCOMPILEDWITHC11) +## Check C++11 or C++0x support +#include(CheckCXXCompilerFlag) +#CHECK_CXX_COMPILER_FLAG("-std=c++11" 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") +# add_definitions(-DCOMPILEDWITHC11) +# message(STATUS "Using flag -std=c++11.") +#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.") +#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() +if(NOT OpenCV_FOUND) + message(FATAL_ERROR "OpenCV > 4.4 not found.") +endif() MESSAGE("OPENCV VERSION:") MESSAGE(${OpenCV_VERSION}) @@ -43,12 +45,12 @@ find_package(Pangolin REQUIRED) find_package(realsense2) include_directories( -${PROJECT_SOURCE_DIR} -${PROJECT_SOURCE_DIR}/include -${PROJECT_SOURCE_DIR}/include/CameraModels -${PROJECT_SOURCE_DIR}/Thirdparty/Sophus -${EIGEN3_INCLUDE_DIR} -${Pangolin_INCLUDE_DIRS} + ${PROJECT_SOURCE_DIR} + ${PROJECT_SOURCE_DIR}/include + ${PROJECT_SOURCE_DIR}/include/CameraModels + ${PROJECT_SOURCE_DIR}/Thirdparty/Sophus + ${EIGEN3_INCLUDE_DIR} + ${Pangolin_INCLUDE_DIRS} ) set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) @@ -125,41 +127,41 @@ ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so -lcrypto ) -# If RealSense SDK is found the library is added and its examples compiled -if(realsense2_FOUND) - include_directories(${PROJECT_NAME} - ${realsense_INCLUDE_DIR} - ) - target_link_libraries(${PROJECT_NAME} - ${realsense2_LIBRARY} - ) -endif() +## If RealSense SDK is found the library is added and its examples compiled +#if(realsense2_FOUND) +# include_directories(${PROJECT_NAME} +# ${realsense_INCLUDE_DIR} +# ) +# target_link_libraries(${PROJECT_NAME} +# ${realsense2_LIBRARY} +# ) +#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() +## 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) @@ -176,15 +178,15 @@ 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() +#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) @@ -266,125 +268,4 @@ if(realsense2_FOUND) 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() +endif() \ No newline at end of file diff --git a/Examples/Stereo/KITTI00-02.yaml b/Examples/Stereo/KITTI00-02.yaml index 64d5141791..d523a7f7b4 100644 --- a/Examples/Stereo/KITTI00-02.yaml +++ b/Examples/Stereo/KITTI00-02.yaml @@ -57,7 +57,7 @@ Viewer.PointSize: 2.0 Viewer.CameraSize: 0.7 Viewer.CameraLineWidth: 3.0 Viewer.ViewpointX: 0.0 -Viewer.ViewpointY: -100 +Viewer.ViewpointY: -100.0 Viewer.ViewpointZ: -0.1 Viewer.ViewpointF: 2000.0 diff --git a/Examples/Stereo/stereo_kitti.cc b/Examples/Stereo/stereo_kitti.cc index fdc8ef0415..db0041a016 100644 --- a/Examples/Stereo/stereo_kitti.cc +++ b/Examples/Stereo/stereo_kitti.cc @@ -177,8 +177,8 @@ void LoadImages(const string &strPathToSequence, vector &vstrImageLeft, } } - string strPrefixLeft = strPathToSequence + "/image_0/"; - string strPrefixRight = strPathToSequence + "/image_1/"; + string strPrefixLeft = strPathToSequence + "/image_2/"; + string strPrefixRight = strPathToSequence + "/image_3/"; const int nTimes = vTimestamps.size(); vstrImageLeft.resize(nTimes); diff --git a/README.md b/README.md index 0ca3a9f2d3..06015008aa 100644 --- a/README.md +++ b/README.md @@ -1,235 +1,54 @@ -# ORB-SLAM3 +# Some bugs in ORB-SLAM3 +## Changed Codes +#### SearchForTriangulation +https://github.com/fishmarch/ORB_SLAM3_Fixed/blob/b66d0b7eedfb363e5bad0cee8ea12065f2d66ae8/src/ORBmatcher.cc#L1085 -### V1.0, December 22th, 2021 -**Authors:** Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, [José M. M. Montiel](http://webdiis.unizar.es/~josemari/), [Juan D. Tardos](http://webdiis.unizar.es/~jdtardos/). +Forgot to change flags to label matched features. -The [Changelog](https://github.com/UZ-SLAMLab/ORB_SLAM3/blob/master/Changelog.md) describes the features of each version. +This would cause multiple keypoints in KF1 are matched with same keypoint in KF2; and then all these new map points contain obervations KF1, but KF1 only records one of them. -ORB-SLAM3 is the first real-time SLAM library able to perform **Visual, Visual-Inertial and Multi-Map SLAM** with **monocular, stereo and RGB-D** cameras, using **pin-hole and fisheye** lens models. In all sensor configurations, ORB-SLAM3 is as robust as the best systems available in the literature, and significantly more accurate. +#### UpdateConnections +https://github.com/fishmarch/ORB_SLAM3_Fixed/blob/b66d0b7eedfb363e5bad0cee8ea12065f2d66ae8/src/KeyFrame.cc#L427-L443 -We provide examples to run ORB-SLAM3 in the [EuRoC dataset](http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) using stereo or monocular, with or without IMU, and in the [TUM-VI dataset](https://vision.in.tum.de/data/datasets/visual-inertial-dataset) using fisheye stereo or monocular, with or without IMU. Videos of some example executions can be found at [ORB-SLAM3 channel](https://www.youtube.com/channel/UCXVt-kXG6T95Z4tVaYlU80Q). +Here caused unidirectional connection. -This software is based on [ORB-SLAM2](https://github.com/raulmur/ORB_SLAM2) developed by [Raul Mur-Artal](http://webdiis.unizar.es/~raulmur/), [Juan D. Tardos](http://webdiis.unizar.es/~jdtardos/), [J. M. M. Montiel](http://webdiis.unizar.es/~josemari/) and [Dorian Galvez-Lopez](http://doriangalvez.com/) ([DBoW2](https://github.com/dorian3d/DBoW2)). +When updating connection of KF, all other keyframes sharing co-observed mappoints are collected. Only when the weights (number of co-obervations) are larger than a threshold (15), the connections are recorded bidirectionally; otherwise, the records should be erased. - +#### Sim3Solver +https://github.com/fishmarch/ORB_SLAM3_Fixed/blob/b66d0b7eedfb363e5bad0cee8ea12065f2d66ae8/src/Sim3Solver.cc#L40-L43 -### Related Publications: +The flag was set wrong here. -[ORB-SLAM3] Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M. M. Montiel and Juan D. Tardós, **ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM**, *IEEE Transactions on Robotics 37(6):1874-1890, Dec. 2021*. **[PDF](https://arxiv.org/abs/2007.11898)**. +This would cause the index cannot be found properly later, and then less matched map points are collected. -[IMU-Initialization] Carlos Campos, J. M. M. Montiel and Juan D. Tardós, **Inertial-Only Optimization for Visual-Inertial Initialization**, *ICRA 2020*. **[PDF](https://arxiv.org/pdf/2003.05766.pdf)** +#### DetectNBestCandidates +https://github.com/fishmarch/ORB_SLAM3_Fixed/blob/b66d0b7eedfb363e5bad0cee8ea12065f2d66ae8/src/KeyFrameDatabase.cc#L710-L726 -[ORBSLAM-Atlas] Richard Elvira, J. M. M. Montiel and Juan D. Tardós, **ORBSLAM-Atlas: a robust and accurate multi-map system**, *IROS 2019*. **[PDF](https://arxiv.org/pdf/1908.11585.pdf)**. +The iterator was not updated when the keyframe is bad. -[ORBSLAM-VI] Raúl Mur-Artal, and Juan D. Tardós, **Visual-inertial monocular SLAM with map reuse**, IEEE Robotics and Automation Letters, vol. 2 no. 2, pp. 796-803, 2017. **[PDF](https://arxiv.org/pdf/1610.05949.pdf)**. +https://github.com/fishmarch/ORB_SLAM3_Fixed/blob/b66d0b7eedfb363e5bad0cee8ea12065f2d66ae8/src/KeyFrameDatabase.cc#L658-L664 -[Stereo and RGB-D] Raúl Mur-Artal and Juan D. Tardós. **ORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras**. *IEEE Transactions on Robotics,* vol. 33, no. 5, pp. 1255-1262, 2017. **[PDF](https://arxiv.org/pdf/1610.06475.pdf)**. +Only computed scores for keyframes in which the number of common words is larger than a threshold, but the scores of other keyframes may also be used later. -[Monocular] Raúl Mur-Artal, José M. M. Montiel and Juan D. Tardós. **ORB-SLAM: A Versatile and Accurate Monocular SLAM System**. *IEEE Transactions on Robotics,* vol. 31, no. 5, pp. 1147-1163, 2015. (**2015 IEEE Transactions on Robotics Best Paper Award**). **[PDF](https://arxiv.org/pdf/1502.00956.pdf)**. +**Update:** But this would make this part very slow. Thus the codes are changed as following. -[DBoW2 Place Recognition] Dorian Gálvez-López and Juan D. Tardós. **Bags of Binary Words for Fast Place Recognition in Image Sequences**. *IEEE Transactions on Robotics,* vol. 28, no. 5, pp. 1188-1197, 2012. **[PDF](http://doriangalvez.com/php/dl.php?dlp=GalvezTRO12.pdf)** +https://github.com/fishmarch/ORB_SLAM3_Fixed/blob/35c2361f82f32b320150a254e1627bcedc455df6/src/KeyFrameDatabase.cc#L687-L691 -# 1. License +#### SearchByProjection -ORB-SLAM3 is released under [GPLv3 license](https://github.com/UZ-SLAMLab/ORB_SLAM3/LICENSE). For a list of all code/library dependencies (and associated licenses), please see [Dependencies.md](https://github.com/UZ-SLAMLab/ORB_SLAM3/blob/master/Dependencies.md). +https://github.com/fishmarch/ORB_SLAM3_Fixed/blob/5a0b298d72f54657e11b5fbd71d39dfe14612482/src/ORBmatcher.cc#L53-L54 -For a closed-source version of ORB-SLAM3 for commercial purposes, please contact the authors: orbslam (at) unizar (dot) es. +These map points should matched already, and some matching parameters may be not computed. -If you use ORB-SLAM3 in an academic work, please cite: - - @article{ORBSLAM3_TRO, - title={{ORB-SLAM3}: An Accurate Open-Source Library for Visual, Visual-Inertial - and Multi-Map {SLAM}}, - author={Campos, Carlos AND Elvira, Richard AND G\´omez, Juan J. AND Montiel, - Jos\'e M. M. AND Tard\'os, Juan D.}, - journal={IEEE Transactions on Robotics}, - volume={37}, - number={6}, - pages={1874-1890}, - year={2021} - } +#### PreSave -# 2. Prerequisites -We have tested the library in **Ubuntu 16.04** and **18.04**, but it should be easy to compile in other platforms. A powerful computer (e.g. i7) will ensure real-time performance and provide more stable and accurate results. +https://github.com/fishmarch/ORB_SLAM3_Fixed/blob/beb05baccd018cbaf63d5d429f952250def7f67b/src/Map.cc#L394-L403 +The original codes may invalidate the iterator because of deleting bad map points. -## C++11 or C++0x Compiler -We use the new thread and chrono functionalities of C++11. -## Pangolin -We use [Pangolin](https://github.com/stevenlovegrove/Pangolin) for visualization and user interface. Dowload and install instructions can be found at: https://github.com/stevenlovegrove/Pangolin. -## OpenCV -We use [OpenCV](http://opencv.org) to manipulate images and features. Dowload and install instructions can be found at: http://opencv.org. **Required at leat 3.0. Tested with OpenCV 3.2.0 and 4.4.0**. -## Eigen3 -Required by g2o (see below). Download and install instructions can be found at: http://eigen.tuxfamily.org. **Required at least 3.1.0**. -## DBoW2 and g2o (Included in Thirdparty folder) -We use modified versions of the [DBoW2](https://github.com/dorian3d/DBoW2) library to perform place recognition and [g2o](https://github.com/RainerKuemmerle/g2o) library to perform non-linear optimizations. Both modified libraries (which are BSD) are included in the *Thirdparty* folder. -## Python -Required to calculate the alignment of the trajectory with the ground truth. **Required Numpy module**. -* (win) http://www.python.org/downloads/windows -* (deb) `sudo apt install libpython2.7-dev` -* (mac) preinstalled with osx -## ROS (optional) - -We provide some examples to process input of a monocular, monocular-inertial, stereo, stereo-inertial or RGB-D camera using ROS. Building these examples is optional. These have been tested with ROS Melodic under Ubuntu 18.04. - -# 3. Building ORB-SLAM3 library and examples - -Clone the repository: -``` -git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git ORB_SLAM3 -``` - -We provide a script `build.sh` to build the *Thirdparty* libraries and *ORB-SLAM3*. Please make sure you have installed all required dependencies (see section 2). Execute: -``` -cd ORB_SLAM3 -chmod +x build.sh -./build.sh -``` - -This will create **libORB_SLAM3.so** at *lib* folder and the executables in *Examples* folder. - -# 4. Running ORB-SLAM3 with your camera - -Directory `Examples` contains several demo programs and calibration files to run ORB-SLAM3 in all sensor configurations with Intel Realsense cameras T265 and D435i. The steps needed to use your own camera are: - -1. Calibrate your camera following `Calibration_Tutorial.pdf` and write your calibration file `your_camera.yaml` - -2. Modify one of the provided demos to suit your specific camera model, and build it - -3. Connect the camera to your computer using USB3 or the appropriate interface - -4. Run ORB-SLAM3. For example, for our D435i camera, we would execute: - -``` -./Examples/Stereo-Inertial/stereo_inertial_realsense_D435i Vocabulary/ORBvoc.txt ./Examples/Stereo-Inertial/RealSense_D435i.yaml -``` - -# 5. EuRoC Examples -[EuRoC dataset](http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) was recorded with two pinhole cameras and an inertial sensor. We provide an example script to launch EuRoC sequences in all the sensor configurations. - -1. Download a sequence (ASL format) from http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets - -2. Open the script "euroc_examples.sh" in the root of the project. Change **pathDatasetEuroc** variable to point to the directory where the dataset has been uncompressed. - -3. Execute the following script to process all the sequences with all sensor configurations: -``` -./euroc_examples -``` - -## Evaluation -EuRoC provides ground truth for each sequence in the IMU body reference. As pure visual executions report trajectories centered in the left camera, we provide in the "evaluation" folder the transformation of the ground truth to the left camera reference. Visual-inertial trajectories use the ground truth from the dataset. - -Execute the following script to process sequences and compute the RMS ATE: -``` -./euroc_eval_examples -``` - -# 6. TUM-VI Examples -[TUM-VI dataset](https://vision.in.tum.de/data/datasets/visual-inertial-dataset) was recorded with two fisheye cameras and an inertial sensor. - -1. Download a sequence from https://vision.in.tum.de/data/datasets/visual-inertial-dataset and uncompress it. - -2. Open the script "tum_vi_examples.sh" in the root of the project. Change **pathDatasetTUM_VI** variable to point to the directory where the dataset has been uncompressed. - -3. Execute the following script to process all the sequences with all sensor configurations: -``` -./tum_vi_examples -``` - -## Evaluation -In TUM-VI ground truth is only available in the room where all sequences start and end. As a result the error measures the drift at the end of the sequence. - -Execute the following script to process sequences and compute the RMS ATE: -``` -./tum_vi_eval_examples -``` - -# 7. ROS Examples - -### Building the nodes for mono, mono-inertial, stereo, stereo-inertial and RGB-D -Tested with ROS Melodic and ubuntu 18.04. - -1. Add the path including *Examples/ROS/ORB_SLAM3* to the ROS_PACKAGE_PATH environment variable. Open .bashrc file: - ``` - gedit ~/.bashrc - ``` -and add at the end the following line. Replace PATH by the folder where you cloned ORB_SLAM3: - - ``` - export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM3/Examples/ROS - ``` - -2. Execute `build_ros.sh` script: - - ``` - chmod +x build_ros.sh - ./build_ros.sh - ``` - -### Running Monocular Node -For a monocular input from topic `/camera/image_raw` run node ORB_SLAM3/Mono. You will need to provide the vocabulary file and a settings file. See the monocular examples above. - - ``` - rosrun ORB_SLAM3 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE - ``` - -### Running Monocular-Inertial Node -For a monocular input from topic `/camera/image_raw` and an inertial input from topic `/imu`, run node ORB_SLAM3/Mono_Inertial. Setting the optional third argument to true will apply CLAHE equalization to images (Mainly for TUM-VI dataset). - - ``` - rosrun ORB_SLAM3 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE [EQUALIZATION] - ``` - -### Running Stereo Node -For a stereo input from topic `/camera/left/image_raw` and `/camera/right/image_raw` run node ORB_SLAM3/Stereo. You will need to provide the vocabulary file and a settings file. For Pinhole camera model, if you **provide rectification matrices** (see Examples/Stereo/EuRoC.yaml example), the node will recitify the images online, **otherwise images must be pre-rectified**. For FishEye camera model, rectification is not required since system works with original images: - - ``` - rosrun ORB_SLAM3 Stereo PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION - ``` - -### Running Stereo-Inertial Node -For a stereo input from topics `/camera/left/image_raw` and `/camera/right/image_raw`, and an inertial input from topic `/imu`, run node ORB_SLAM3/Stereo_Inertial. You will need to provide the vocabulary file and a settings file, including rectification matrices if required in a similar way to Stereo case: - - ``` - rosrun ORB_SLAM3 Stereo_Inertial PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION [EQUALIZATION] - ``` - -### Running RGB_D Node -For an RGB-D input from topics `/camera/rgb/image_raw` and `/camera/depth_registered/image_raw`, run node ORB_SLAM3/RGBD. You will need to provide the vocabulary file and a settings file. See the RGB-D example above. - - ``` - rosrun ORB_SLAM3 RGBD PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE - ``` - -**Running ROS example:** Download a rosbag (e.g. V1_02_medium.bag) from the EuRoC dataset (http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets). Open 3 tabs on the terminal and run the following command at each tab for a Stereo-Inertial configuration: - ``` - roscore - ``` - - ``` - rosrun ORB_SLAM3 Stereo_Inertial Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/EuRoC.yaml true - ``` - - ``` - rosbag play --pause V1_02_medium.bag /cam0/image_raw:=/camera/left/image_raw /cam1/image_raw:=/camera/right/image_raw /imu0:=/imu - ``` - -Once ORB-SLAM3 has loaded the vocabulary, press space in the rosbag tab. - -**Remark:** For rosbags from TUM-VI dataset, some play issue may appear due to chunk size. One possible solution is to rebag them with the default chunk size, for example: - ``` - rosrun rosbag fastrebag.py dataset-room1_512_16.bag dataset-room1_512_16_small_chunks.bag - ``` - -# 8. Running time analysis -A flag in `include\Config.h` activates time measurements. It is necessary to uncomment the line `#define REGISTER_TIMES` to obtain the time stats of one execution which is shown at the terminal and stored in a text file(`ExecTimeMean.txt`). - -# 9. Calibration -You can find a tutorial for visual-inertial calibration and a detailed description of the contents of valid configuration files at `Calibration_Tutorial.pdf` diff --git a/Thirdparty/DBoW2/CMakeLists.txt b/Thirdparty/DBoW2/CMakeLists.txt index c312b255a8..3f1170c5a3 100644 --- a/Thirdparty/DBoW2/CMakeLists.txt +++ b/Thirdparty/DBoW2/CMakeLists.txt @@ -7,6 +7,8 @@ endif() set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") +set(CMAKE_CXX_STANDARD 14) +add_definitions(-DCOMPILEDWITHC11) set(HDRS_DBOW2 DBoW2/BowVector.h diff --git a/Thirdparty/Sophus/CMakeLists.txt b/Thirdparty/Sophus/CMakeLists.txt index 933445b49b..15b171bcfb 100644 --- a/Thirdparty/Sophus/CMakeLists.txt +++ b/Thirdparty/Sophus/CMakeLists.txt @@ -27,6 +27,8 @@ ELSEIF("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") ELSEIF(CMAKE_CXX_COMPILER_ID MATCHES "^MSVC$") ADD_DEFINITIONS("-D _USE_MATH_DEFINES /bigobj /wd4305 /wd4244 /MP") ENDIF() +set(CMAKE_CXX_STANDARD 14) +add_definitions(-DCOMPILEDWITHC11) # Add local path for finding packages, set the local version first list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules") diff --git a/Thirdparty/g2o/CMakeLists.txt b/Thirdparty/g2o/CMakeLists.txt index 1a32ff9a2b..987a4a31b4 100644 --- a/Thirdparty/g2o/CMakeLists.txt +++ b/Thirdparty/g2o/CMakeLists.txt @@ -58,6 +58,8 @@ SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -march=native") SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -O3 -march=native") # SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3") # SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -O3") +set(CMAKE_CXX_STANDARD 14) +add_definitions(-DCOMPILEDWITHC11) # activate warnings !!! SET(g2o_C_FLAGS "${g2o_C_FLAGS} -Wall -W") diff --git a/build.sh b/build.sh index 96d1c0941c..7198d9dbee 100755 --- a/build.sh +++ b/build.sh @@ -32,9 +32,9 @@ 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 +#echo "Configuring and building ORB_SLAM3 ..." +# +#mkdir build +#cd build +#cmake .. -DCMAKE_BUILD_TYPE=Release +#make -j4 diff --git a/include/MapPoint.h b/include/MapPoint.h index 29cb4203d0..c6e1a187f0 100644 --- a/include/MapPoint.h +++ b/include/MapPoint.h @@ -157,7 +157,7 @@ class MapPoint void PrintObservations(); - void PreSave(set& spKF,set& spMP); + bool PreSave(set& spKF,set& spMP); void PostLoad(map& mpKFid, map& mpMPid); public: diff --git a/include/Settings.h b/include/Settings.h index beab7de2f7..b40742a9f3 100644 --- a/include/Settings.h +++ b/include/Settings.h @@ -69,7 +69,7 @@ namespace ORB_SLAM3 { GeometricCamera* camera1() {return calibration1_;} GeometricCamera* camera2() {return calibration2_;} cv::Mat camera1DistortionCoef() {return cv::Mat(vPinHoleDistorsion1_.size(),1,CV_32F,vPinHoleDistorsion1_.data());} - cv::Mat camera2DistortionCoef() {return cv::Mat(vPinHoleDistorsion2_.size(),1,CV_32F,vPinHoleDistorsion1_.data());} + cv::Mat camera2DistortionCoef() {return cv::Mat(vPinHoleDistorsion2_.size(),1,CV_32F,vPinHoleDistorsion2_.data());} Sophus::SE3f Tlr() {return Tlr_;} float bf() {return bf_;} diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc index 293ab48182..dfb76918b0 100644 --- a/src/KeyFrame.cc +++ b/src/KeyFrame.cc @@ -424,7 +424,7 @@ void KeyFrame::UpdateConnections(bool upParent) vPairs.reserve(KFcounter.size()); if(!upParent) cout << "UPDATE_CONN: current KF " << mnId << endl; - for(map::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++) + for(map::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend;) { if(!upParent) cout << " UPDATE_CONN: KF " << mit->first->mnId << " ; num matches: " << mit->second << endl; @@ -437,7 +437,9 @@ void KeyFrame::UpdateConnections(bool upParent) { vPairs.push_back(make_pair(mit->second,mit->first)); (mit->first)->AddConnection(this,mit->second); - } + mit++; + }else + mit = KFcounter.erase(mit); } if(vPairs.empty()) diff --git a/src/KeyFrameDatabase.cc b/src/KeyFrameDatabase.cc index 13b4da6115..e190bd54e4 100644 --- a/src/KeyFrameDatabase.cc +++ b/src/KeyFrameDatabase.cc @@ -655,7 +655,6 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector &v for(list::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++) { KeyFrame* pKFi = *lit; - if(pKFi->mnPlaceRecognitionWords>minCommonWords) { nscores++; @@ -685,7 +684,11 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector &v KeyFrame* pKF2 = *vit; if(pKF2->mnPlaceRecognitionQuery!=pKF->mnId) continue; - + if(pKF2->mnPlaceRecognitionWords<=minCommonWords){ + float si = mpVoc->score(pKF->mBowVec,pKF2->mBowVec); + pKF2->mPlaceRecognitionScore=si; + pKF2->mnPlaceRecognitionWords = minCommonWords+1; + } accScore+=pKF2->mPlaceRecognitionScore; if(pKF2->mPlaceRecognitionScore>bestScore) { @@ -709,20 +712,19 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector &v while(i < lAccScoreAndMatch.size() && (vpLoopCand.size() < nNumCandidates || vpMergeCand.size() < nNumCandidates)) { KeyFrame* pKFi = it->second; - if(pKFi->isBad()) - continue; - - if(!spAlreadyAddedKF.count(pKFi)) - { - if(pKF->GetMap() == pKFi->GetMap() && vpLoopCand.size() < nNumCandidates) - { - vpLoopCand.push_back(pKFi); - } - else if(pKF->GetMap() != pKFi->GetMap() && vpMergeCand.size() < nNumCandidates && !pKFi->GetMap()->IsBad()) + if(!pKFi->isBad()){ + if(!spAlreadyAddedKF.count(pKFi)) { - vpMergeCand.push_back(pKFi); + if(pKF->GetMap() == pKFi->GetMap() && vpLoopCand.size() < nNumCandidates) + { + vpLoopCand.push_back(pKFi); + } + else if(pKF->GetMap() != pKFi->GetMap() && vpMergeCand.size() < nNumCandidates && !pKFi->GetMap()->IsBad()) + { + vpMergeCand.push_back(pKFi); + } + spAlreadyAddedKF.insert(pKFi); } - spAlreadyAddedKF.insert(pKFi); } i++; it++; diff --git a/src/Map.cc b/src/Map.cc index f1f8cd041c..8622cfeca8 100644 --- a/src/Map.cc +++ b/src/Map.cc @@ -359,7 +359,8 @@ void Map::SetLastMapChange(int currentChangeId) void Map::PreSave(std::set &spCams) { int nMPWithoutObs = 0; - for(MapPoint* pMPi : mspMapPoints) + vector vMPs = GetAllMapPoints(); + for(MapPoint* pMPi : vMPs) { if(!pMPi || pMPi->isBad()) continue; @@ -390,24 +391,27 @@ void Map::PreSave(std::set &spCams) // Backup of MapPoints mvpBackupMapPoints.clear(); - for(MapPoint* pMPi : mspMapPoints) + vMPs = GetAllMapPoints(); + for(MapPoint* pMPi : vMPs) { if(!pMPi || pMPi->isBad()) continue; + if(pMPi->PreSave(mspKeyFrames,mspMapPoints)) + continue; mvpBackupMapPoints.push_back(pMPi); - pMPi->PreSave(mspKeyFrames,mspMapPoints); } // Backup of KeyFrames mvpBackupKeyFrames.clear(); + std::set spBackupMapPoints(mvpBackupMapPoints.begin(), mvpBackupMapPoints.end()); for(KeyFrame* pKFi : mspKeyFrames) { if(!pKFi || pKFi->isBad()) continue; mvpBackupKeyFrames.push_back(pKFi); - pKFi->PreSave(mspKeyFrames,mspMapPoints, spCams); + pKFi->PreSave(mspKeyFrames,spBackupMapPoints, spCams); } mnBackupKFinitialID = -1; diff --git a/src/MapPoint.cc b/src/MapPoint.cc index 1cf802c233..a3707338ab 100644 --- a/src/MapPoint.cc +++ b/src/MapPoint.cc @@ -569,7 +569,7 @@ void MapPoint::UpdateMap(Map* pMap) mpMap = pMap; } -void MapPoint::PreSave(set& spKF,set& spMP) +bool MapPoint::PreSave(set& spKF,set& spMP) { mBackupReplacedId = -1; if(mpReplaced && spMP.find(mpReplaced) != spMP.end()) @@ -578,7 +578,8 @@ void MapPoint::PreSave(set& spKF,set& spMP) mBackupObservationsId1.clear(); mBackupObservationsId2.clear(); // Save the id and position in each KF who view it - for(std::map >::const_iterator it = mObservations.begin(), end = mObservations.end(); it != end; ++it) + std::map> obs = GetObservations(); + for(std::map >::const_iterator it = obs.begin(), end = obs.end(); it != end; ++it) { KeyFrame* pKFi = it->first; if(spKF.find(pKFi) != spKF.end()) @@ -597,6 +598,7 @@ void MapPoint::PreSave(set& spKF,set& spMP) { mBackupRefKFId = mpRefKF->mnId; } + return mbBad; } void MapPoint::PostLoad(map& mpKFid, map& mpMPid) diff --git a/src/ORBmatcher.cc b/src/ORBmatcher.cc index 9129683e4e..ababb53bad 100644 --- a/src/ORBmatcher.cc +++ b/src/ORBmatcher.cc @@ -49,6 +49,10 @@ namespace ORB_SLAM3 for(size_t iMP=0; iMPmnLastFrameSeen == F.mnId) + continue; + if(!pMP->mbTrackInView && !pMP->mbTrackInViewR) continue; @@ -1082,6 +1086,7 @@ namespace ORB_SLAM3 : (bestIdx2 < pKF2 -> NLeft) ? pKF2 -> mvKeys[bestIdx2] : pKF2 -> mvKeysRight[bestIdx2 - pKF2 -> NLeft]; vMatches12[idx1]=bestIdx2; + vbMatched2[bestIdx2]=true; nmatches++; if(mbCheckOrientation) diff --git a/src/Settings.cc b/src/Settings.cc index 93a50be626..510a3b3232 100644 --- a/src/Settings.cc +++ b/src/Settings.cc @@ -550,7 +550,7 @@ namespace ORB_SLAM3 { output << " ]" << endl; } - if(settings.sensor_ == System::STEREO || settings.sensor_ == System::IMU_STEREO){ + if((settings.sensor_ == System::STEREO || settings.sensor_ == System::IMU_STEREO) && settings.cameraType_ != Settings::Rectified){ output << "\t-Camera 2 parameters ("; if(settings.cameraType_ == Settings::PinHole || settings.cameraType_ == Settings::Rectified){ output << "Pinhole"; diff --git a/src/Sim3Solver.cc b/src/Sim3Solver.cc index b8d3abfb14..b5f47ec99a 100644 --- a/src/Sim3Solver.cc +++ b/src/Sim3Solver.cc @@ -37,10 +37,10 @@ Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector mnIterations(0), mnBestInliers(0), mbFixScale(bFixScale), pCamera1(pKF1->mpCamera), pCamera2(pKF2->mpCamera) { - bool bDifferentKFs = false; + bool bDifferentKFs = true; if(vpKeyFrameMatchedMP.empty()) { - bDifferentKFs = true; + bDifferentKFs = false; vpKeyFrameMatchedMP = vector(vpMatched12.size(), pKF2); }