├── CMakeLists.txt ├── Dependencies.md ├── Examples ├── Monocular │ ├── EuRoC.yaml │ ├── EuRoC_TimeStamps │ │ ├── MH01.txt │ │ ├── MH02.txt │ │ ├── MH03.txt │ │ ├── MH04.txt │ │ ├── MH05.txt │ │ ├── V101.txt │ │ ├── V102.txt │ │ ├── V103.txt │ │ ├── V201.txt │ │ ├── V202.txt │ │ └── V203.txt │ ├── KITTI00-02.yaml │ ├── KITTI03.yaml │ ├── KITTI04-12.yaml │ ├── TUM1.yaml │ ├── TUM2.yaml │ ├── TUM3.yaml │ ├── mono_euroc │ ├── mono_euroc.cc │ ├── mono_kitti │ ├── mono_kitti.cc │ ├── mono_tum │ └── mono_tum.cc ├── RGB-D │ ├── TUM1.yaml │ ├── TUM2.yaml │ ├── TUM3.yaml │ ├── associations │ │ ├── fr1_desk.txt │ │ ├── fr1_desk2.txt │ │ ├── fr1_room.txt │ │ ├── fr1_xyz.txt │ │ ├── fr2_desk.txt │ │ ├── fr2_xyz.txt │ │ ├── fr3_nstr_tex_near.txt │ │ ├── fr3_office.txt │ │ ├── fr3_office_val.txt │ │ ├── fr3_str_tex_far.txt │ │ └── fr3_str_tex_near.txt │ ├── rgbd_tum │ └── rgbd_tum.cc ├── ROS │ └── ORB_SLAM2 │ │ ├── Asus.yaml │ │ ├── CMakeLists.txt │ │ ├── manifest.xml │ │ └── src │ │ ├── AR │ │ ├── ViewerAR.cc │ │ ├── ViewerAR.h │ │ └── ros_mono_ar.cc │ │ ├── ros_mono.cc │ │ ├── ros_rgbd.cc │ │ └── ros_stereo.cc └── Stereo │ ├── EuRoC.yaml │ ├── EuRoC_TimeStamps │ ├── MH01.txt │ ├── MH02.txt │ ├── MH03.txt │ ├── MH04.txt │ ├── MH05.txt │ ├── V101.txt │ ├── V102.txt │ ├── V103.txt │ ├── V201.txt │ ├── V202.txt │ └── V203.txt │ ├── KITTI00-02.yaml │ ├── KITTI03.yaml │ ├── KITTI04-12.yaml │ ├── stereo_euroc │ ├── stereo_euroc.cc │ ├── stereo_kitti │ └── stereo_kitti.cc ├── ExpResults ├── KITTI │ └── groundtruth │ │ ├── 00.txt │ │ ├── 01.txt │ │ ├── 02.txt │ │ ├── 03.txt │ │ ├── 04.txt │ │ ├── 05.txt │ │ ├── 06.txt │ │ ├── 07.txt │ │ ├── 08.txt │ │ ├── 09.txt │ │ └── 10.txt └── TUM │ └── Localization │ ├── associate.py │ ├── associate.pyc │ ├── evaluate_ate.py │ ├── evaluate_rpe.py │ └── plot_trajectory_into_image.py ├── LICENSE.txt ├── License-gpl.txt ├── README.md ├── build.sh ├── example_picutures ├── ObjectMappingKITTI1.gif ├── ObjectMappingTUM_fr2_desk.gif └── notes.txt ├── include ├── Converter.h ├── Frame.h ├── FrameDrawer.h ├── Initializer.h ├── KeyFrame.h ├── KeyFrameDatabase.h ├── LocalMapping.h ├── LoopClosing.h ├── Map.h ├── MapDrawer.h ├── MapPoint.h ├── ORBVocabulary.h ├── ORBextractor.h ├── ORBmatcher.h ├── ObjectMatcher.h ├── ObjectOptimizer.h ├── ObjectTypes.h ├── Optimizer.h ├── PnPsolver.h ├── Semantic.h ├── Sim3Solver.h ├── System.h ├── Tracking.h └── Viewer.h ├── run_exp_kitti.py ├── run_exp_tum.py └── src ├── Converter.cc ├── Frame.cc ├── FrameDrawer.cc ├── Initializer.cc ├── KeyFrame.cc ├── KeyFrameDatabase.cc ├── LocalMapping.cc ├── LoopClosing.cc ├── Map.cc ├── MapDrawer.cc ├── MapPoint.cc ├── ORBextractor.cc ├── ORBmatcher.cc ├── ObjectMatcher.cc ├── ObjectOptimizer.cc ├── ObjectTypes.cc ├── Optimizer.cc ├── PnPsolver.cc ├── Semantic.cc ├── Sim3Solver.cc ├── System.cc ├── Tracking.cc └── Viewer.cc /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(ORB_SLAM2) 3 | 4 | IF(NOT CMAKE_BUILD_TYPE) 5 | SET(CMAKE_BUILD_TYPE Release) 6 | ENDIF() 7 | 8 | MESSAGE("Build type: " ${CMAKE_BUILD_TYPE}) 9 | 10 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 11 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 12 | 13 | # Check C++11 or C++0x support 14 | include(CheckCXXCompilerFlag) 15 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 16 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 17 | if(COMPILER_SUPPORTS_CXX11) 18 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 19 | add_definitions(-DCOMPILEDWITHC11) 20 | message(STATUS "Using flag -std=c++11.") 21 | elseif(COMPILER_SUPPORTS_CXX0X) 22 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 23 | add_definitions(-DCOMPILEDWITHC0X) 24 | message(STATUS "Using flag -std=c++0x.") 25 | else() 26 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 27 | endif() 28 | 29 | LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) 30 | 31 | find_package(OpenCV 3.0 QUIET) 32 | if(NOT OpenCV_FOUND) 33 | find_package(OpenCV 2.4.3 QUIET) 34 | if(NOT OpenCV_FOUND) 35 | message(FATAL_ERROR "OpenCV > 2.4.3 not found.") 36 | endif() 37 | endif() 38 | 39 | find_package(Eigen3 3.1.0 REQUIRED) 40 | find_package(Pangolin REQUIRED) 41 | find_package(PCL REQUIRED) 42 | 43 | include_directories( 44 | ${PROJECT_SOURCE_DIR} 45 | ${PROJECT_SOURCE_DIR}/include 46 | ${EIGEN3_INCLUDE_DIR} 47 | ${Pangolin_INCLUDE_DIRS} 48 | ${PCL_INCLUDE_DIRS} 49 | ) 50 | 51 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) 52 | 53 | add_library(${PROJECT_NAME} SHARED 54 | src/System.cc 55 | src/Tracking.cc 56 | src/LocalMapping.cc 57 | src/LoopClosing.cc 58 | src/ORBextractor.cc 59 | src/ORBmatcher.cc 60 | src/FrameDrawer.cc 61 | src/Converter.cc 62 | src/MapPoint.cc 63 | src/KeyFrame.cc 64 | src/Map.cc 65 | src/MapDrawer.cc 66 | src/Optimizer.cc 67 | src/PnPsolver.cc 68 | src/Frame.cc 69 | src/KeyFrameDatabase.cc 70 | src/Sim3Solver.cc 71 | src/Initializer.cc 72 | src/Viewer.cc 73 | src/Semantic.cc 74 | src/ObjectTypes.cc 75 | src/ObjectMatcher.cc 76 | src/ObjectOptimizer.cc 77 | ) 78 | 79 | target_link_libraries(${PROJECT_NAME} 80 | ${OpenCV_LIBS} 81 | ${EIGEN3_LIBS} 82 | ${Pangolin_LIBRARIES} 83 | ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so 84 | ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so 85 | ${PCL_LIBRARIES} 86 | ) 87 | 88 | # Build examples 89 | 90 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D) 91 | 92 | add_executable(rgbd_tum 93 | Examples/RGB-D/rgbd_tum.cc) 94 | target_link_libraries(rgbd_tum ${PROJECT_NAME}) 95 | 96 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo) 97 | 98 | add_executable(stereo_kitti 99 | Examples/Stereo/stereo_kitti.cc) 100 | target_link_libraries(stereo_kitti ${PROJECT_NAME}) 101 | 102 | add_executable(stereo_euroc 103 | Examples/Stereo/stereo_euroc.cc) 104 | target_link_libraries(stereo_euroc ${PROJECT_NAME}) 105 | 106 | 107 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular) 108 | 109 | add_executable(mono_tum 110 | Examples/Monocular/mono_tum.cc) 111 | target_link_libraries(mono_tum ${PROJECT_NAME}) 112 | 113 | add_executable(mono_kitti 114 | Examples/Monocular/mono_kitti.cc) 115 | target_link_libraries(mono_kitti ${PROJECT_NAME}) 116 | 117 | add_executable(mono_euroc 118 | Examples/Monocular/mono_euroc.cc) 119 | target_link_libraries(mono_euroc ${PROJECT_NAME}) 120 | 121 | -------------------------------------------------------------------------------- /Dependencies.md: -------------------------------------------------------------------------------- 1 | ## List of Known Dependencies 2 | ### ObjectSLAM 3 | 4 | In this document we list all the pieces of code included by ORB-SLAM2 and linked libraries which are not property of the authors of ORB-SLAM2. 5 | 6 | ##### Code in **src** and **include** folders 7 | 8 | * *ORBextractor.cc*. 9 | This is a modified version of orb.cpp of OpenCV library. The original code is BSD licensed. 10 | * *PnPsolver.h, PnPsolver.cc*. 11 | This is a modified version of the epnp.h and epnp.cc of Vincent Lepetit. 12 | This code can be found in popular BSD licensed computer vision libraries as [OpenCV](https://github.com/Itseez/opencv/blob/master/modules/calib3d/src/epnp.cpp) and [OpenGV](https://github.com/laurentkneip/opengv/blob/master/src/absolute_pose/modules/Epnp.cpp). The original code is FreeBSD. 13 | * Function *ORBmatcher::DescriptorDistance* in *ORBmatcher.cc*. 14 | The code is from: http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel. 15 | The code is in the public domain. 16 | * ObjectTypes.cc, ObjectTypes.h, Tracking.cc, Tracking.h 17 | 18 | ​ The code related to point cloud processing is from [PCL](https://pointclouds.org/). 19 | 20 | ##### Code in Thirdparty folder 21 | 22 | * All code in **DBoW2** folder. 23 | This is a modified version of [DBoW2](https://github.com/dorian3d/DBoW2) and [DLib](https://github.com/dorian3d/DLib) library. All files included are BSD licensed. 24 | 25 | * All code in **g2o** folder. 26 | This is a modified version of [g2o](https://github.com/RainerKuemmerle/g2o). All files included are BSD licensed. 27 | 28 | ##### Library dependencies 29 | 30 | * **Pangolin (visualization and user interface)**. 31 | [MIT license](https://en.wikipedia.org/wiki/MIT_License). 32 | 33 | * **OpenCV**. 34 | BSD license. 35 | 36 | * **Eigen3**. 37 | For versions greater than 3.1.1 is MPL2, earlier versions are LGPLv3. 38 | 39 | * **ROS (Optional, only if you build Examples/ROS)**. 40 | BSD license. In the manifest.xml the only declared package dependencies are roscpp, tf, sensor_msgs, image_transport, cv_bridge, which are all BSD licensed. 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /Examples/Monocular/EuRoC.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 458.654 9 | Camera.fy: 457.296 10 | Camera.cx: 367.215 11 | Camera.cy: 248.375 12 | 13 | Camera.k1: -0.28340811 14 | Camera.k2: 0.07395907 15 | Camera.p1: 0.00019359 16 | Camera.p2: 1.76187114e-05 17 | 18 | # Camera frames per second 19 | Camera.fps: 20.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 1000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #--------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.05 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 0.9 50 | Viewer.PointSize:2 51 | Viewer.CameraSize: 0.08 52 | Viewer.CameraLineWidth: 3 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -0.7 55 | Viewer.ViewpointZ: -1.8 56 | Viewer.ViewpointF: 500 57 | 58 | -------------------------------------------------------------------------------- /Examples/Monocular/KITTI00-02.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 718.856 9 | Camera.fy: 718.856 10 | Camera.cx: 607.1928 11 | Camera.cy: 185.2157 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | # Camera frames per second 19 | Camera.fps: 10.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 2000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #-------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.1 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 1 50 | Viewer.PointSize:2 51 | Viewer.CameraSize: 0.15 52 | Viewer.CameraLineWidth: 2 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -10 55 | Viewer.ViewpointZ: -0.1 56 | Viewer.ViewpointF: 2000 57 | 58 | -------------------------------------------------------------------------------- /Examples/Monocular/KITTI03.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 721.5377 9 | Camera.fy: 721.5377 10 | Camera.cx: 609.5593 11 | Camera.cy: 172.854 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | # Camera frames per second 19 | Camera.fps: 10.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 2000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #-------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.1 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 1 50 | Viewer.PointSize:2 51 | Viewer.CameraSize: 0.15 52 | Viewer.CameraLineWidth: 2 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -10 55 | Viewer.ViewpointZ: -0.1 56 | Viewer.ViewpointF: 2000 57 | 58 | -------------------------------------------------------------------------------- /Examples/Monocular/KITTI04-12.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 707.0912 9 | Camera.fy: 707.0912 10 | Camera.cx: 601.8873 11 | Camera.cy: 183.1104 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | # Camera frames per second 19 | Camera.fps: 10.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 2000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #-------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.1 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 1 50 | Viewer.PointSize:2 51 | Viewer.CameraSize: 0.15 52 | Viewer.CameraLineWidth: 2 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -10 55 | Viewer.ViewpointZ: -0.1 56 | Viewer.ViewpointF: 2000 57 | 58 | -------------------------------------------------------------------------------- /Examples/Monocular/TUM1.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 517.306408 9 | Camera.fy: 516.469215 10 | Camera.cx: 318.643040 11 | Camera.cy: 255.313989 12 | 13 | Camera.k1: 0.262383 14 | Camera.k2: -0.953104 15 | Camera.p1: -0.005358 16 | Camera.p2: 0.002628 17 | Camera.k3: 1.163314 18 | 19 | # Camera frames per second 20 | Camera.fps: 30.0 21 | 22 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 23 | Camera.RGB: 1 24 | 25 | #-------------------------------------------------------------------------------------------- 26 | # ORB Parameters 27 | #-------------------------------------------------------------------------------------------- 28 | 29 | # ORB Extractor: Number of features per image 30 | ORBextractor.nFeatures: 1000 31 | 32 | # ORB Extractor: Scale factor between levels in the scale pyramid 33 | ORBextractor.scaleFactor: 1.2 34 | 35 | # ORB Extractor: Number of levels in the scale pyramid 36 | ORBextractor.nLevels: 8 37 | 38 | # ORB Extractor: Fast threshold 39 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 40 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 41 | # You can lower these values if your images have low contrast 42 | ORBextractor.iniThFAST: 20 43 | ORBextractor.minThFAST: 7 44 | 45 | #-------------------------------------------------------------------------------------------- 46 | # Viewer Parameters 47 | #-------------------------------------------------------------------------------------------- 48 | Viewer.KeyFrameSize: 0.05 49 | Viewer.KeyFrameLineWidth: 1 50 | Viewer.GraphLineWidth: 0.9 51 | Viewer.PointSize:2 52 | Viewer.CameraSize: 0.08 53 | Viewer.CameraLineWidth: 3 54 | Viewer.ViewpointX: 0 55 | Viewer.ViewpointY: -0.7 56 | Viewer.ViewpointZ: -1.8 57 | Viewer.ViewpointF: 500 58 | 59 | -------------------------------------------------------------------------------- /Examples/Monocular/TUM2.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 520.908620 9 | Camera.fy: 521.007327 10 | Camera.cx: 325.141442 11 | Camera.cy: 249.701764 12 | 13 | Camera.k1: 0.231222 14 | Camera.k2: -0.784899 15 | Camera.p1: -0.003257 16 | Camera.p2: -0.000105 17 | Camera.k3: 0.917205 18 | 19 | # Camera frames per second 20 | Camera.fps: 30.0 21 | 22 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 23 | Camera.RGB: 1 24 | 25 | #-------------------------------------------------------------------------------------------- 26 | # ORB Parameters 27 | #-------------------------------------------------------------------------------------------- 28 | 29 | # ORB Extractor: Number of features per image 30 | ORBextractor.nFeatures: 1000 31 | 32 | # ORB Extractor: Scale factor between levels in the scale pyramid 33 | ORBextractor.scaleFactor: 1.2 34 | 35 | # ORB Extractor: Number of levels in the scale pyramid 36 | ORBextractor.nLevels: 8 37 | 38 | # ORB Extractor: Fast threshold 39 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 40 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 41 | # You can lower these values if your images have low contrast 42 | ORBextractor.iniThFAST: 20 43 | ORBextractor.minThFAST: 7 44 | 45 | #-------------------------------------------------------------------------------------------- 46 | # Viewer Parameters 47 | #-------------------------------------------------------------------------------------------- 48 | Viewer.KeyFrameSize: 0.05 49 | Viewer.KeyFrameLineWidth: 1 50 | Viewer.GraphLineWidth: 0.9 51 | Viewer.PointSize:2 52 | Viewer.CameraSize: 0.08 53 | Viewer.CameraLineWidth: 3 54 | Viewer.ViewpointX: 0 55 | Viewer.ViewpointY: -0.7 56 | Viewer.ViewpointZ: -1.8 57 | Viewer.ViewpointF: 500 58 | 59 | -------------------------------------------------------------------------------- /Examples/Monocular/TUM3.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 535.4 9 | Camera.fy: 539.2 10 | Camera.cx: 320.1 11 | Camera.cy: 247.6 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | # Camera frames per second 19 | Camera.fps: 30.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 1000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #-------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.05 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 0.9 50 | Viewer.PointSize:2 51 | Viewer.CameraSize: 0.08 52 | Viewer.CameraLineWidth: 3 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -0.7 55 | Viewer.ViewpointZ: -1.8 56 | Viewer.ViewpointF: 500 57 | 58 | -------------------------------------------------------------------------------- /Examples/Monocular/mono_euroc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yangliu9527/Object_SLAM/0154923286a9b5fb4b61032b3341e77d4ec883cf/Examples/Monocular/mono_euroc -------------------------------------------------------------------------------- /Examples/Monocular/mono_euroc.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | 30 | #include 31 | 32 | using namespace std; 33 | 34 | void LoadImages(const string &strImagePath, const string &strPathTimes, 35 | vector &vstrImages, vector &vTimeStamps); 36 | 37 | int main(int argc, char **argv) 38 | { 39 | if(argc != 5) 40 | { 41 | cerr << endl << "Usage: ./mono_tum path_to_vocabulary path_to_settings path_to_image_folder path_to_times_file" << endl; 42 | return 1; 43 | } 44 | 45 | // Retrieve paths to images 46 | vector vstrImageFilenames; 47 | vector vTimestamps; 48 | LoadImages(string(argv[3]), string(argv[4]), vstrImageFilenames, vTimestamps); 49 | 50 | int nImages = vstrImageFilenames.size(); 51 | 52 | if(nImages<=0) 53 | { 54 | cerr << "ERROR: Failed to load images" << endl; 55 | return 1; 56 | } 57 | 58 | // Create SLAM system. It initializes all system threads and gets ready to process frames. 59 | ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true); 60 | 61 | // Vector for tracking time statistics 62 | vector vTimesTrack; 63 | vTimesTrack.resize(nImages); 64 | 65 | cout << endl << "-------" << endl; 66 | cout << "Start processing sequence ..." << endl; 67 | cout << "Images in the sequence: " << nImages << endl << endl; 68 | 69 | // Main loop 70 | cv::Mat im; 71 | for(int ni=0; ni >(t2 - t1).count(); 100 | 101 | vTimesTrack[ni]=ttrack; 102 | 103 | // Wait to load the next frame 104 | double T=0; 105 | if(ni0) 108 | T = tframe-vTimestamps[ni-1]; 109 | 110 | if(ttrack &vstrImages, vector &vTimeStamps) 136 | { 137 | ifstream fTimes; 138 | fTimes.open(strPathTimes.c_str()); 139 | vTimeStamps.reserve(5000); 140 | vstrImages.reserve(5000); 141 | while(!fTimes.eof()) 142 | { 143 | string s; 144 | getline(fTimes,s); 145 | if(!s.empty()) 146 | { 147 | stringstream ss; 148 | ss << s; 149 | vstrImages.push_back(strImagePath + "/" + ss.str() + ".png"); 150 | double t; 151 | ss >> t; 152 | vTimeStamps.push_back(t/1e9); 153 | 154 | } 155 | } 156 | } 157 | -------------------------------------------------------------------------------- /Examples/Monocular/mono_kitti: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yangliu9527/Object_SLAM/0154923286a9b5fb4b61032b3341e77d4ec883cf/Examples/Monocular/mono_kitti -------------------------------------------------------------------------------- /Examples/Monocular/mono_kitti.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | #include"System.h" 32 | 33 | using namespace std; 34 | 35 | void LoadImages(const string &strSequence, vector &vstrImageFilenames, 36 | vector &vTimestamps); 37 | 38 | int main(int argc, char **argv) 39 | { 40 | if(argc != 4) 41 | { 42 | cerr << endl << "Usage: ./mono_kitti path_to_vocabulary path_to_settings path_to_sequence" << endl; 43 | return 1; 44 | } 45 | 46 | // Retrieve paths to images 47 | vector vstrImageFilenames; 48 | vector vTimestamps; 49 | LoadImages(string(argv[3]), vstrImageFilenames, vTimestamps); 50 | 51 | int nImages = vstrImageFilenames.size(); 52 | 53 | // Create SLAM system. It initializes all system threads and gets ready to process frames. 54 | ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true); 55 | 56 | // Vector for tracking time statistics 57 | vector vTimesTrack; 58 | vTimesTrack.resize(nImages); 59 | 60 | cout << endl << "-------" << endl; 61 | cout << "Start processing sequence ..." << endl; 62 | cout << "Images in the sequence: " << nImages << endl << endl; 63 | 64 | // Main loop 65 | cv::Mat im; 66 | for(int ni=0; ni >(t2 - t1).count(); 94 | 95 | vTimesTrack[ni]=ttrack; 96 | 97 | // Wait to load the next frame 98 | double T=0; 99 | if(ni0) 102 | T = tframe-vTimestamps[ni-1]; 103 | 104 | if(ttrack &vstrImageFilenames, vector &vTimestamps) 129 | { 130 | ifstream fTimes; 131 | string strPathTimeFile = strPathToSequence + "/times.txt"; 132 | fTimes.open(strPathTimeFile.c_str()); 133 | while(!fTimes.eof()) 134 | { 135 | string s; 136 | getline(fTimes,s); 137 | if(!s.empty()) 138 | { 139 | stringstream ss; 140 | ss << s; 141 | double t; 142 | ss >> t; 143 | vTimestamps.push_back(t); 144 | } 145 | } 146 | 147 | string strPrefixLeft = strPathToSequence + "/image_0/"; 148 | 149 | const int nTimes = vTimestamps.size(); 150 | vstrImageFilenames.resize(nTimes); 151 | 152 | for(int i=0; i (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | 30 | #include 31 | 32 | using namespace std; 33 | 34 | void LoadImages(const string &strFile, vector &vstrImageFilenames, 35 | vector &vTimestamps); 36 | 37 | int main(int argc, char **argv) 38 | { 39 | if(argc != 4) 40 | { 41 | cerr << endl << "Usage: ./mono_tum path_to_vocabulary path_to_settings path_to_sequence" << endl; 42 | return 1; 43 | } 44 | 45 | // Retrieve paths to images 46 | vector vstrImageFilenames; 47 | vector vTimestamps; 48 | string strFile = string(argv[3])+"/rgb.txt"; 49 | LoadImages(strFile, vstrImageFilenames, vTimestamps); 50 | 51 | int nImages = vstrImageFilenames.size(); 52 | 53 | // Create SLAM system. It initializes all system threads and gets ready to process frames. 54 | ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true); 55 | 56 | // Vector for tracking time statistics 57 | vector vTimesTrack; 58 | vTimesTrack.resize(nImages); 59 | 60 | cout << endl << "-------" << endl; 61 | cout << "Start processing sequence ..." << endl; 62 | cout << "Images in the sequence: " << nImages << endl << endl; 63 | 64 | // Main loop 65 | cv::Mat im; 66 | for(int ni=0; ni >(t2 - t1).count(); 95 | 96 | vTimesTrack[ni]=ttrack; 97 | 98 | // Wait to load the next frame 99 | double T=0; 100 | if(ni0) 103 | T = tframe-vTimestamps[ni-1]; 104 | 105 | if(ttrack &vstrImageFilenames, vector &vTimestamps) 130 | { 131 | ifstream f; 132 | f.open(strFile.c_str()); 133 | 134 | // skip first three lines 135 | string s0; 136 | getline(f,s0); 137 | getline(f,s0); 138 | getline(f,s0); 139 | 140 | while(!f.eof()) 141 | { 142 | string s; 143 | getline(f,s); 144 | if(!s.empty()) 145 | { 146 | stringstream ss; 147 | ss << s; 148 | double t; 149 | string sRGB; 150 | ss >> t; 151 | vTimestamps.push_back(t); 152 | ss >> sRGB; 153 | vstrImageFilenames.push_back(sRGB); 154 | } 155 | } 156 | } 157 | -------------------------------------------------------------------------------- /Examples/RGB-D/TUM1.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 517.306408 9 | Camera.fy: 516.469215 10 | Camera.cx: 318.643040 11 | Camera.cy: 255.313989 12 | 13 | Camera.k1: 0.262383 14 | Camera.k2: -0.953104 15 | Camera.p1: -0.005358 16 | Camera.p2: 0.002628 17 | Camera.k3: 1.163314 18 | 19 | Camera.width: 640 20 | Camera.height: 480 21 | 22 | # Camera frames per second 23 | Camera.fps: 30.0 24 | 25 | # IR projector baseline times fx (aprox.) 26 | Camera.bf: 40.0 27 | 28 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 29 | Camera.RGB: 1 30 | 31 | # Close/Far threshold. Baseline times. 32 | ThDepth: 40.0 33 | 34 | # Deptmap values factor 35 | DepthMapFactor: 5000.0 36 | 37 | #-------------------------------------------------------------------------------------------- 38 | # ORB Parameters 39 | #-------------------------------------------------------------------------------------------- 40 | 41 | # ORB Extractor: Number of features per image 42 | ORBextractor.nFeatures: 1000 43 | 44 | # ORB Extractor: Scale factor between levels in the scale pyramid 45 | ORBextractor.scaleFactor: 1.2 46 | 47 | # ORB Extractor: Number of levels in the scale pyramid 48 | ORBextractor.nLevels: 8 49 | 50 | # ORB Extractor: Fast threshold 51 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 52 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 53 | # You can lower these values if your images have low contrast 54 | ORBextractor.iniThFAST: 20 55 | ORBextractor.minThFAST: 7 56 | 57 | #-------------------------------------------------------------------------------------------- 58 | # Viewer Parameters 59 | #-------------------------------------------------------------------------------------------- 60 | Viewer.KeyFrameSize: 0.05 61 | Viewer.KeyFrameLineWidth: 1 62 | Viewer.GraphLineWidth: 0.9 63 | Viewer.PointSize:2 64 | Viewer.CameraSize: 0.08 65 | Viewer.CameraLineWidth: 3 66 | Viewer.ViewpointX: 0 67 | Viewer.ViewpointY: -0.7 68 | Viewer.ViewpointZ: -1.8 69 | Viewer.ViewpointF: 500 70 | 71 | -------------------------------------------------------------------------------- /Examples/RGB-D/TUM2.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 520.908620 9 | Camera.fy: 521.007327 10 | Camera.cx: 325.141442 11 | Camera.cy: 249.701764 12 | 13 | Camera.k1: 0.231222 14 | Camera.k2: -0.784899 15 | Camera.p1: -0.003257 16 | Camera.p2: -0.000105 17 | Camera.k3: 0.917205 18 | 19 | Camera.width: 640 20 | Camera.height: 480 21 | 22 | # Camera frames per second 23 | Camera.fps: 30.0 24 | 25 | # IR projector baseline times fx (aprox.) 26 | Camera.bf: 40.0 27 | 28 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 29 | Camera.RGB: 1 30 | 31 | # Close/Far threshold. Baseline times. 32 | ThDepth: 40.0 33 | 34 | # Deptmap values factor 35 | DepthMapFactor: 5208.0 36 | 37 | #-------------------------------------------------------------------------------------------- 38 | # ORB Parameters 39 | #-------------------------------------------------------------------------------------------- 40 | 41 | # ORB Extractor: Number of features per image 42 | ORBextractor.nFeatures: 1000 43 | 44 | # ORB Extractor: Scale factor between levels in the scale pyramid 45 | ORBextractor.scaleFactor: 1.2 46 | 47 | # ORB Extractor: Number of levels in the scale pyramid 48 | ORBextractor.nLevels: 8 49 | 50 | # ORB Extractor: Fast threshold 51 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 52 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 53 | # You can lower these values if your images have low contrast 54 | ORBextractor.iniThFAST: 20 55 | ORBextractor.minThFAST: 7 56 | 57 | #-------------------------------------------------------------------------------------------- 58 | # Viewer Parameters 59 | #-------------------------------------------------------------------------------------------- 60 | Viewer.KeyFrameSize: 0.05 61 | Viewer.KeyFrameLineWidth: 1 62 | Viewer.GraphLineWidth: 0.9 63 | Viewer.PointSize:2 64 | Viewer.CameraSize: 0.08 65 | Viewer.CameraLineWidth: 3 66 | Viewer.ViewpointX: 0 67 | Viewer.ViewpointY: -0.7 68 | Viewer.ViewpointZ: -1.8 69 | Viewer.ViewpointF: 500 70 | 71 | -------------------------------------------------------------------------------- /Examples/RGB-D/TUM3.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 535.4 9 | Camera.fy: 539.2 10 | Camera.cx: 320.1 11 | Camera.cy: 247.6 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 640 19 | Camera.height: 480 20 | 21 | # Camera frames per second 22 | Camera.fps: 30.0 23 | 24 | # IR projector baseline times fx (aprox.) 25 | Camera.bf: 40.0 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 40.0 32 | 33 | # Deptmap values factor 34 | DepthMapFactor: 5000.0 35 | 36 | #-------------------------------------------------------------------------------------------- 37 | # ORB Parameters 38 | #-------------------------------------------------------------------------------------------- 39 | 40 | # ORB Extractor: Number of features per image 41 | ORBextractor.nFeatures: 1000 42 | 43 | # ORB Extractor: Scale factor between levels in the scale pyramid 44 | ORBextractor.scaleFactor: 1.2 45 | 46 | # ORB Extractor: Number of levels in the scale pyramid 47 | ORBextractor.nLevels: 8 48 | 49 | # ORB Extractor: Fast threshold 50 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 51 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 52 | # You can lower these values if your images have low contrast 53 | ORBextractor.iniThFAST: 20 54 | ORBextractor.minThFAST: 7 55 | 56 | #-------------------------------------------------------------------------------------------- 57 | # Viewer Parameters 58 | #-------------------------------------------------------------------------------------------- 59 | Viewer.KeyFrameSize: 0.05 60 | Viewer.KeyFrameLineWidth: 1 61 | Viewer.GraphLineWidth: 0.9 62 | Viewer.PointSize:2 63 | Viewer.CameraSize: 0.08 64 | Viewer.CameraLineWidth: 3 65 | Viewer.ViewpointX: 0 66 | Viewer.ViewpointY: -0.7 67 | Viewer.ViewpointZ: -1.8 68 | Viewer.ViewpointF: 500 69 | 70 | -------------------------------------------------------------------------------- /Examples/RGB-D/rgbd_tum: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yangliu9527/Object_SLAM/0154923286a9b5fb4b61032b3341e77d4ec883cf/Examples/RGB-D/rgbd_tum -------------------------------------------------------------------------------- /Examples/RGB-D/rgbd_tum.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | 30 | #include 31 | 32 | using namespace std; 33 | 34 | void LoadImages(const string &strAssociationFilename, vector &vstrImageFilenamesRGB, 35 | vector &vstrImageFilenamesD, vector &vTimestamps); 36 | 37 | int main(int argc, char **argv) 38 | { 39 | if(argc != 5) 40 | { 41 | cerr << endl << "Usage: ./rgbd_tum path_to_vocabulary path_to_settings path_to_sequence path_to_association" << endl; 42 | return 1; 43 | } 44 | 45 | // Retrieve paths to images 46 | vector vstrImageFilenamesRGB; 47 | vector vstrImageFilenamesD; 48 | vector vTimestamps; 49 | string strAssociationFilename = string(argv[4]); 50 | LoadImages(strAssociationFilename, vstrImageFilenamesRGB, vstrImageFilenamesD, vTimestamps); 51 | 52 | // Check consistency in the number of images and depthmaps 53 | int nImages = vstrImageFilenamesRGB.size(); 54 | if(vstrImageFilenamesRGB.empty()) 55 | { 56 | cerr << endl << "No images found in provided path." << endl; 57 | return 1; 58 | } 59 | else if(vstrImageFilenamesD.size()!=vstrImageFilenamesRGB.size()) 60 | { 61 | cerr << endl << "Different number of images for rgb and depth." << endl; 62 | return 1; 63 | } 64 | 65 | // Create SLAM system. It initializes all system threads and gets ready to process frames. 66 | ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true); 67 | 68 | // Vector for tracking time statistics 69 | vector vTimesTrack; 70 | vTimesTrack.resize(nImages); 71 | 72 | cout << endl << "-------" << endl; 73 | cout << "Start processing sequence ..." << endl; 74 | cout << "Images in the sequence: " << nImages << endl << endl; 75 | 76 | // Main loop 77 | cv::Mat imRGB, imD; 78 | for(int ni=0; ni >(t2 - t1).count(); 108 | 109 | vTimesTrack[ni]=ttrack; 110 | 111 | // Wait to load the next frame 112 | double T=0; 113 | if(ni0) 116 | T = tframe-vTimestamps[ni-1]; 117 | 118 | if(ttrack &vstrImageFilenamesRGB, 145 | vector &vstrImageFilenamesD, vector &vTimestamps) 146 | { 147 | ifstream fAssociation; 148 | fAssociation.open(strAssociationFilename.c_str()); 149 | while(!fAssociation.eof()) 150 | { 151 | string s; 152 | getline(fAssociation,s); 153 | if(!s.empty()) 154 | { 155 | stringstream ss; 156 | ss << s; 157 | double t; 158 | string sRGB, sD; 159 | ss >> t; 160 | vTimestamps.push_back(t); 161 | ss >> sRGB; 162 | vstrImageFilenamesRGB.push_back(sRGB); 163 | ss >> t; 164 | ss >> sD; 165 | vstrImageFilenamesD.push_back(sD); 166 | 167 | } 168 | } 169 | } 170 | -------------------------------------------------------------------------------- /Examples/ROS/ORB_SLAM2/Asus.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 535.4 9 | Camera.fy: 539.2 10 | Camera.cx: 320.1 11 | Camera.cy: 247.6 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 640 19 | Camera.height: 480 20 | 21 | # Camera frames per second 22 | Camera.fps: 30.0 23 | 24 | # IR projector baseline times fx (aprox.) 25 | Camera.bf: 40.0 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 40.0 32 | 33 | # Deptmap values factor 34 | DepthMapFactor: 1.0 35 | 36 | #-------------------------------------------------------------------------------------------- 37 | # ORB Parameters 38 | #-------------------------------------------------------------------------------------------- 39 | 40 | # ORB Extractor: Number of features per image 41 | ORBextractor.nFeatures: 1000 42 | 43 | # ORB Extractor: Scale factor between levels in the scale pyramid 44 | ORBextractor.scaleFactor: 1.2 45 | 46 | # ORB Extractor: Number of levels in the scale pyramid 47 | ORBextractor.nLevels: 8 48 | 49 | # ORB Extractor: Fast threshold 50 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 51 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 52 | # You can lower these values if your images have low contrast 53 | ORBextractor.iniThFAST: 20 54 | ORBextractor.minThFAST: 7 55 | 56 | #-------------------------------------------------------------------------------------------- 57 | # Viewer Parameters 58 | #-------------------------------------------------------------------------------------------- 59 | Viewer.KeyFrameSize: 0.05 60 | Viewer.KeyFrameLineWidth: 1 61 | Viewer.GraphLineWidth: 0.9 62 | Viewer.PointSize:2 63 | Viewer.CameraSize: 0.08 64 | Viewer.CameraLineWidth: 3 65 | Viewer.ViewpointX: 0 66 | Viewer.ViewpointY: -0.7 67 | Viewer.ViewpointZ: -1.8 68 | Viewer.ViewpointF: 500 69 | 70 | -------------------------------------------------------------------------------- /Examples/ROS/ORB_SLAM2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | rosbuild_init() 5 | 6 | IF(NOT ROS_BUILD_TYPE) 7 | SET(ROS_BUILD_TYPE Release) 8 | ENDIF() 9 | 10 | MESSAGE("Build type: " ${ROS_BUILD_TYPE}) 11 | 12 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 13 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 14 | 15 | # Check C++11 or C++0x support 16 | include(CheckCXXCompilerFlag) 17 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 18 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 19 | if(COMPILER_SUPPORTS_CXX11) 20 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 21 | add_definitions(-DCOMPILEDWITHC11) 22 | message(STATUS "Using flag -std=c++11.") 23 | elseif(COMPILER_SUPPORTS_CXX0X) 24 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 25 | add_definitions(-DCOMPILEDWITHC0X) 26 | message(STATUS "Using flag -std=c++0x.") 27 | else() 28 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 29 | endif() 30 | 31 | LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../../../cmake_modules) 32 | 33 | find_package(OpenCV 3.0 QUIET) 34 | if(NOT OpenCV_FOUND) 35 | find_package(OpenCV 2.4.3 QUIET) 36 | if(NOT OpenCV_FOUND) 37 | message(FATAL_ERROR "OpenCV > 2.4.3 not found.") 38 | endif() 39 | endif() 40 | 41 | find_package(Eigen3 3.1.0 REQUIRED) 42 | find_package(Pangolin REQUIRED) 43 | 44 | include_directories( 45 | ${PROJECT_SOURCE_DIR} 46 | ${PROJECT_SOURCE_DIR}/../../../ 47 | ${PROJECT_SOURCE_DIR}/../../../include 48 | ${Pangolin_INCLUDE_DIRS} 49 | ) 50 | 51 | set(LIBS 52 | ${OpenCV_LIBS} 53 | ${EIGEN3_LIBS} 54 | ${Pangolin_LIBRARIES} 55 | ${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so 56 | ${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so 57 | ${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM2.so 58 | ) 59 | 60 | # Node for monocular camera 61 | rosbuild_add_executable(Mono 62 | src/ros_mono.cc 63 | ) 64 | 65 | target_link_libraries(Mono 66 | ${LIBS} 67 | ) 68 | 69 | # Node for monocular camera (Augmented Reality Demo) 70 | rosbuild_add_executable(MonoAR 71 | src/AR/ros_mono_ar.cc 72 | src/AR/ViewerAR.h 73 | src/AR/ViewerAR.cc 74 | ) 75 | 76 | target_link_libraries(MonoAR 77 | ${LIBS} 78 | ) 79 | 80 | # Node for stereo camera 81 | rosbuild_add_executable(Stereo 82 | src/ros_stereo.cc 83 | ) 84 | 85 | target_link_libraries(Stereo 86 | ${LIBS} 87 | ) 88 | 89 | # Node for RGB-D camera 90 | rosbuild_add_executable(RGBD 91 | src/ros_rgbd.cc 92 | ) 93 | 94 | target_link_libraries(RGBD 95 | ${LIBS} 96 | ) 97 | 98 | -------------------------------------------------------------------------------- /Examples/ROS/ORB_SLAM2/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ORB_SLAM2 5 | 6 | 7 | Raul Mur-Artal 8 | GPLv3 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #ifndef VIEWERAR_H 23 | #define VIEWERAR_H 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include"../../../include/System.h" 30 | 31 | namespace ORB_SLAM2 32 | { 33 | 34 | class Plane 35 | { 36 | public: 37 | Plane(const std::vector &vMPs, const cv::Mat &Tcw); 38 | Plane(const float &nx, const float &ny, const float &nz, const float &ox, const float &oy, const float &oz); 39 | 40 | void Recompute(); 41 | 42 | //normal 43 | cv::Mat n; 44 | //origin 45 | cv::Mat o; 46 | //arbitrary orientation along normal 47 | float rang; 48 | //transformation from world to the plane 49 | cv::Mat Tpw; 50 | pangolin::OpenGlMatrix glTpw; 51 | //MapPoints that define the plane 52 | std::vector mvMPs; 53 | //camera pose when the plane was first observed (to compute normal direction) 54 | cv::Mat mTcw, XC; 55 | }; 56 | 57 | class ViewerAR 58 | { 59 | public: 60 | ViewerAR(); 61 | 62 | void SetFPS(const float fps){ 63 | mFPS = fps; 64 | mT=1e3/fps; 65 | } 66 | 67 | void SetSLAM(ORB_SLAM2::System* pSystem){ 68 | mpSystem = pSystem; 69 | } 70 | 71 | // Main thread function. 72 | void Run(); 73 | 74 | void SetCameraCalibration(const float &fx_, const float &fy_, const float &cx_, const float &cy_){ 75 | fx = fx_; fy = fy_; cx = cx_; cy = cy_; 76 | } 77 | 78 | void SetImagePose(const cv::Mat &im, const cv::Mat &Tcw, const int &status, 79 | const std::vector &vKeys, const std::vector &vMPs); 80 | 81 | void GetImagePose(cv::Mat &im, cv::Mat &Tcw, int &status, 82 | std::vector &vKeys, std::vector &vMPs); 83 | 84 | private: 85 | 86 | //SLAM 87 | ORB_SLAM2::System* mpSystem; 88 | 89 | void PrintStatus(const int &status, const bool &bLocMode, cv::Mat &im); 90 | void AddTextToImage(const std::string &s, cv::Mat &im, const int r=0, const int g=0, const int b=0); 91 | void LoadCameraPose(const cv::Mat &Tcw); 92 | void DrawImageTexture(pangolin::GlTexture &imageTexture, cv::Mat &im); 93 | void DrawCube(const float &size, const float x=0, const float y=0, const float z=0); 94 | void DrawPlane(int ndivs, float ndivsize); 95 | void DrawPlane(Plane* pPlane, int ndivs, float ndivsize); 96 | void DrawTrackedPoints(const std::vector &vKeys, const std::vector &vMPs, cv::Mat &im); 97 | 98 | Plane* DetectPlane(const cv::Mat Tcw, const std::vector &vMPs, const int iterations=50); 99 | 100 | // frame rate 101 | float mFPS, mT; 102 | float fx,fy,cx,cy; 103 | 104 | // Last processed image and computed pose by the SLAM 105 | std::mutex mMutexPoseImage; 106 | cv::Mat mTcw; 107 | cv::Mat mImage; 108 | int mStatus; 109 | std::vector mvKeys; 110 | std::vector mvMPs; 111 | 112 | }; 113 | 114 | 115 | } 116 | 117 | 118 | #endif // VIEWERAR_H 119 | 120 | 121 | -------------------------------------------------------------------------------- /Examples/ROS/ORB_SLAM2/src/AR/ros_mono_ar.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | 33 | #include"../../../include/System.h" 34 | 35 | #include"ViewerAR.h" 36 | 37 | using namespace std; 38 | 39 | 40 | ORB_SLAM2::ViewerAR viewerAR; 41 | bool bRGB = true; 42 | 43 | cv::Mat K; 44 | cv::Mat DistCoef; 45 | 46 | 47 | class ImageGrabber 48 | { 49 | public: 50 | ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){} 51 | 52 | void GrabImage(const sensor_msgs::ImageConstPtr& msg); 53 | 54 | ORB_SLAM2::System* mpSLAM; 55 | }; 56 | 57 | int main(int argc, char **argv) 58 | { 59 | ros::init(argc, argv, "Mono"); 60 | ros::start(); 61 | 62 | if(argc != 3) 63 | { 64 | cerr << endl << "Usage: rosrun ORB_SLAM2 Mono path_to_vocabulary path_to_settings" << endl; 65 | ros::shutdown(); 66 | return 1; 67 | } 68 | 69 | // Create SLAM system. It initializes all system threads and gets ready to process frames. 70 | ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,false); 71 | 72 | 73 | cout << endl << endl; 74 | cout << "-----------------------" << endl; 75 | cout << "Augmented Reality Demo" << endl; 76 | cout << "1) Translate the camera to initialize SLAM." << endl; 77 | cout << "2) Look at a planar region and translate the camera." << endl; 78 | cout << "3) Press Insert Cube to place a virtual cube in the plane. " << endl; 79 | cout << endl; 80 | cout << "You can place several cubes in different planes." << endl; 81 | cout << "-----------------------" << endl; 82 | cout << endl; 83 | 84 | 85 | viewerAR.SetSLAM(&SLAM); 86 | 87 | ImageGrabber igb(&SLAM); 88 | 89 | ros::NodeHandle nodeHandler; 90 | ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb); 91 | 92 | 93 | cv::FileStorage fSettings(argv[2], cv::FileStorage::READ); 94 | bRGB = static_cast((int)fSettings["Camera.RGB"]); 95 | float fps = fSettings["Camera.fps"]; 96 | viewerAR.SetFPS(fps); 97 | 98 | float fx = fSettings["Camera.fx"]; 99 | float fy = fSettings["Camera.fy"]; 100 | float cx = fSettings["Camera.cx"]; 101 | float cy = fSettings["Camera.cy"]; 102 | 103 | viewerAR.SetCameraCalibration(fx,fy,cx,cy); 104 | 105 | K = cv::Mat::eye(3,3,CV_32F); 106 | K.at(0,0) = fx; 107 | K.at(1,1) = fy; 108 | K.at(0,2) = cx; 109 | K.at(1,2) = cy; 110 | 111 | DistCoef = cv::Mat::zeros(4,1,CV_32F); 112 | DistCoef.at(0) = fSettings["Camera.k1"]; 113 | DistCoef.at(1) = fSettings["Camera.k2"]; 114 | DistCoef.at(2) = fSettings["Camera.p1"]; 115 | DistCoef.at(3) = fSettings["Camera.p2"]; 116 | const float k3 = fSettings["Camera.k3"]; 117 | if(k3!=0) 118 | { 119 | DistCoef.resize(5); 120 | DistCoef.at(4) = k3; 121 | } 122 | 123 | thread tViewer = thread(&ORB_SLAM2::ViewerAR::Run,&viewerAR); 124 | 125 | ros::spin(); 126 | 127 | // Stop all threads 128 | SLAM.Shutdown(); 129 | 130 | // Save camera trajectory 131 | SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); 132 | 133 | ros::shutdown(); 134 | 135 | return 0; 136 | } 137 | 138 | void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg) 139 | { 140 | // Copy the ros image message to cv::Mat. 141 | cv_bridge::CvImageConstPtr cv_ptr; 142 | try 143 | { 144 | cv_ptr = cv_bridge::toCvShare(msg); 145 | } 146 | catch (cv_bridge::Exception& e) 147 | { 148 | ROS_ERROR("cv_bridge exception: %s", e.what()); 149 | return; 150 | } 151 | cv::Mat im = cv_ptr->image.clone(); 152 | cv::Mat imu; 153 | cv::Mat Tcw = mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec()); 154 | int state = mpSLAM->GetTrackingState(); 155 | vector vMPs = mpSLAM->GetTrackedMapPoints(); 156 | vector vKeys = mpSLAM->GetTrackedKeyPointsUn(); 157 | 158 | cv::undistort(im,imu,K,DistCoef); 159 | 160 | if(bRGB) 161 | viewerAR.SetImagePose(imu,Tcw,state,vKeys,vMPs); 162 | else 163 | { 164 | cv::cvtColor(imu,imu,CV_RGB2BGR); 165 | viewerAR.SetImagePose(imu,Tcw,state,vKeys,vMPs); 166 | } 167 | } 168 | 169 | 170 | -------------------------------------------------------------------------------- /Examples/ROS/ORB_SLAM2/src/ros_mono.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | #include 31 | 32 | #include"../../../include/System.h" 33 | 34 | using namespace std; 35 | 36 | class ImageGrabber 37 | { 38 | public: 39 | ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){} 40 | 41 | void GrabImage(const sensor_msgs::ImageConstPtr& msg); 42 | 43 | ORB_SLAM2::System* mpSLAM; 44 | }; 45 | 46 | int main(int argc, char **argv) 47 | { 48 | ros::init(argc, argv, "Mono"); 49 | ros::start(); 50 | 51 | if(argc != 3) 52 | { 53 | cerr << endl << "Usage: rosrun ORB_SLAM2 Mono path_to_vocabulary path_to_settings" << endl; 54 | ros::shutdown(); 55 | return 1; 56 | } 57 | 58 | // Create SLAM system. It initializes all system threads and gets ready to process frames. 59 | ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true); 60 | 61 | ImageGrabber igb(&SLAM); 62 | 63 | ros::NodeHandle nodeHandler; 64 | ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb); 65 | 66 | ros::spin(); 67 | 68 | // Stop all threads 69 | SLAM.Shutdown(); 70 | 71 | // Save camera trajectory 72 | SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); 73 | 74 | ros::shutdown(); 75 | 76 | return 0; 77 | } 78 | 79 | void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg) 80 | { 81 | // Copy the ros image message to cv::Mat. 82 | cv_bridge::CvImageConstPtr cv_ptr; 83 | try 84 | { 85 | cv_ptr = cv_bridge::toCvShare(msg); 86 | } 87 | catch (cv_bridge::Exception& e) 88 | { 89 | ROS_ERROR("cv_bridge exception: %s", e.what()); 90 | return; 91 | } 92 | 93 | mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec()); 94 | } 95 | 96 | 97 | -------------------------------------------------------------------------------- /Examples/ROS/ORB_SLAM2/src/ros_rgbd.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | #include"../../../include/System.h" 36 | 37 | using namespace std; 38 | 39 | class ImageGrabber 40 | { 41 | public: 42 | ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){} 43 | 44 | void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD); 45 | 46 | ORB_SLAM2::System* mpSLAM; 47 | }; 48 | 49 | int main(int argc, char **argv) 50 | { 51 | ros::init(argc, argv, "RGBD"); 52 | ros::start(); 53 | 54 | if(argc != 3) 55 | { 56 | cerr << endl << "Usage: rosrun ORB_SLAM2 RGBD path_to_vocabulary path_to_settings" << endl; 57 | ros::shutdown(); 58 | return 1; 59 | } 60 | 61 | // Create SLAM system. It initializes all system threads and gets ready to process frames. 62 | ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true); 63 | 64 | ImageGrabber igb(&SLAM); 65 | 66 | ros::NodeHandle nh; 67 | 68 | message_filters::Subscriber rgb_sub(nh, "/camera/rgb/image_raw", 1); 69 | message_filters::Subscriber depth_sub(nh, "camera/depth_registered/image_raw", 1); 70 | typedef message_filters::sync_policies::ApproximateTime sync_pol; 71 | message_filters::Synchronizer sync(sync_pol(10), rgb_sub,depth_sub); 72 | sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2)); 73 | 74 | ros::spin(); 75 | 76 | // Stop all threads 77 | SLAM.Shutdown(); 78 | 79 | // Save camera trajectory 80 | SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); 81 | 82 | ros::shutdown(); 83 | 84 | return 0; 85 | } 86 | 87 | void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD) 88 | { 89 | // Copy the ros image message to cv::Mat. 90 | cv_bridge::CvImageConstPtr cv_ptrRGB; 91 | try 92 | { 93 | cv_ptrRGB = cv_bridge::toCvShare(msgRGB); 94 | } 95 | catch (cv_bridge::Exception& e) 96 | { 97 | ROS_ERROR("cv_bridge exception: %s", e.what()); 98 | return; 99 | } 100 | 101 | cv_bridge::CvImageConstPtr cv_ptrD; 102 | try 103 | { 104 | cv_ptrD = cv_bridge::toCvShare(msgD); 105 | } 106 | catch (cv_bridge::Exception& e) 107 | { 108 | ROS_ERROR("cv_bridge exception: %s", e.what()); 109 | return; 110 | } 111 | 112 | mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec()); 113 | } 114 | 115 | 116 | -------------------------------------------------------------------------------- /Examples/ROS/ORB_SLAM2/src/ros_stereo.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | #include"../../../include/System.h" 36 | 37 | using namespace std; 38 | 39 | class ImageGrabber 40 | { 41 | public: 42 | ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){} 43 | 44 | void GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight); 45 | 46 | ORB_SLAM2::System* mpSLAM; 47 | bool do_rectify; 48 | cv::Mat M1l,M2l,M1r,M2r; 49 | }; 50 | 51 | int main(int argc, char **argv) 52 | { 53 | ros::init(argc, argv, "RGBD"); 54 | ros::start(); 55 | 56 | if(argc != 4) 57 | { 58 | cerr << endl << "Usage: rosrun ORB_SLAM2 Stereo path_to_vocabulary path_to_settings do_rectify" << endl; 59 | ros::shutdown(); 60 | return 1; 61 | } 62 | 63 | // Create SLAM system. It initializes all system threads and gets ready to process frames. 64 | ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO,true); 65 | 66 | ImageGrabber igb(&SLAM); 67 | 68 | stringstream ss(argv[3]); 69 | ss >> boolalpha >> igb.do_rectify; 70 | 71 | if(igb.do_rectify) 72 | { 73 | // Load settings related to stereo calibration 74 | cv::FileStorage fsSettings(argv[2], cv::FileStorage::READ); 75 | if(!fsSettings.isOpened()) 76 | { 77 | cerr << "ERROR: Wrong path to settings" << endl; 78 | return -1; 79 | } 80 | 81 | cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r; 82 | fsSettings["LEFT.K"] >> K_l; 83 | fsSettings["RIGHT.K"] >> K_r; 84 | 85 | fsSettings["LEFT.P"] >> P_l; 86 | fsSettings["RIGHT.P"] >> P_r; 87 | 88 | fsSettings["LEFT.R"] >> R_l; 89 | fsSettings["RIGHT.R"] >> R_r; 90 | 91 | fsSettings["LEFT.D"] >> D_l; 92 | fsSettings["RIGHT.D"] >> D_r; 93 | 94 | int rows_l = fsSettings["LEFT.height"]; 95 | int cols_l = fsSettings["LEFT.width"]; 96 | int rows_r = fsSettings["RIGHT.height"]; 97 | int cols_r = fsSettings["RIGHT.width"]; 98 | 99 | if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() || 100 | rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0) 101 | { 102 | cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl; 103 | return -1; 104 | } 105 | 106 | cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,igb.M1l,igb.M2l); 107 | cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,igb.M1r,igb.M2r); 108 | } 109 | 110 | ros::NodeHandle nh; 111 | 112 | message_filters::Subscriber left_sub(nh, "/camera/left/image_raw", 1); 113 | message_filters::Subscriber right_sub(nh, "camera/right/image_raw", 1); 114 | typedef message_filters::sync_policies::ApproximateTime sync_pol; 115 | message_filters::Synchronizer sync(sync_pol(10), left_sub,right_sub); 116 | sync.registerCallback(boost::bind(&ImageGrabber::GrabStereo,&igb,_1,_2)); 117 | 118 | ros::spin(); 119 | 120 | // Stop all threads 121 | SLAM.Shutdown(); 122 | 123 | // Save camera trajectory 124 | SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory_TUM_Format.txt"); 125 | SLAM.SaveTrajectoryTUM("FrameTrajectory_TUM_Format.txt"); 126 | SLAM.SaveTrajectoryKITTI("FrameTrajectory_KITTI_Format.txt"); 127 | 128 | ros::shutdown(); 129 | 130 | return 0; 131 | } 132 | 133 | void ImageGrabber::GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight) 134 | { 135 | // Copy the ros image message to cv::Mat. 136 | cv_bridge::CvImageConstPtr cv_ptrLeft; 137 | try 138 | { 139 | cv_ptrLeft = cv_bridge::toCvShare(msgLeft); 140 | } 141 | catch (cv_bridge::Exception& e) 142 | { 143 | ROS_ERROR("cv_bridge exception: %s", e.what()); 144 | return; 145 | } 146 | 147 | cv_bridge::CvImageConstPtr cv_ptrRight; 148 | try 149 | { 150 | cv_ptrRight = cv_bridge::toCvShare(msgRight); 151 | } 152 | catch (cv_bridge::Exception& e) 153 | { 154 | ROS_ERROR("cv_bridge exception: %s", e.what()); 155 | return; 156 | } 157 | 158 | if(do_rectify) 159 | { 160 | cv::Mat imLeft, imRight; 161 | cv::remap(cv_ptrLeft->image,imLeft,M1l,M2l,cv::INTER_LINEAR); 162 | cv::remap(cv_ptrRight->image,imRight,M1r,M2r,cv::INTER_LINEAR); 163 | mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec()); 164 | } 165 | else 166 | { 167 | mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec()); 168 | } 169 | 170 | } 171 | 172 | 173 | -------------------------------------------------------------------------------- /Examples/Stereo/EuRoC.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 435.2046959714599 9 | Camera.fy: 435.2046959714599 10 | Camera.cx: 367.4517211914062 11 | Camera.cy: 252.2008514404297 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 752 19 | Camera.height: 480 20 | 21 | # Camera frames per second 22 | Camera.fps: 20.0 23 | 24 | # stereo baseline times fx 25 | Camera.bf: 47.90639384423901 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 35 32 | 33 | #-------------------------------------------------------------------------------------------- 34 | # Stereo Rectification. Only if you need to pre-rectify the images. 35 | # Camera.fx, .fy, etc must be the same as in LEFT.P 36 | #-------------------------------------------------------------------------------------------- 37 | LEFT.height: 480 38 | LEFT.width: 752 39 | LEFT.D: !!opencv-matrix 40 | rows: 1 41 | cols: 5 42 | dt: d 43 | data:[-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0] 44 | LEFT.K: !!opencv-matrix 45 | rows: 3 46 | cols: 3 47 | dt: d 48 | data: [458.654, 0.0, 367.215, 0.0, 457.296, 248.375, 0.0, 0.0, 1.0] 49 | LEFT.R: !!opencv-matrix 50 | rows: 3 51 | cols: 3 52 | dt: d 53 | data: [0.999966347530033, -0.001422739138722922, 0.008079580483432283, 0.001365741834644127, 0.9999741760894847, 0.007055629199258132, -0.008089410156878961, -0.007044357138835809, 0.9999424675829176] 54 | LEFT.P: !!opencv-matrix 55 | rows: 3 56 | cols: 4 57 | dt: d 58 | data: [435.2046959714599, 0, 367.4517211914062, 0, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 0] 59 | 60 | RIGHT.height: 480 61 | RIGHT.width: 752 62 | RIGHT.D: !!opencv-matrix 63 | rows: 1 64 | cols: 5 65 | dt: d 66 | data:[-0.28368365, 0.07451284, -0.00010473, -3.555907e-05, 0.0] 67 | RIGHT.K: !!opencv-matrix 68 | rows: 3 69 | cols: 3 70 | dt: d 71 | data: [457.587, 0.0, 379.999, 0.0, 456.134, 255.238, 0.0, 0.0, 1] 72 | RIGHT.R: !!opencv-matrix 73 | rows: 3 74 | cols: 3 75 | dt: d 76 | data: [0.9999633526194376, -0.003625811871560086, 0.007755443660172947, 0.003680398547259526, 0.9999684752771629, -0.007035845251224894, -0.007729688520722713, 0.007064130529506649, 0.999945173484644] 77 | RIGHT.P: !!opencv-matrix 78 | rows: 3 79 | cols: 4 80 | dt: d 81 | data: [435.2046959714599, 0, 367.4517211914062, -47.90639384423901, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 0] 82 | 83 | #-------------------------------------------------------------------------------------------- 84 | # ORB Parameters 85 | #-------------------------------------------------------------------------------------------- 86 | 87 | # ORB Extractor: Number of features per image 88 | ORBextractor.nFeatures: 1200 89 | 90 | # ORB Extractor: Scale factor between levels in the scale pyramid 91 | ORBextractor.scaleFactor: 1.2 92 | 93 | # ORB Extractor: Number of levels in the scale pyramid 94 | ORBextractor.nLevels: 8 95 | 96 | # ORB Extractor: Fast threshold 97 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 98 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 99 | # You can lower these values if your images have low contrast 100 | ORBextractor.iniThFAST: 20 101 | ORBextractor.minThFAST: 7 102 | 103 | #-------------------------------------------------------------------------------------------- 104 | # Viewer Parameters 105 | #-------------------------------------------------------------------------------------------- 106 | Viewer.KeyFrameSize: 0.05 107 | Viewer.KeyFrameLineWidth: 1 108 | Viewer.GraphLineWidth: 0.9 109 | Viewer.PointSize:2 110 | Viewer.CameraSize: 0.08 111 | Viewer.CameraLineWidth: 3 112 | Viewer.ViewpointX: 0 113 | Viewer.ViewpointY: -0.7 114 | Viewer.ViewpointZ: -1.8 115 | Viewer.ViewpointF: 500 116 | 117 | -------------------------------------------------------------------------------- /Examples/Stereo/KITTI00-02.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 718.856 9 | Camera.fy: 718.856 10 | Camera.cx: 607.1928 11 | Camera.cy: 185.2157 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 1241 19 | Camera.height: 376 20 | 21 | # Camera frames per second 22 | Camera.fps: 10.0 23 | 24 | # stereo baseline times fx 25 | Camera.bf: 386.1448 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 35 32 | 33 | #-------------------------------------------------------------------------------------------- 34 | # ORB Parameters 35 | #-------------------------------------------------------------------------------------------- 36 | 37 | # ORB Extractor: Number of features per image 38 | ORBextractor.nFeatures: 2000 39 | 40 | # ORB Extractor: Scale factor between levels in the scale pyramid 41 | ORBextractor.scaleFactor: 1.2 42 | 43 | # ORB Extractor: Number of levels in the scale pyramid 44 | ORBextractor.nLevels: 8 45 | 46 | # ORB Extractor: Fast threshold 47 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 48 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 49 | # You can lower these values if your images have low contrast 50 | ORBextractor.iniThFAST: 20 51 | ORBextractor.minThFAST: 7 52 | 53 | #-------------------------------------------------------------------------------------------- 54 | # Viewer Parameters 55 | #-------------------------------------------------------------------------------------------- 56 | Viewer.KeyFrameSize: 0.6 57 | Viewer.KeyFrameLineWidth: 2 58 | Viewer.GraphLineWidth: 1 59 | Viewer.PointSize:2 60 | Viewer.CameraSize: 0.7 61 | Viewer.CameraLineWidth: 3 62 | Viewer.ViewpointX: 0 63 | Viewer.ViewpointY: -100 64 | Viewer.ViewpointZ: -0.1 65 | Viewer.ViewpointF: 2000 66 | 67 | -------------------------------------------------------------------------------- /Examples/Stereo/KITTI03.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 721.5377 9 | Camera.fy: 721.5377 10 | Camera.cx: 609.5593 11 | Camera.cy: 172.854 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 1241 19 | Camera.height: 376 20 | 21 | # Camera frames per second 22 | Camera.fps: 10.0 23 | 24 | # stereo baseline times fx 25 | Camera.bf: 387.5744 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 40 32 | 33 | #-------------------------------------------------------------------------------------------- 34 | # ORB Parameters 35 | #-------------------------------------------------------------------------------------------- 36 | 37 | # ORB Extractor: Number of features per image 38 | ORBextractor.nFeatures: 2000 39 | 40 | # ORB Extractor: Scale factor between levels in the scale pyramid 41 | ORBextractor.scaleFactor: 1.2 42 | 43 | # ORB Extractor: Number of levels in the scale pyramid 44 | ORBextractor.nLevels: 8 45 | 46 | # ORB Extractor: Fast threshold 47 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 48 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 49 | # You can lower these values if your images have low contrast 50 | ORBextractor.iniThFAST: 20 51 | ORBextractor.minThFAST: 7 52 | 53 | #-------------------------------------------------------------------------------------------- 54 | # Viewer Parameters 55 | #-------------------------------------------------------------------------------------------- 56 | Viewer.KeyFrameSize: 0.6 57 | Viewer.KeyFrameLineWidth: 2 58 | Viewer.GraphLineWidth: 1 59 | Viewer.PointSize:2 60 | Viewer.CameraSize: 0.7 61 | Viewer.CameraLineWidth: 3 62 | Viewer.ViewpointX: 0 63 | Viewer.ViewpointY: -100 64 | Viewer.ViewpointZ: -0.1 65 | Viewer.ViewpointF: 2000 66 | 67 | -------------------------------------------------------------------------------- /Examples/Stereo/KITTI04-12.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 707.0912 9 | Camera.fy: 707.0912 10 | Camera.cx: 601.8873 11 | Camera.cy: 183.1104 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 1241 19 | Camera.height: 376 20 | 21 | # Camera frames per second 22 | Camera.fps: 10.0 23 | 24 | # stereo baseline times fx 25 | Camera.bf: 379.8145 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 40 32 | 33 | #-------------------------------------------------------------------------------------------- 34 | # ORB Parameters 35 | #-------------------------------------------------------------------------------------------- 36 | 37 | # ORB Extractor: Number of features per image 38 | ORBextractor.nFeatures: 2000 39 | 40 | # ORB Extractor: Scale factor between levels in the scale pyramid 41 | ORBextractor.scaleFactor: 1.2 42 | 43 | # ORB Extractor: Number of levels in the scale pyramid 44 | ORBextractor.nLevels: 8 45 | 46 | # ORB Extractor: Fast threshold 47 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 48 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 49 | # You can lower these values if your images have low contrast 50 | ORBextractor.iniThFAST: 12 51 | ORBextractor.minThFAST: 7 52 | 53 | #-------------------------------------------------------------------------------------------- 54 | # Viewer Parameters 55 | #-------------------------------------------------------------------------------------------- 56 | Viewer.KeyFrameSize: 0.6 57 | Viewer.KeyFrameLineWidth: 2 58 | Viewer.GraphLineWidth: 1 59 | Viewer.PointSize:2 60 | Viewer.CameraSize: 0.7 61 | Viewer.CameraLineWidth: 3 62 | Viewer.ViewpointX: 0 63 | Viewer.ViewpointY: -100 64 | Viewer.ViewpointZ: -0.1 65 | Viewer.ViewpointF: 2000 66 | 67 | -------------------------------------------------------------------------------- /Examples/Stereo/stereo_euroc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yangliu9527/Object_SLAM/0154923286a9b5fb4b61032b3341e77d4ec883cf/Examples/Stereo/stereo_euroc -------------------------------------------------------------------------------- /Examples/Stereo/stereo_euroc.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | #include 32 | 33 | using namespace std; 34 | 35 | void LoadImages(const string &strPathLeft, const string &strPathRight, const string &strPathTimes, 36 | vector &vstrImageLeft, vector &vstrImageRight, vector &vTimeStamps); 37 | 38 | int main(int argc, char **argv) 39 | { 40 | if(argc != 6) 41 | { 42 | cerr << endl << "Usage: ./stereo_euroc path_to_vocabulary path_to_settings path_to_left_folder path_to_right_folder path_to_times_file" << endl; 43 | return 1; 44 | } 45 | 46 | // Retrieve paths to images 47 | vector vstrImageLeft; 48 | vector vstrImageRight; 49 | vector vTimeStamp; 50 | LoadImages(string(argv[3]), string(argv[4]), string(argv[5]), vstrImageLeft, vstrImageRight, vTimeStamp); 51 | 52 | if(vstrImageLeft.empty() || vstrImageRight.empty()) 53 | { 54 | cerr << "ERROR: No images in provided path." << endl; 55 | return 1; 56 | } 57 | 58 | if(vstrImageLeft.size()!=vstrImageRight.size()) 59 | { 60 | cerr << "ERROR: Different number of left and right images." << endl; 61 | return 1; 62 | } 63 | 64 | // Read rectification parameters 65 | cv::FileStorage fsSettings(argv[2], cv::FileStorage::READ); 66 | if(!fsSettings.isOpened()) 67 | { 68 | cerr << "ERROR: Wrong path to settings" << endl; 69 | return -1; 70 | } 71 | 72 | cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r; 73 | fsSettings["LEFT.K"] >> K_l; 74 | fsSettings["RIGHT.K"] >> K_r; 75 | 76 | fsSettings["LEFT.P"] >> P_l; 77 | fsSettings["RIGHT.P"] >> P_r; 78 | 79 | fsSettings["LEFT.R"] >> R_l; 80 | fsSettings["RIGHT.R"] >> R_r; 81 | 82 | fsSettings["LEFT.D"] >> D_l; 83 | fsSettings["RIGHT.D"] >> D_r; 84 | 85 | int rows_l = fsSettings["LEFT.height"]; 86 | int cols_l = fsSettings["LEFT.width"]; 87 | int rows_r = fsSettings["RIGHT.height"]; 88 | int cols_r = fsSettings["RIGHT.width"]; 89 | 90 | if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() || 91 | rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0) 92 | { 93 | cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl; 94 | return -1; 95 | } 96 | 97 | cv::Mat M1l,M2l,M1r,M2r; 98 | cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l); 99 | cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r); 100 | 101 | 102 | const int nImages = vstrImageLeft.size(); 103 | 104 | // Create SLAM system. It initializes all system threads and gets ready to process frames. 105 | ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO,true); 106 | 107 | // Vector for tracking time statistics 108 | vector vTimesTrack; 109 | vTimesTrack.resize(nImages); 110 | 111 | cout << endl << "-------" << endl; 112 | cout << "Start processing sequence ..." << endl; 113 | cout << "Images in the sequence: " << nImages << endl << endl; 114 | 115 | // Main loop 116 | cv::Mat imLeft, imRight, imLeftRect, imRightRect; 117 | for(int ni=0; ni >(t2 - t1).count(); 159 | 160 | vTimesTrack[ni]=ttrack; 161 | 162 | // Wait to load the next frame 163 | double T=0; 164 | if(ni0) 167 | T = tframe-vTimeStamp[ni-1]; 168 | 169 | if(ttrack &vstrImageLeft, vector &vstrImageRight, vector &vTimeStamps) 195 | { 196 | ifstream fTimes; 197 | fTimes.open(strPathTimes.c_str()); 198 | vTimeStamps.reserve(5000); 199 | vstrImageLeft.reserve(5000); 200 | vstrImageRight.reserve(5000); 201 | while(!fTimes.eof()) 202 | { 203 | string s; 204 | getline(fTimes,s); 205 | if(!s.empty()) 206 | { 207 | stringstream ss; 208 | ss << s; 209 | vstrImageLeft.push_back(strPathLeft + "/" + ss.str() + ".png"); 210 | vstrImageRight.push_back(strPathRight + "/" + ss.str() + ".png"); 211 | double t; 212 | ss >> t; 213 | vTimeStamps.push_back(t/1e9); 214 | 215 | } 216 | } 217 | } 218 | -------------------------------------------------------------------------------- /Examples/Stereo/stereo_kitti: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yangliu9527/Object_SLAM/0154923286a9b5fb4b61032b3341e77d4ec883cf/Examples/Stereo/stereo_kitti -------------------------------------------------------------------------------- /Examples/Stereo/stereo_kitti.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | #include 32 | 33 | using namespace std; 34 | 35 | void LoadImages(const string &strPathToSequence, vector &vstrImageLeft, 36 | vector &vstrImageRight, vector &vTimestamps); 37 | 38 | int main(int argc, char **argv) 39 | { 40 | if(argc != 4) 41 | { 42 | cerr << endl << "Usage: ./stereo_kitti path_to_vocabulary path_to_settings path_to_sequence" << endl; 43 | return 1; 44 | } 45 | 46 | // Retrieve paths to images 47 | vector vstrImageLeft; 48 | vector vstrImageRight; 49 | vector vTimestamps; 50 | LoadImages(string(argv[3]), vstrImageLeft, vstrImageRight, vTimestamps); 51 | 52 | const int nImages = vstrImageLeft.size(); 53 | 54 | // Create SLAM system. It initializes all system threads and gets ready to process frames. 55 | ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO,true); 56 | 57 | // Vector for tracking time statistics 58 | vector vTimesTrack; 59 | vTimesTrack.resize(nImages); 60 | 61 | cout << endl << "-------" << endl; 62 | cout << "Start processing sequence ..." << endl; 63 | cout << "Images in the sequence: " << nImages << endl << endl; 64 | 65 | // Main loop 66 | cv::Mat imLeft, imRight; 67 | for(int ni=0; ni >(t2 - t1).count(); 97 | 98 | vTimesTrack[ni]=ttrack; 99 | 100 | // Wait to load the next frame 101 | double T=0; 102 | if(ni0) 105 | T = tframe-vTimestamps[ni-1]; 106 | 107 | if(ttrack &vstrImageLeft, 133 | vector &vstrImageRight, vector &vTimestamps) 134 | { 135 | ifstream fTimes; 136 | string strPathTimeFile = strPathToSequence + "/times.txt"; 137 | fTimes.open(strPathTimeFile.c_str()); 138 | while(!fTimes.eof()) 139 | { 140 | string s; 141 | getline(fTimes,s); 142 | if(!s.empty()) 143 | { 144 | stringstream ss; 145 | ss << s; 146 | double t; 147 | ss >> t; 148 | vTimestamps.push_back(t); 149 | } 150 | } 151 | 152 | string strPrefixLeft = strPathToSequence + "/image_2/"; 153 | string strPrefixRight = strPathToSequence + "/image_3/"; 154 | 155 | const int nTimes = vTimestamps.size(); 156 | vstrImageLeft.resize(nTimes); 157 | vstrImageRight.resize(nTimes); 158 | 159 | for(int i=0; i0 and line[0]!="#"] 68 | list = [(float(l[0]),l[1:]) for l in list if len(l)>1] 69 | return dict(list) 70 | 71 | def associate(first_list, second_list,offset,max_difference): 72 | """ 73 | Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim 74 | to find the closest match for every input tuple. 75 | 76 | Input: 77 | first_list -- first dictionary of (stamp,data) tuples 78 | second_list -- second dictionary of (stamp,data) tuples 79 | offset -- time offset between both dictionaries (e.g., to model the delay between the sensors) 80 | max_difference -- search radius for candidate generation 81 | 82 | Output: 83 | matches -- list of matched tuples ((stamp1,data1),(stamp2,data2)) 84 | 85 | """ 86 | first_keys = first_list.keys() 87 | second_keys = second_list.keys() 88 | potential_matches = [(abs(a - (b + offset)), a, b) 89 | for a in first_keys 90 | for b in second_keys 91 | if abs(a - (b + offset)) < max_difference] 92 | potential_matches.sort() 93 | matches = [] 94 | for diff, a, b in potential_matches: 95 | if a in first_keys and b in second_keys: 96 | first_keys.remove(a) 97 | second_keys.remove(b) 98 | matches.append((a, b)) 99 | 100 | matches.sort() 101 | return matches 102 | 103 | if __name__ == '__main__': 104 | 105 | # parse command line 106 | parser = argparse.ArgumentParser(description=''' 107 | This script takes two data files with timestamps and associates them 108 | ''') 109 | parser.add_argument('first_file', help='first text file (format: timestamp data)') 110 | parser.add_argument('second_file', help='second text file (format: timestamp data)') 111 | parser.add_argument('--first_only', help='only output associated lines from first file', action='store_true') 112 | parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0) 113 | parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02) 114 | args = parser.parse_args() 115 | 116 | first_list = read_file_list(args.first_file) 117 | second_list = read_file_list(args.second_file) 118 | 119 | matches = associate(first_list, second_list,float(args.offset),float(args.max_difference)) 120 | 121 | if args.first_only: 122 | for a,b in matches: 123 | print("%f %s"%(a," ".join(first_list[a]))) 124 | else: 125 | for a,b in matches: 126 | print("%f %s %f %s"%(a," ".join(first_list[a]),b-float(args.offset)," ".join(second_list[b]))) 127 | 128 | 129 | -------------------------------------------------------------------------------- /ExpResults/TUM/Localization/associate.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yangliu9527/Object_SLAM/0154923286a9b5fb4b61032b3341e77d4ec883cf/ExpResults/TUM/Localization/associate.pyc -------------------------------------------------------------------------------- /ExpResults/TUM/Localization/plot_trajectory_into_image.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2013, Juergen Sturm, TUM 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of TUM nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | # the resulting .ply file can be viewed for example with meshlab 35 | # sudo apt-get install meshlab 36 | 37 | """ 38 | This script plots a trajectory into an image sequence. 39 | """ 40 | 41 | import argparse 42 | import sys 43 | import os 44 | from associate import * 45 | from evaluate import * 46 | from generate_pointcloud import * 47 | from PIL import Image, ImageDraw 48 | 49 | focalLength = 525.0 50 | centerX = 319.5 51 | centerY = 239.5 52 | 53 | def point(pose,px,py,pz): 54 | """ 55 | Project a 3D point into the camera. 56 | 57 | Input: 58 | pose -- camera pose 59 | px,py,pz -- point in global frame 60 | 61 | Output: 62 | u,v -- pixel coordinates 63 | 64 | """ 65 | p = pose.dot(numpy.matrix([[px],[py],[pz],[1]])) 66 | X = p[0,0] 67 | Y = p[1,0] 68 | Z = p[2,0] 69 | u = X/Z * focalLength + centerX 70 | v = Y/Z * focalLength + centerY 71 | return [u,v] 72 | 73 | 74 | if __name__ == '__main__': 75 | parser = argparse.ArgumentParser(description=''' 76 | This script plots a trajectory into an image sequence. 77 | ''') 78 | parser.add_argument('image_list', help='input image list (format: timestamp filename)') 79 | parser.add_argument('trajectory_file', help='input trajectory (format: timestamp tx ty tz qx qy qz qw)') 80 | parser.add_argument('out_image', help='file name of the result (format: png)') 81 | args = parser.parse_args() 82 | 83 | image_list = read_file_list(args.image_list) 84 | pose_list = read_file_list(args.trajectory_file) 85 | traj = read_trajectory(args.trajectory_file) 86 | 87 | matches = associate(image_list, pose_list,0,0.02) 88 | 89 | stamps = image_list.keys() 90 | stamps.sort() 91 | 92 | matches_dict = dict(matches) 93 | for stamp in stamps: 94 | image_file = image_list[stamp][0] 95 | image = Image.open(image_file) 96 | print "image stamp: %f"%stamp 97 | 98 | if stamp in matches_dict: 99 | print "pose stamp: %f"%matches_dict[stamp] 100 | pose = traj[matches_dict[stamp]] 101 | 102 | stamps = traj.keys() 103 | stamps.sort() 104 | 105 | xy = [] 106 | draw = ImageDraw.Draw(image) 107 | size = 0.01 108 | 109 | for s in stamps: 110 | p = traj[s] 111 | rel_pose = numpy.dot(numpy.linalg.inv(pose),p) 112 | if rel_pose[2,3]<0.01: continue 113 | u,v = point(rel_pose,0,0,0) 114 | if u<0 or v<0 or u>640 or v>480: continue 115 | draw.line(point(rel_pose,0,0,0) + point(rel_pose,size,0,0), fill="#ff0000") 116 | draw.line(point(rel_pose,0,0,0) + point(rel_pose,0,size,0), fill="#00ff00") 117 | draw.line(point(rel_pose,0,0,0) + point(rel_pose,0,0,size), fill="#0000ff") 118 | del draw 119 | 120 | image.save(os.path.splitext(args.out_image)[0]+"-%f.png"%stamp) 121 | 122 | 123 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | ObjectSLAM is released under a GPLv3 license (see License-gpl.txt). 2 | Please see Dependencies.md for a list of all included code and library dependencies which are not property of the authors of ObjectDataAssociation. 3 | 4 | For a closed-source version of ObjectSLAM for commercial purposes, please contact the authors. 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Object-aware data association for the semantically constrained visual SLAM 2 | **Authors:** Liu Yang 3 | 4 | This is an object-level semantic visual system, which is based on [ORB_SLAM2](https://github.com/raulmur/ORB_SLAM2) and supports RGB-D and Stereo modes. We are aimed to solve two problems: the object-level data association and the semantically constrained pose optimization. The code is an open source version implementation of our work. 5 | 6 | ``` 7 | @article{liu2023object, 8 | title={Object-aware data association for the semantically constrained visual SLAM}, 9 | author={Liu, Yang and Guo, Chi and Wang, Yingli}, 10 | journal={Intelligent Service Robotics}, 11 | volume={16}, 12 | number={2}, 13 | pages={155--176}, 14 | year={2023}, 15 | publisher={Springer} 16 | } 17 | ``` 18 | 19 | ![image](https://github.com/yangliu9527/Object_SLAM/blob/master/example_picutures/ObjectMappingKITTI1.gif) 20 | 21 | ![image](https://github.com/yangliu9527/Object_SLAM/blob/master/example_picutures/ObjectMappingTUM_fr2_desk.gif) 22 | 23 | # 1. Prerequisites 24 | We have tested the library in **Ubuntu 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. 25 | 26 | ## C++11 or C++0x Compiler 27 | We use the new thread and chrono functionalities of C++11. 28 | 29 | ## Pangolin 30 | 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. 31 | 32 | ## OpenCV 33 | 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 2.4.3. Tested with OpenCV 2.4.11 and OpenCV 3.2**. 34 | 35 | ## Eigen3 36 | Required by g2o (see below). Download and install instructions can be found at: http://eigen.tuxfamily.org. **Required at least 3.1.0**. 37 | 38 | ## DBoW2 and g2o (Included in Thirdparty folder) 39 | 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. 40 | 41 | ## PCL 42 | 43 | We use PCL libraries to process point cloud. Dowload and install instructions can be found at: https://github.com/PointCloudLibrary/pcl and https://pointclouds.org/. We used command `sudo apt-get install libpcl-dev` in Ubuntu18.04 to install PCL. 44 | 45 | # 2. Building ObjectSLAM 46 | 47 | Clone the repository: 48 | ``` 49 | git clone https://github.com/yangliu9527/object_slam 50 | ``` 51 | There are some thirdparty files we can not upload due to the size. Users can copy them from [ORB_SLAM2](https://github.com/raulmur/ORB_SLAM2). Two directories need to be copy: Thirdparty and Vocabulary. 52 | 53 | 54 | We provide a script `build.sh` to build the *Thirdparty* libraries and *ObjectSLAM*. Please make sure you have installed all required dependencies (see section 2). Execute: 55 | ``` 56 | cd object_slam 57 | chmod +x build.sh 58 | ./build.sh 59 | ``` 60 | 61 | This will create **libORB_SLAM2.so** at *lib* folder and the executables **rgbd_tum**, **stereo_kitti** in *Examples* folder. 62 | 63 | # 3. Prepare Datasets 64 | The system supports TUM RGB-D and KITTI-Odometry datasets. The interfaces are the same as ORB_SLAM2. In addition, our system needs semantic features extracted from the original images. We use [YOLACT](https://github.com/dbolya/yolact) to extract instance segmentation results offline and feed the results with image frames. We provide a demo sequence in BaiduDisk： 65 | 66 | link：https://pan.baidu.com/s/1arvRnTaDZe1bgWENx4_Cdg 67 | 68 | code：lud0 69 | 70 | # 4. Run System 71 | 72 | We provide two python scripts to run the system on different sequences. Please refer to run_exp_tum.py and run_exp_kitti.py for running commands. 73 | 74 | It should be noted that the dataset path and the threshold of semantic score should be added into the .yaml file. 75 | 76 | -------------------------------------------------------------------------------- /build.sh: -------------------------------------------------------------------------------- 1 | echo "Configuring and building Thirdparty/DBoW2 ..." 2 | 3 | cd Thirdparty/DBoW2 4 | mkdir build 5 | cd build 6 | cmake .. -DCMAKE_BUILD_TYPE=Release 7 | make -j14 8 | 9 | cd ../../g2o 10 | 11 | echo "Configuring and building Thirdparty/g2o ..." 12 | 13 | mkdir build 14 | cd build 15 | cmake .. -DCMAKE_BUILD_TYPE=Release 16 | make -j14 17 | 18 | cd ../../../ 19 | 20 | echo "Uncompress vocabulary ..." 21 | 22 | cd Vocabulary 23 | tar -xf ORBvoc.txt.tar.gz 24 | cd .. 25 | 26 | echo "Configuring and building ORB_SLAM2 ..." 27 | 28 | mkdir build 29 | cd build 30 | cmake .. -DCMAKE_BUILD_TYPE=Release 31 | make -j14 32 | -------------------------------------------------------------------------------- /example_picutures/ObjectMappingKITTI1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yangliu9527/Object_SLAM/0154923286a9b5fb4b61032b3341e77d4ec883cf/example_picutures/ObjectMappingKITTI1.gif -------------------------------------------------------------------------------- /example_picutures/ObjectMappingTUM_fr2_desk.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yangliu9527/Object_SLAM/0154923286a9b5fb4b61032b3341e77d4ec883cf/example_picutures/ObjectMappingTUM_fr2_desk.gif -------------------------------------------------------------------------------- /example_picutures/notes.txt: -------------------------------------------------------------------------------- 1 | Some demo pictures of the program 2 | -------------------------------------------------------------------------------- /include/Converter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef CONVERTER_H 22 | #define CONVERTER_H 23 | 24 | #include 25 | 26 | #include 27 | #include"Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" 28 | #include"Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" 29 | 30 | namespace ORB_SLAM2 31 | { 32 | 33 | class Converter 34 | { 35 | public: 36 | static std::vector toDescriptorVector(const cv::Mat &Descriptors); 37 | 38 | static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT); 39 | static g2o::SE3Quat toSE3Quat(const g2o::Sim3 &gSim3); 40 | 41 | static cv::Mat toCvMat(const g2o::SE3Quat &SE3); 42 | static cv::Mat toCvMat(const g2o::Sim3 &Sim3); 43 | static cv::Mat toCvMat(const Eigen::Matrix &m); 44 | static cv::Mat toCvMat(const Eigen::Matrix3d &m); 45 | static cv::Mat toCvMat(const Eigen::Matrix &m); 46 | static cv::Mat toCvSE3(const Eigen::Matrix &R, const Eigen::Matrix &t); 47 | 48 | static Eigen::Matrix toVector3d(const cv::Mat &cvVector); 49 | static Eigen::Matrix toVector3d(const cv::Point3f &cvPoint); 50 | static Eigen::Matrix toMatrix3d(const cv::Mat &cvMat3); 51 | 52 | static cv::Mat toCvMat(const Eigen::Matrix &Mat4); 53 | static cv::Mat toCvMat(const Eigen::Matrix &Vec3); 54 | static cv::Mat toCvMat(const Eigen::Matrix &Mat3); 55 | static Eigen::Vector3f toEigenVec3(const cv::Mat &Vec3f); 56 | static Eigen::Matrix toEigenMat3(const cv::Mat &Mat3f); 57 | static Eigen::Matrix toEigenMat4(const cv::Mat &Mat4f); 58 | 59 | 60 | 61 | static std::vector toQuaternion(const cv::Mat &M); 62 | 63 | 64 | }; 65 | 66 | }// namespace ORB_SLAM 67 | 68 | #endif // CONVERTER_H 69 | -------------------------------------------------------------------------------- /include/FrameDrawer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef FRAMEDRAWER_H 22 | #define FRAMEDRAWER_H 23 | 24 | #include "Tracking.h" 25 | #include "MapPoint.h" 26 | #include "Map.h" 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | 33 | /////new codes//// 34 | #include "ObjectTypes.h" 35 | 36 | 37 | namespace ORB_SLAM2 38 | { 39 | 40 | class Tracking; 41 | class Viewer; 42 | 43 | class FrameDrawer 44 | { 45 | public: 46 | FrameDrawer(Map* pMap); 47 | 48 | // Update info from the last processed frame. 49 | void Update(Tracking *pTracker); 50 | 51 | // Draw last processed frame. 52 | cv::Mat DrawFrame(); 53 | 54 | //////new codes////// 55 | vector mvCurrentObject2Ds; 56 | void DrawCurrentObject2Ds(cv::Mat &im); 57 | 58 | protected: 59 | 60 | void DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText); 61 | 62 | // Info of the frame to be drawn 63 | cv::Mat mIm; 64 | int N; 65 | vector mvCurrentKeys; 66 | vector mvbMap, mvbVO; 67 | bool mbOnlyTracking; 68 | int mnTracked, mnTrackedVO; 69 | vector mvIniKeys; 70 | vector mvIniMatches; 71 | int mState; 72 | 73 | Map* mpMap; 74 | 75 | std::mutex mMutex; 76 | }; 77 | 78 | } //namespace ORB_SLAM 79 | 80 | #endif // FRAMEDRAWER_H 81 | -------------------------------------------------------------------------------- /include/Initializer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | #ifndef INITIALIZER_H 21 | #define INITIALIZER_H 22 | 23 | #include 24 | #include "Frame.h" 25 | 26 | 27 | namespace ORB_SLAM2 28 | { 29 | 30 | // THIS IS THE INITIALIZER FOR MONOCULAR SLAM. NOT USED IN THE STEREO OR RGBD CASE. 31 | class Initializer 32 | { 33 | typedef pair Match; 34 | 35 | public: 36 | 37 | // Fix the reference frame 38 | Initializer(const Frame &ReferenceFrame, float sigma = 1.0, int iterations = 200); 39 | 40 | // Computes in parallel a fundamental matrix and a homography 41 | // Selects a model and tries to recover the motion and the structure from motion 42 | bool Initialize(const Frame &CurrentFrame, const vector &vMatches12, 43 | cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated); 44 | 45 | 46 | private: 47 | 48 | void FindHomography(vector &vbMatchesInliers, float &score, cv::Mat &H21); 49 | void FindFundamental(vector &vbInliers, float &score, cv::Mat &F21); 50 | 51 | cv::Mat ComputeH21(const vector &vP1, const vector &vP2); 52 | cv::Mat ComputeF21(const vector &vP1, const vector &vP2); 53 | 54 | float CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector &vbMatchesInliers, float sigma); 55 | 56 | float CheckFundamental(const cv::Mat &F21, vector &vbMatchesInliers, float sigma); 57 | 58 | bool ReconstructF(vector &vbMatchesInliers, cv::Mat &F21, cv::Mat &K, 59 | cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated); 60 | 61 | bool ReconstructH(vector &vbMatchesInliers, cv::Mat &H21, cv::Mat &K, 62 | cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated); 63 | 64 | void Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D); 65 | 66 | void Normalize(const vector &vKeys, vector &vNormalizedPoints, cv::Mat &T); 67 | 68 | int CheckRT(const cv::Mat &R, const cv::Mat &t, const vector &vKeys1, const vector &vKeys2, 69 | const vector &vMatches12, vector &vbInliers, 70 | const cv::Mat &K, vector &vP3D, float th2, vector &vbGood, float ¶llax); 71 | 72 | void DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t); 73 | 74 | 75 | // Keypoints from Reference Frame (Frame 1) 76 | vector mvKeys1; 77 | 78 | // Keypoints from Current Frame (Frame 2) 79 | vector mvKeys2; 80 | 81 | // Current Matches from Reference to Current 82 | vector mvMatches12; 83 | vector mvbMatched1; 84 | 85 | // Calibration 86 | cv::Mat mK; 87 | 88 | // Standard Deviation and Variance 89 | float mSigma, mSigma2; 90 | 91 | // Ransac max iterations 92 | int mMaxIterations; 93 | 94 | // Ransac sets 95 | vector > mvSets; 96 | 97 | }; 98 | 99 | } //namespace ORB_SLAM 100 | 101 | #endif // INITIALIZER_H 102 | -------------------------------------------------------------------------------- /include/KeyFrame.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef KEYFRAME_H 22 | #define KEYFRAME_H 23 | 24 | #include "MapPoint.h" 25 | #include "Thirdparty/DBoW2/DBoW2/BowVector.h" 26 | #include "Thirdparty/DBoW2/DBoW2/FeatureVector.h" 27 | #include "ORBVocabulary.h" 28 | #include "ORBextractor.h" 29 | #include "Frame.h" 30 | #include "KeyFrameDatabase.h" 31 | #include "ObjectTypes.h" 32 | 33 | #include 34 | 35 | 36 | namespace ORB_SLAM2 37 | { 38 | 39 | class Map; 40 | class MapPoint; 41 | class Frame; 42 | class KeyFrameDatabase; 43 | class Object2D; 44 | class Object3D; 45 | 46 | class KeyFrame 47 | { 48 | public: 49 | KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB); 50 | 51 | //new codes// 52 | vector mvObject2Ds; 53 | vector mvpObject3Ds; 54 | //Number of Object2Ds 55 | int N_O; 56 | //KeyPoint index in object: [Object2DIndex, KpIndex in Object2D] 57 | vector> mvObjectKpIndices; 58 | 59 | // Pose functions 60 | void SetPose(const cv::Mat &Tcw); 61 | cv::Mat GetPose(); 62 | cv::Mat GetPoseInverse(); 63 | cv::Mat GetCameraCenter(); 64 | cv::Mat GetStereoCenter(); 65 | cv::Mat GetRotation(); 66 | cv::Mat GetTranslation(); 67 | 68 | // Bag of Words Representation 69 | void ComputeBoW(); 70 | 71 | // Covisibility graph functions 72 | void AddConnection(KeyFrame* pKF, const int &weight); 73 | void EraseConnection(KeyFrame* pKF); 74 | void UpdateConnections(); 75 | void UpdateBestCovisibles(); 76 | std::set GetConnectedKeyFrames(); 77 | std::vector GetVectorCovisibleKeyFrames(); 78 | std::vector GetBestCovisibilityKeyFrames(const int &N); 79 | std::vector GetCovisiblesByWeight(const int &w); 80 | int GetWeight(KeyFrame* pKF); 81 | 82 | // Spanning tree functions 83 | void AddChild(KeyFrame* pKF); 84 | void EraseChild(KeyFrame* pKF); 85 | void ChangeParent(KeyFrame* pKF); 86 | std::set GetChilds(); 87 | KeyFrame* GetParent(); 88 | bool hasChild(KeyFrame* pKF); 89 | 90 | // Loop Edges 91 | void AddLoopEdge(KeyFrame* pKF); 92 | std::set GetLoopEdges(); 93 | 94 | // MapPoint observation functions 95 | void AddMapPoint(MapPoint* pMP, const size_t &idx); 96 | void EraseMapPointMatch(const size_t &idx); 97 | void EraseMapPointMatch(MapPoint* pMP); 98 | void ReplaceMapPointMatch(const size_t &idx, MapPoint* pMP); 99 | std::set GetMapPoints(); 100 | std::vector GetMapPointMatches(); 101 | int TrackedMapPoints(const int &minObs); 102 | MapPoint* GetMapPoint(const size_t &idx); 103 | 104 | // KeyPoint functions 105 | std::vector GetFeaturesInArea(const float &x, const float &y, const float &r) const; 106 | cv::Mat UnprojectStereo(int i); 107 | 108 | // Image 109 | bool IsInImage(const float &x, const float &y) const; 110 | 111 | // Enable/Disable bad flag changes 112 | void SetNotErase(); 113 | void SetErase(); 114 | 115 | // Set/check bad flag 116 | void SetBadFlag(); 117 | bool isBad(); 118 | 119 | // Compute Scene Depth (q=2 median). Used in monocular. 120 | float ComputeSceneMedianDepth(const int q); 121 | 122 | static bool weightComp( int a, int b){ 123 | return a>b; 124 | } 125 | 126 | static bool lId(KeyFrame* pKF1, KeyFrame* pKF2){ 127 | return pKF1->mnIdmnId; 128 | } 129 | 130 | 131 | // The following variables are accesed from only 1 thread or never change (no mutex needed). 132 | public: 133 | 134 | static long unsigned int nNextId; 135 | long unsigned int mnId; 136 | const long unsigned int mnFrameId; 137 | 138 | const double mTimeStamp; 139 | 140 | // Grid (to speed up feature matching) 141 | const int mnGridCols; 142 | const int mnGridRows; 143 | const float mfGridElementWidthInv; 144 | const float mfGridElementHeightInv; 145 | 146 | // Variables used by the tracking 147 | long unsigned int mnTrackReferenceForFrame; 148 | long unsigned int mnFuseTargetForKF; 149 | 150 | // Variables used by the local mapping 151 | long unsigned int mnBALocalForKF; 152 | long unsigned int mnBAFixedForKF; 153 | 154 | // Variables used by the keyframe database 155 | long unsigned int mnLoopQuery; 156 | int mnLoopWords; 157 | float mLoopScore; 158 | long unsigned int mnRelocQuery; 159 | int mnRelocWords; 160 | float mRelocScore; 161 | 162 | // Variables used by loop closing 163 | cv::Mat mTcwGBA; 164 | cv::Mat mTcwBefGBA; 165 | long unsigned int mnBAGlobalForKF; 166 | 167 | // Calibration parameters 168 | const float fx, fy, cx, cy, invfx, invfy, mbf, mb, mThDepth; 169 | 170 | // Number of KeyPoints 171 | const int N; 172 | 173 | // KeyPoints, stereo coordinate and descriptors (all associated by an index) 174 | const std::vector mvKeys; 175 | const std::vector mvKeysUn; 176 | const std::vector mvuRight; // negative value for monocular points 177 | const std::vector mvDepth; // negative value for monocular points 178 | const cv::Mat mDescriptors; 179 | 180 | //BoW 181 | DBoW2::BowVector mBowVec; 182 | DBoW2::FeatureVector mFeatVec; 183 | 184 | // Pose relative to parent (this is computed when bad flag is activated) 185 | cv::Mat mTcp; 186 | 187 | // Scale 188 | const int mnScaleLevels; 189 | const float mfScaleFactor; 190 | const float mfLogScaleFactor; 191 | const std::vector mvScaleFactors; 192 | const std::vector mvLevelSigma2; 193 | const std::vector mvInvLevelSigma2; 194 | 195 | // Image bounds and calibration 196 | const int mnMinX; 197 | const int mnMinY; 198 | const int mnMaxX; 199 | const int mnMaxY; 200 | const cv::Mat mK; 201 | 202 | 203 | // The following variables need to be accessed trough a mutex to be thread safe. 204 | protected: 205 | 206 | // SE3 Pose and camera center 207 | cv::Mat Tcw; 208 | cv::Mat Twc; 209 | cv::Mat Ow; 210 | 211 | cv::Mat Cw; // Stereo middel point. Only for visualization 212 | 213 | // MapPoints associated to keypoints 214 | std::vector mvpMapPoints; 215 | 216 | // BoW 217 | KeyFrameDatabase* mpKeyFrameDB; 218 | ORBVocabulary* mpORBvocabulary; 219 | 220 | // Grid over the image to speed up feature matching 221 | std::vector< std::vector > > mGrid; 222 | 223 | std::map mConnectedKeyFrameWeights; 224 | std::vector mvpOrderedConnectedKeyFrames; 225 | std::vector mvOrderedWeights; 226 | 227 | // Spanning Tree and Loop Edges 228 | bool mbFirstConnection; 229 | KeyFrame* mpParent; 230 | std::set mspChildrens; 231 | std::set mspLoopEdges; 232 | 233 | // Bad flags 234 | bool mbNotErase; 235 | bool mbToBeErased; 236 | bool mbBad; 237 | 238 | float mHalfBaseline; // Only for visualization 239 | 240 | Map* mpMap; 241 | 242 | std::mutex mMutexPose; 243 | std::mutex mMutexConnections; 244 | std::mutex mMutexFeatures; 245 | }; 246 | 247 | } //namespace ORB_SLAM 248 | 249 | #endif // KEYFRAME_H 250 | -------------------------------------------------------------------------------- /include/KeyFrameDatabase.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef KEYFRAMEDATABASE_H 22 | #define KEYFRAMEDATABASE_H 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include "KeyFrame.h" 29 | #include "Frame.h" 30 | #include "ORBVocabulary.h" 31 | 32 | #include 33 | 34 | 35 | namespace ORB_SLAM2 36 | { 37 | 38 | class KeyFrame; 39 | class Frame; 40 | 41 | 42 | class KeyFrameDatabase 43 | { 44 | public: 45 | 46 | KeyFrameDatabase(const ORBVocabulary &voc); 47 | 48 | void add(KeyFrame* pKF); 49 | 50 | void erase(KeyFrame* pKF); 51 | 52 | void clear(); 53 | 54 | // Loop Detection 55 | std::vector DetectLoopCandidates(KeyFrame* pKF, float minScore); 56 | 57 | // Relocalization 58 | std::vector DetectRelocalizationCandidates(Frame* F); 59 | 60 | protected: 61 | 62 | // Associated vocabulary 63 | const ORBVocabulary* mpVoc; 64 | 65 | // Inverted file 66 | std::vector > mvInvertedFile; 67 | 68 | // Mutex 69 | std::mutex mMutex; 70 | }; 71 | 72 | } //namespace ORB_SLAM 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /include/LocalMapping.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef LOCALMAPPING_H 22 | #define LOCALMAPPING_H 23 | 24 | #include "KeyFrame.h" 25 | #include "Map.h" 26 | #include "LoopClosing.h" 27 | #include "Tracking.h" 28 | #include "KeyFrameDatabase.h" 29 | 30 | #include 31 | 32 | 33 | namespace ORB_SLAM2 34 | { 35 | 36 | class Tracking; 37 | class LoopClosing; 38 | class Map; 39 | 40 | class LocalMapping 41 | { 42 | public: 43 | LocalMapping(Map* pMap, const float bMonocular); 44 | 45 | void SetLoopCloser(LoopClosing* pLoopCloser); 46 | 47 | void SetTracker(Tracking* pTracker); 48 | 49 | // Main function 50 | void Run(); 51 | 52 | void InsertKeyFrame(KeyFrame* pKF); 53 | 54 | // Thread Synch 55 | void RequestStop(); 56 | void RequestReset(); 57 | bool Stop(); 58 | void Release(); 59 | bool isStopped(); 60 | bool stopRequested(); 61 | bool AcceptKeyFrames(); 62 | void SetAcceptKeyFrames(bool flag); 63 | bool SetNotStop(bool flag); 64 | 65 | void InterruptBA(); 66 | 67 | void RequestFinish(); 68 | bool isFinished(); 69 | 70 | int KeyframesInQueue(){ 71 | unique_lock lock(mMutexNewKFs); 72 | return mlNewKeyFrames.size(); 73 | } 74 | 75 | protected: 76 | 77 | bool CheckNewKeyFrames(); 78 | void ProcessNewKeyFrame(); 79 | void CreateNewMapPoints(); 80 | 81 | void MapPointCulling(); 82 | void SearchInNeighbors(); 83 | 84 | void KeyFrameCulling(); 85 | 86 | cv::Mat ComputeF12(KeyFrame* &pKF1, KeyFrame* &pKF2); 87 | 88 | cv::Mat SkewSymmetricMatrix(const cv::Mat &v); 89 | 90 | bool mbMonocular; 91 | 92 | void ResetIfRequested(); 93 | bool mbResetRequested; 94 | std::mutex mMutexReset; 95 | 96 | bool CheckFinish(); 97 | void SetFinish(); 98 | bool mbFinishRequested; 99 | bool mbFinished; 100 | std::mutex mMutexFinish; 101 | 102 | Map* mpMap; 103 | 104 | LoopClosing* mpLoopCloser; 105 | Tracking* mpTracker; 106 | 107 | std::list mlNewKeyFrames; 108 | 109 | KeyFrame* mpCurrentKeyFrame; 110 | 111 | std::list mlpRecentAddedMapPoints; 112 | 113 | std::mutex mMutexNewKFs; 114 | 115 | bool mbAbortBA; 116 | 117 | bool mbStopped; 118 | bool mbStopRequested; 119 | bool mbNotStop; 120 | std::mutex mMutexStop; 121 | 122 | bool mbAcceptKeyFrames; 123 | std::mutex mMutexAccept; 124 | }; 125 | 126 | } //namespace ORB_SLAM 127 | 128 | #endif // LOCALMAPPING_H 129 | -------------------------------------------------------------------------------- /include/LoopClosing.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef LOOPCLOSING_H 22 | #define LOOPCLOSING_H 23 | 24 | #include "KeyFrame.h" 25 | #include "LocalMapping.h" 26 | #include "Map.h" 27 | #include "ORBVocabulary.h" 28 | #include "Tracking.h" 29 | 30 | #include "KeyFrameDatabase.h" 31 | 32 | #include 33 | #include 34 | #include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" 35 | 36 | namespace ORB_SLAM2 37 | { 38 | 39 | class Tracking; 40 | class LocalMapping; 41 | class KeyFrameDatabase; 42 | 43 | 44 | class LoopClosing 45 | { 46 | public: 47 | 48 | typedef pair,int> ConsistentGroup; 49 | typedef map, 50 | Eigen::aligned_allocator > > KeyFrameAndPose; 51 | 52 | public: 53 | 54 | LoopClosing(Map* pMap, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale); 55 | 56 | void SetTracker(Tracking* pTracker); 57 | 58 | void SetLocalMapper(LocalMapping* pLocalMapper); 59 | 60 | // Main function 61 | void Run(); 62 | 63 | void InsertKeyFrame(KeyFrame *pKF); 64 | 65 | void RequestReset(); 66 | 67 | // This function will run in a separate thread 68 | void RunGlobalBundleAdjustment(unsigned long nLoopKF); 69 | 70 | bool isRunningGBA(){ 71 | unique_lock lock(mMutexGBA); 72 | return mbRunningGBA; 73 | } 74 | bool isFinishedGBA(){ 75 | unique_lock lock(mMutexGBA); 76 | return mbFinishedGBA; 77 | } 78 | 79 | void RequestFinish(); 80 | 81 | bool isFinished(); 82 | 83 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 84 | 85 | protected: 86 | 87 | bool CheckNewKeyFrames(); 88 | 89 | bool DetectLoop(); 90 | 91 | bool ComputeSim3(); 92 | 93 | void SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap); 94 | 95 | void CorrectLoop(); 96 | 97 | void ResetIfRequested(); 98 | bool mbResetRequested; 99 | std::mutex mMutexReset; 100 | 101 | bool CheckFinish(); 102 | void SetFinish(); 103 | bool mbFinishRequested; 104 | bool mbFinished; 105 | std::mutex mMutexFinish; 106 | 107 | Map* mpMap; 108 | Tracking* mpTracker; 109 | 110 | KeyFrameDatabase* mpKeyFrameDB; 111 | ORBVocabulary* mpORBVocabulary; 112 | 113 | LocalMapping *mpLocalMapper; 114 | 115 | std::list mlpLoopKeyFrameQueue; 116 | 117 | std::mutex mMutexLoopQueue; 118 | 119 | // Loop detector parameters 120 | float mnCovisibilityConsistencyTh; 121 | 122 | // Loop detector variables 123 | KeyFrame* mpCurrentKF; 124 | KeyFrame* mpMatchedKF; 125 | std::vector mvConsistentGroups; 126 | std::vector mvpEnoughConsistentCandidates; 127 | std::vector mvpCurrentConnectedKFs; 128 | std::vector mvpCurrentMatchedPoints; 129 | std::vector mvpLoopMapPoints; 130 | cv::Mat mScw; 131 | g2o::Sim3 mg2oScw; 132 | 133 | long unsigned int mLastLoopKFid; 134 | 135 | // Variables related to Global Bundle Adjustment 136 | bool mbRunningGBA; 137 | bool mbFinishedGBA; 138 | bool mbStopGBA; 139 | std::mutex mMutexGBA; 140 | std::thread* mpThreadGBA; 141 | 142 | // Fix scale in the stereo/RGB-D case 143 | bool mbFixScale; 144 | 145 | 146 | bool mnFullBAIdx; 147 | }; 148 | 149 | } //namespace ORB_SLAM 150 | 151 | #endif // LOOPCLOSING_H 152 | -------------------------------------------------------------------------------- /include/Map.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef MAP_H 22 | #define MAP_H 23 | 24 | #include "MapPoint.h" 25 | #include "KeyFrame.h" 26 | #include 27 | #include "ObjectTypes.h" 28 | 29 | 30 | #include 31 | 32 | 33 | 34 | namespace ORB_SLAM2 35 | { 36 | 37 | class MapPoint; 38 | class KeyFrame; 39 | class Object3D; 40 | 41 | class Map 42 | { 43 | public: 44 | Map(); 45 | 46 | void AddKeyFrame(KeyFrame* pKF); 47 | void AddMapPoint(MapPoint* pMP); 48 | void EraseMapPoint(MapPoint* pMP); 49 | void EraseKeyFrame(KeyFrame* pKF); 50 | void SetReferenceMapPoints(const std::vector &vpMPs); 51 | void InformNewBigChange(); 52 | int GetLastBigChangeIdx(); 53 | 54 | std::vector GetAllKeyFrames(); 55 | std::vector GetAllMapPoints(); 56 | std::vector GetReferenceMapPoints(); 57 | 58 | long unsigned int MapPointsInMap(); 59 | long unsigned KeyFramesInMap(); 60 | 61 | long unsigned int GetMaxKFid(); 62 | 63 | void clear(); 64 | 65 | vector mvpKeyFrameOrigins; 66 | 67 | std::mutex mMutexMapUpdate; 68 | 69 | // This avoid that two points are created simultaneously in separate threads (id conflict) 70 | std::mutex mMutexPointCreation; 71 | 72 | ///////////new codes////// 73 | std::set mspObject3Ds; 74 | void AddObject3D(Object3D* pObject3D); 75 | std::vector GetAllObject3Ds(); 76 | 77 | bool CheckIfMerge(Object3D* pObj3D1, Object3D* pObj3D2); 78 | void ObjectMapRegularization(); 79 | void MergeObject(vector vpObj3DtoMerge); 80 | void ClearInvalidObject(); 81 | 82 | protected: 83 | std::set mspMapPoints; 84 | std::set mspKeyFrames; 85 | 86 | std::vector mvpReferenceMapPoints; 87 | 88 | long unsigned int mnMaxKFid; 89 | 90 | // Index related to a big change in the map (loop closure, global BA) 91 | int mnBigChangeIdx; 92 | 93 | std::mutex mMutexMap; 94 | }; 95 | 96 | } //namespace ORB_SLAM 97 | 98 | #endif // MAP_H 99 | -------------------------------------------------------------------------------- /include/MapDrawer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef MAPDRAWER_H 22 | #define MAPDRAWER_H 23 | 24 | #include"Map.h" 25 | #include"MapPoint.h" 26 | #include"KeyFrame.h" 27 | #include 28 | #include "Tracking.h" 29 | 30 | 31 | #include 32 | 33 | namespace ORB_SLAM2 34 | { 35 | class Tracking; 36 | 37 | class MapDrawer 38 | { 39 | public: 40 | MapDrawer(Map* pMap, const string &strSettingPath); 41 | 42 | Map* mpMap; 43 | 44 | void DrawMapPoints(); 45 | void DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph); 46 | void DrawCurrentCamera(pangolin::OpenGlMatrix &Twc); 47 | void SetCurrentCameraPose(const cv::Mat &Tcw); 48 | void SetReferenceKeyFrame(KeyFrame *pKF); 49 | void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M); 50 | ///////new codes///// 51 | Frame mCurrentFrame; 52 | vector mvpMapObj3Ds; 53 | void DrawCurrentObjects(); 54 | void DrawMapObjects(bool bDrawCube, bool bDrawQuadric, bool bDrawMapPoints, bool bDrawCenter); 55 | void Update(Tracking *pTracker); 56 | 57 | private: 58 | 59 | float mKeyFrameSize; 60 | float mKeyFrameLineWidth; 61 | float mGraphLineWidth; 62 | float mPointSize; 63 | float mCameraSize; 64 | float mCameraLineWidth; 65 | 66 | cv::Mat mCameraPose; 67 | 68 | std::mutex mMutexCamera; 69 | }; 70 | 71 | } //namespace ORB_SLAM 72 | 73 | #endif // MAPDRAWER_H 74 | -------------------------------------------------------------------------------- /include/MapPoint.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef MAPPOINT_H 22 | #define MAPPOINT_H 23 | 24 | #include"KeyFrame.h" 25 | #include"Frame.h" 26 | #include"Map.h" 27 | 28 | #include 29 | #include 30 | 31 | namespace ORB_SLAM2 32 | { 33 | 34 | class KeyFrame; 35 | class Map; 36 | class Frame; 37 | 38 | 39 | class MapPoint 40 | { 41 | public: 42 | MapPoint(const cv::Mat &Pos, KeyFrame* pRefKF, Map* pMap); 43 | MapPoint(const cv::Mat &Pos, Map* pMap, Frame* pFrame, const int &idxF); 44 | 45 | void SetWorldPos(const cv::Mat &Pos); 46 | cv::Mat GetWorldPos(); 47 | 48 | cv::Mat GetNormal(); 49 | KeyFrame* GetReferenceKeyFrame(); 50 | 51 | std::map GetObservations(); 52 | int Observations(); 53 | 54 | void AddObservation(KeyFrame* pKF,size_t idx); 55 | void EraseObservation(KeyFrame* pKF); 56 | 57 | int GetIndexInKeyFrame(KeyFrame* pKF); 58 | bool IsInKeyFrame(KeyFrame* pKF); 59 | 60 | void SetBadFlag(); 61 | bool isBad(); 62 | 63 | void Replace(MapPoint* pMP); 64 | MapPoint* GetReplaced(); 65 | 66 | void IncreaseVisible(int n=1); 67 | void IncreaseFound(int n=1); 68 | float GetFoundRatio(); 69 | inline int GetFound(){ 70 | return mnFound; 71 | } 72 | 73 | void ComputeDistinctiveDescriptors(); 74 | 75 | cv::Mat GetDescriptor(); 76 | 77 | void UpdateNormalAndDepth(); 78 | 79 | float GetMinDistanceInvariance(); 80 | float GetMaxDistanceInvariance(); 81 | int PredictScale(const float ¤tDist, KeyFrame*pKF); 82 | int PredictScale(const float ¤tDist, Frame* pF); 83 | 84 | /////new codes//// 85 | //Label Analyze 86 | map mLabelCnt; 87 | map> mLabelProbCnt; 88 | void AddLabelCnt(int label); 89 | void AddLabelCntProb(int label, float prob); 90 | 91 | //Tracked Analyze 92 | map mTrackIDCnt; 93 | void AddTrackIDCnt(int track_id); 94 | map mInsertedTime;// show which update index when the mappoint is inserted 95 | void SetInsertedTime(int track_id, int update_cnt); 96 | map mTrackIDBad; 97 | void setTrackIDBad(int track_id); 98 | 99 | float GetLabelProb(int label); 100 | int GetLabelCntSum(); 101 | int GetLabelCnt(int label); 102 | int GetTrackedCnt(int track_id); 103 | 104 | 105 | public: 106 | long unsigned int mnId; 107 | static long unsigned int nNextId; 108 | long int mnFirstKFid; 109 | long int mnFirstFrame; 110 | int nObs; 111 | 112 | // Variables used by the tracking 113 | float mTrackProjX; 114 | float mTrackProjY; 115 | float mTrackProjXR; 116 | bool mbTrackInView; 117 | int mnTrackScaleLevel; 118 | float mTrackViewCos; 119 | long unsigned int mnTrackReferenceForFrame; 120 | long unsigned int mnLastFrameSeen; 121 | 122 | // Variables used by local mapping 123 | long unsigned int mnBALocalForKF; 124 | long unsigned int mnFuseCandidateForKF; 125 | 126 | // Variables used by loop closing 127 | long unsigned int mnLoopPointForKF; 128 | long unsigned int mnCorrectedByKF; 129 | long unsigned int mnCorrectedReference; 130 | cv::Mat mPosGBA; 131 | long unsigned int mnBAGlobalForKF; 132 | 133 | 134 | static std::mutex mGlobalMutex; 135 | 136 | protected: 137 | 138 | // Position in absolute coordinates 139 | cv::Mat mWorldPos; 140 | 141 | // Keyframes observing the point and associated index in keyframe 142 | std::map mObservations; 143 | 144 | // Mean viewing direction 145 | cv::Mat mNormalVector; 146 | 147 | // Best descriptor to fast matching 148 | cv::Mat mDescriptor; 149 | 150 | // Reference KeyFrame 151 | KeyFrame* mpRefKF; 152 | 153 | // Tracking counters 154 | int mnVisible; 155 | int mnFound; 156 | 157 | // Bad flag (we do not currently erase MapPoint from memory) 158 | bool mbBad; 159 | MapPoint* mpReplaced; 160 | 161 | // Scale invariance distances 162 | float mfMinDistance; 163 | float mfMaxDistance; 164 | 165 | Map* mpMap; 166 | 167 | std::mutex mMutexPos; 168 | std::mutex mMutexFeatures; 169 | }; 170 | 171 | } //namespace ORB_SLAM 172 | 173 | #endif // MAPPOINT_H 174 | -------------------------------------------------------------------------------- /include/ORBVocabulary.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #ifndef ORBVOCABULARY_H 23 | #define ORBVOCABULARY_H 24 | 25 | #include"Thirdparty/DBoW2/DBoW2/FORB.h" 26 | #include"Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h" 27 | 28 | namespace ORB_SLAM2 29 | { 30 | 31 | typedef DBoW2::TemplatedVocabulary 32 | ORBVocabulary; 33 | 34 | } //namespace ORB_SLAM 35 | 36 | #endif // ORBVOCABULARY_H 37 | -------------------------------------------------------------------------------- /include/ORBextractor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef ORBEXTRACTOR_H 22 | #define ORBEXTRACTOR_H 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | 29 | namespace ORB_SLAM2 30 | { 31 | 32 | class ExtractorNode 33 | { 34 | public: 35 | ExtractorNode():bNoMore(false){} 36 | 37 | void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4); 38 | 39 | std::vector vKeys; 40 | cv::Point2i UL, UR, BL, BR; 41 | std::list::iterator lit; 42 | bool bNoMore; 43 | }; 44 | 45 | class ORBextractor 46 | { 47 | public: 48 | 49 | enum {HARRIS_SCORE=0, FAST_SCORE=1 }; 50 | 51 | ORBextractor(int nfeatures, float scaleFactor, int nlevels, 52 | int iniThFAST, int minThFAST); 53 | 54 | ~ORBextractor(){} 55 | 56 | // Compute the ORB features and descriptors on an image. 57 | // ORB are dispersed on the image using an octree. 58 | // Mask is ignored in the current implementation. 59 | void operator()( cv::InputArray image, cv::InputArray mask, 60 | std::vector& keypoints, 61 | cv::OutputArray descriptors); 62 | 63 | int inline GetLevels(){ 64 | return nlevels;} 65 | 66 | float inline GetScaleFactor(){ 67 | return scaleFactor;} 68 | 69 | std::vector inline GetScaleFactors(){ 70 | return mvScaleFactor; 71 | } 72 | 73 | std::vector inline GetInverseScaleFactors(){ 74 | return mvInvScaleFactor; 75 | } 76 | 77 | std::vector inline GetScaleSigmaSquares(){ 78 | return mvLevelSigma2; 79 | } 80 | 81 | std::vector inline GetInverseScaleSigmaSquares(){ 82 | return mvInvLevelSigma2; 83 | } 84 | 85 | std::vector mvImagePyramid; 86 | 87 | protected: 88 | 89 | void ComputePyramid(cv::Mat image); 90 | void ComputeKeyPointsOctTree(std::vector >& allKeypoints); 91 | std::vector DistributeOctTree(const std::vector& vToDistributeKeys, const int &minX, 92 | const int &maxX, const int &minY, const int &maxY, const int &nFeatures, const int &level); 93 | 94 | void ComputeKeyPointsOld(std::vector >& allKeypoints); 95 | std::vector pattern; 96 | 97 | int nfeatures; 98 | double scaleFactor; 99 | int nlevels; 100 | int iniThFAST; 101 | int minThFAST; 102 | 103 | std::vector mnFeaturesPerLevel; 104 | 105 | std::vector umax; 106 | 107 | std::vector mvScaleFactor; 108 | std::vector mvInvScaleFactor; 109 | std::vector mvLevelSigma2; 110 | std::vector mvInvLevelSigma2; 111 | }; 112 | 113 | } //namespace ORB_SLAM 114 | 115 | #endif 116 | 117 | -------------------------------------------------------------------------------- /include/ORBmatcher.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #ifndef ORBMATCHER_H 23 | #define ORBMATCHER_H 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include"MapPoint.h" 30 | #include"KeyFrame.h" 31 | #include"Frame.h" 32 | 33 | 34 | namespace ORB_SLAM2 35 | { 36 | 37 | class ORBmatcher 38 | { 39 | public: 40 | 41 | ORBmatcher(float nnratio=0.6, bool checkOri=true); 42 | 43 | // Computes the Hamming distance between two ORB descriptors 44 | static int DescriptorDistance(const cv::Mat &a, const cv::Mat &b); 45 | 46 | // Search matches between Frame keypoints and projected MapPoints. Returns number of matches 47 | // Used to track the local map (Tracking) 48 | int SearchByProjection(Frame &F, const std::vector &vpMapPoints, const float th=3); 49 | 50 | // Project MapPoints tracked in last frame into the current frame and search matches. 51 | // Used to track from previous frame (Tracking) 52 | int SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono); 53 | 54 | // Project MapPoints seen in KeyFrame into the Frame and search matches. 55 | // Used in relocalisation (Tracking) 56 | int SearchByProjection(Frame &CurrentFrame, KeyFrame* pKF, const std::set &sAlreadyFound, const float th, const int ORBdist); 57 | 58 | // Project MapPoints using a Similarity Transformation and search matches. 59 | // Used in loop detection (Loop Closing) 60 | int SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const std::vector &vpPoints, std::vector &vpMatched, int th); 61 | 62 | // Search matches between MapPoints in a KeyFrame and ORB in a Frame. 63 | // Brute force constrained to ORB that belong to the same vocabulary node (at a certain level) 64 | // Used in Relocalisation and Loop Detection 65 | int SearchByBoW(KeyFrame *pKF, Frame &F, std::vector &vpMapPointMatches); 66 | int SearchByBoW(KeyFrame *pKF1, KeyFrame* pKF2, std::vector &vpMatches12); 67 | 68 | // Matching for the Map Initialization (only used in the monocular case) 69 | int SearchForInitialization(Frame &F1, Frame &F2, std::vector &vbPrevMatched, std::vector &vnMatches12, int windowSize=10); 70 | 71 | // Matching to triangulate new MapPoints. Check Epipolar Constraint. 72 | int SearchForTriangulation(KeyFrame *pKF1, KeyFrame* pKF2, cv::Mat F12, 73 | std::vector > &vMatchedPairs, const bool bOnlyStereo); 74 | 75 | // Search matches between MapPoints seen in KF1 and KF2 transforming by a Sim3 [s12*R12|t12] 76 | // In the stereo and RGB-D case, s12=1 77 | int SearchBySim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector &vpMatches12, const float &s12, const cv::Mat &R12, const cv::Mat &t12, const float th); 78 | 79 | // Project MapPoints into KeyFrame and search for duplicated MapPoints. 80 | int Fuse(KeyFrame* pKF, const vector &vpMapPoints, const float th=3.0); 81 | 82 | // Project MapPoints into KeyFrame using a given Sim3 and search for duplicated MapPoints. 83 | int Fuse(KeyFrame* pKF, cv::Mat Scw, const std::vector &vpPoints, float th, vector &vpReplacePoint); 84 | 85 | public: 86 | 87 | static const int TH_LOW; 88 | static const int TH_HIGH; 89 | static const int HISTO_LENGTH; 90 | 91 | 92 | protected: 93 | 94 | bool CheckDistEpipolarLine(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &F12, const KeyFrame *pKF); 95 | 96 | float RadiusByViewingCos(const float &viewCos); 97 | 98 | void ComputeThreeMaxima(std::vector* histo, const int L, int &ind1, int &ind2, int &ind3); 99 | 100 | float mfNNratio; 101 | bool mbCheckOrientation; 102 | }; 103 | 104 | }// namespace ORB_SLAM 105 | 106 | #endif // ORBMATCHER_H 107 | -------------------------------------------------------------------------------- /include/ObjectMatcher.h: -------------------------------------------------------------------------------- 1 | #ifndef OBJECTMATCHER_H 2 | #define OBJECTMATCHER_H 3 | #include "Frame.h" 4 | 5 | 6 | namespace ORB_SLAM2{ 7 | 8 | class Frame; 9 | 10 | class ObjectMatcher{ 11 | public: 12 | ObjectMatcher(); 13 | void MatchTwoFrame(Frame &PreF, Frame &CurF); 14 | void MatchMapToFrame(vector vpObject3Ds, Frame &F); 15 | //int MatchByOpticalFlow(Frame &PreF, Frame &CurF); 16 | void MatchByHSV(vector &vpObject3Ds, Frame &F); 17 | int MatchByORBMatch(Frame &PreF, Frame &CurF); 18 | void RejectOutliersByRANSAC(vector &pre_pts, vector &cur_pts, vector &status); 19 | float CalCosineSimilarity(cv::Mat &Hist1, cv::Mat &Hist2); 20 | void ReduceVector(vector &vpts, vector &status); 21 | void ReduceVector(vector> &matches, vector &status); 22 | int DescriptorDistance(const cv::Mat &a, const cv::Mat &b); 23 | void ComputeThreeMaxima(vector* histo, const int L, int &ind1, int &ind2, int &ind3); 24 | float static Compute3DIoU(Object3D *pObj3D1, Object3D *pObj3D2); 25 | float static Compute3DIoU(vector BBox3D1, vector BBox3D2);//BBox3D = [MaxX, MaxY, MaxZ, MinX, MinY, MinZ] 26 | float static Compute3DOverlapRatio(Object3D *pObj3D1, Object3D *pObj3D2); 27 | float static Compute3DOverlapRatio(vector BBox3D1, vector BBox3D2);//BBox3D = [MaxX, MaxY, MaxZ, MinX, MinY, MinZ] 28 | 29 | 30 | 31 | static const int TH_HIGH; 32 | static const int TH_LOW; 33 | static const int HISTO_LENGTH; 34 | 35 | float CosSimThreshold = 0.95; 36 | 37 | 38 | 39 | 40 | }; 41 | 42 | 43 | } 44 | 45 | #endif -------------------------------------------------------------------------------- /include/ObjectOptimizer.h: -------------------------------------------------------------------------------- 1 | #ifndef OBJECTOPTIMIZER_H 2 | #define OBJECTOPTIMIZER_H 3 | 4 | #include "Map.h" 5 | #include "MapPoint.h" 6 | #include "KeyFrame.h" 7 | #include "LoopClosing.h" 8 | #include "Frame.h" 9 | 10 | #include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" 11 | 12 | namespace ORB_SLAM2 13 | { 14 | 15 | 16 | extern int N_AllSemanticConstraintNum; 17 | 18 | class ObjectOptimizer 19 | { 20 | 21 | public: 22 | int static PoseOptimization1(Frame* pFrame);//all MapPoints, fix observations 23 | int static PoseOptimization2(Frame* pFrame);// 24 | 25 | 26 | 27 | 28 | 29 | 30 | }; 31 | 32 | 33 | } 34 | 35 | #endif -------------------------------------------------------------------------------- /include/ObjectTypes.h: -------------------------------------------------------------------------------- 1 | #ifndef OBJECTTYPES_H 2 | #define OBJECTTYPES_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "Frame.h" 8 | #include "KeyFrame.h" 9 | #include "Eigen/Core" 10 | #include 11 | #include "Thirdparty/DBoW2/DBoW2/BowVector.h" 12 | #include "Thirdparty/DBoW2/DBoW2/FeatureVector.h" 13 | #include "MapPoint.h" 14 | using namespace std; 15 | 16 | namespace ORB_SLAM2 17 | { 18 | 19 | #define MIN_OBJ3DMP_NUM 5 20 | 21 | template void ReduceVector(vector &vec, vector status); 22 | template void ReduceVector(vector &vec, vector status) 23 | { 24 | int n = status.size(); 25 | int k = 0; 26 | for(int i=0;i kps, vector desps, vector depths, vector framekp_indices, cv::Mat &HSV_histogram, ORBVocabulary* pORBvocabulary); 48 | int label; 49 | float prob; 50 | float x,y,w,h; 51 | cv::Mat mask; 52 | //For optimization 53 | cv::Mat mDistTransImg; 54 | //ORB key points in object mask 55 | vector mvKeyPoints; 56 | int N_Kp; 57 | vector mvpMapPoints; 58 | int N_Matched_Mp; 59 | vector mvFrameKpIndices; 60 | 61 | //ORB descriptors 62 | vector mvDescriptors; 63 | //ORB depth 64 | vector mvDepths; 65 | //BoW 66 | void ComputeBoW(ORBVocabulary* pORBvocabulary); 67 | //ORBVocabulary* mpORBvocabulary; 68 | DBoW2::BowVector mBowVec; 69 | DBoW2::FeatureVector mFeatVec; 70 | 71 | //HSV histogram 72 | cv::Mat mHSVHistogram; 73 | 74 | //track_id 75 | int track_id = -1; 76 | 77 | static vector DynaLabels; 78 | }; 79 | 80 | struct ObjectObservation 81 | { 82 | Eigen::Vector3f ObjectCenterW; 83 | cv::Mat ObsCamPoseTcw; 84 | cv::Mat AppearanceVec; 85 | }; 86 | 87 | 88 | class Object3D 89 | { 90 | public: 91 | Object3D(vector vpObj3DMps, int Obj2DidxF, Frame *F); 92 | Object3D(Object2D &Obj2D, Frame *F); 93 | Object3D(Object2D &Obj2D, KeyFrame *KF); 94 | 95 | 96 | 97 | int mLabel; 98 | int mTrackID; 99 | bool mbVaild; 100 | //Next track ID 101 | static long unsigned int nNextId; 102 | //Center of map point cloud, different from position 103 | Eigen::Vector3f mCenterW; 104 | //Bounding Box information 105 | float mMaxXw,mMaxYw,mMinXw,mMinYw,mMinZw,mMaxZw; 106 | //Map points in Object3D 107 | vector mvpMapPoints; 108 | //Observation history 109 | vector mvObservations; 110 | //Update with a new Object2D Observation 111 | void Update(int Obj2DidxF, Frame *F); 112 | int mUpdateCnt; 113 | //Update with merge Object3D 114 | bool mbReplaced; 115 | Object3D* mpReplaced; 116 | 117 | //Calculate metric information 118 | void CalculateCenterW(); 119 | void CalculateSize(); 120 | void CalculateCenterAndSize(); 121 | 122 | //Reject Outliers 123 | int mToLastRejectOutliers; 124 | int mnNewAddedMpNum; 125 | bool isMpBad(MapPoint* pMp); 126 | void RejectOutliers(); 127 | void RejectOutliersTEST0(vector &vpMps);//weighted standard deviation of clusters// 128 | void RejectOutliersTEST1(vector &vpMps);//weighted standard deviation of clusters && ratio of clusters// 129 | void RejectOutliersTEST2(vector &vpMps);//weighted standard deviation of clusters && standard deviation of points// 130 | void RejectOutliersTEST3(vector &vpMps);//ratio of clusters || standard deviation of points// 131 | 132 | //Reject Outliers By clustering and standard deviation 133 | void RejectOutliersTEST4(vector &vpMps);//--------------ratio of clusters-------------// 134 | void RejectOutliersTEST5(vector &vpMps);//---------standard deviation of points----------// 135 | void RejectOutliersTEST6(vector &vpMps);//------------label count--------------// 136 | void RejectOutliersTEST7(vector &vpMps);//-------------ratio of clusters || standard deviation of points-------------// 137 | 138 | 139 | }; 140 | 141 | 142 | 143 | } 144 | 145 | #endif -------------------------------------------------------------------------------- /include/Optimizer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef OPTIMIZER_H 22 | #define OPTIMIZER_H 23 | 24 | #include "Map.h" 25 | #include "MapPoint.h" 26 | #include "KeyFrame.h" 27 | #include "LoopClosing.h" 28 | #include "Frame.h" 29 | 30 | #include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" 31 | 32 | namespace ORB_SLAM2 33 | { 34 | 35 | class LoopClosing; 36 | 37 | class Optimizer 38 | { 39 | public: 40 | void static BundleAdjustment(const std::vector &vpKF, const std::vector &vpMP, 41 | int nIterations = 5, bool *pbStopFlag=NULL, const unsigned long nLoopKF=0, 42 | const bool bRobust = true); 43 | void static GlobalBundleAdjustemnt(Map* pMap, int nIterations=5, bool *pbStopFlag=NULL, 44 | const unsigned long nLoopKF=0, const bool bRobust = true); 45 | void static LocalBundleAdjustment(KeyFrame* pKF, bool *pbStopFlag, Map *pMap); 46 | int static PoseOptimization(Frame* pFrame); 47 | 48 | // if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise (mono) 49 | void static OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF, 50 | const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, 51 | const LoopClosing::KeyFrameAndPose &CorrectedSim3, 52 | const map > &LoopConnections, 53 | const bool &bFixScale); 54 | 55 | // if bFixScale is true, optimize SE3 (stereo,rgbd), Sim3 otherwise (mono) 56 | static int OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector &vpMatches1, 57 | g2o::Sim3 &g2oS12, const float th2, const bool bFixScale); 58 | }; 59 | 60 | } //namespace ORB_SLAM 61 | 62 | #endif // OPTIMIZER_H 63 | -------------------------------------------------------------------------------- /include/PnPsolver.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * This file is a modified version of EPnP , see FreeBSD license below. 4 | * 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 6 | * For more information see 7 | * 8 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * ORB-SLAM2 is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with ORB-SLAM2. If not, see . 20 | */ 21 | 22 | /** 23 | * Copyright (c) 2009, V. Lepetit, EPFL 24 | * All rights reserved. 25 | * 26 | * Redistribution and use in source and binary forms, with or without 27 | * modification, are permitted provided that the following conditions are met: 28 | * 29 | * 1. Redistributions of source code must retain the above copyright notice, this 30 | * list of conditions and the following disclaimer. 31 | * 2. Redistributions in binary form must reproduce the above copyright notice, 32 | * this list of conditions and the following disclaimer in the documentation 33 | * and/or other materials provided with the distribution. 34 | * 35 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 36 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 37 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 38 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 39 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 40 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 41 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 42 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 43 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 44 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 45 | * 46 | * The views and conclusions contained in the software and documentation are those 47 | * of the authors and should not be interpreted as representing official policies, 48 | * either expressed or implied, of the FreeBSD Project 49 | */ 50 | 51 | #ifndef PNPSOLVER_H 52 | #define PNPSOLVER_H 53 | 54 | #include 55 | #include "MapPoint.h" 56 | #include "Frame.h" 57 | 58 | namespace ORB_SLAM2 59 | { 60 | 61 | class PnPsolver { 62 | public: 63 | PnPsolver(const Frame &F, const vector &vpMapPointMatches); 64 | 65 | ~PnPsolver(); 66 | 67 | void SetRansacParameters(double probability = 0.99, int minInliers = 8 , int maxIterations = 300, int minSet = 4, float epsilon = 0.4, 68 | float th2 = 5.991); 69 | 70 | cv::Mat find(vector &vbInliers, int &nInliers); 71 | 72 | cv::Mat iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers); 73 | 74 | private: 75 | 76 | void CheckInliers(); 77 | bool Refine(); 78 | 79 | // Functions from the original EPnP code 80 | void set_maximum_number_of_correspondences(const int n); 81 | void reset_correspondences(void); 82 | void add_correspondence(const double X, const double Y, const double Z, 83 | const double u, const double v); 84 | 85 | double compute_pose(double R[3][3], double T[3]); 86 | 87 | void relative_error(double & rot_err, double & transl_err, 88 | const double Rtrue[3][3], const double ttrue[3], 89 | const double Rest[3][3], const double test[3]); 90 | 91 | void print_pose(const double R[3][3], const double t[3]); 92 | double reprojection_error(const double R[3][3], const double t[3]); 93 | 94 | void choose_control_points(void); 95 | void compute_barycentric_coordinates(void); 96 | void fill_M(CvMat * M, const int row, const double * alphas, const double u, const double v); 97 | void compute_ccs(const double * betas, const double * ut); 98 | void compute_pcs(void); 99 | 100 | void solve_for_sign(void); 101 | 102 | void find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho, double * betas); 103 | void find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, double * betas); 104 | void find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho, double * betas); 105 | void qr_solve(CvMat * A, CvMat * b, CvMat * X); 106 | 107 | double dot(const double * v1, const double * v2); 108 | double dist2(const double * p1, const double * p2); 109 | 110 | void compute_rho(double * rho); 111 | void compute_L_6x10(const double * ut, double * l_6x10); 112 | 113 | void gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double current_betas[4]); 114 | void compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho, 115 | double cb[4], CvMat * A, CvMat * b); 116 | 117 | double compute_R_and_t(const double * ut, const double * betas, 118 | double R[3][3], double t[3]); 119 | 120 | void estimate_R_and_t(double R[3][3], double t[3]); 121 | 122 | void copy_R_and_t(const double R_dst[3][3], const double t_dst[3], 123 | double R_src[3][3], double t_src[3]); 124 | 125 | void mat_to_quat(const double R[3][3], double q[4]); 126 | 127 | 128 | double uc, vc, fu, fv; 129 | 130 | double * pws, * us, * alphas, * pcs; 131 | int maximum_number_of_correspondences; 132 | int number_of_correspondences; 133 | 134 | double cws[4][3], ccs[4][3]; 135 | double cws_determinant; 136 | 137 | vector mvpMapPointMatches; 138 | 139 | // 2D Points 140 | vector mvP2D; 141 | vector mvSigma2; 142 | 143 | // 3D Points 144 | vector mvP3Dw; 145 | 146 | // Index in Frame 147 | vector mvKeyPointIndices; 148 | 149 | // Current Estimation 150 | double mRi[3][3]; 151 | double mti[3]; 152 | cv::Mat mTcwi; 153 | vector mvbInliersi; 154 | int mnInliersi; 155 | 156 | // Current Ransac State 157 | int mnIterations; 158 | vector mvbBestInliers; 159 | int mnBestInliers; 160 | cv::Mat mBestTcw; 161 | 162 | // Refined 163 | cv::Mat mRefinedTcw; 164 | vector mvbRefinedInliers; 165 | int mnRefinedInliers; 166 | 167 | // Number of Correspondences 168 | int N; 169 | 170 | // Indices for random selection [0 .. N-1] 171 | vector mvAllIndices; 172 | 173 | // RANSAC probability 174 | double mRansacProb; 175 | 176 | // RANSAC min inliers 177 | int mRansacMinInliers; 178 | 179 | // RANSAC max iterations 180 | int mRansacMaxIts; 181 | 182 | // RANSAC expected inliers/total ratio 183 | float mRansacEpsilon; 184 | 185 | // RANSAC Threshold inlier/outlier. Max error e = dist(P1,T_12*P2)^2 186 | float mRansacTh; 187 | 188 | // RANSAC Minimun Set used at each iteration 189 | int mRansacMinSet; 190 | 191 | // Max square error associated with scale level. Max error = th*th*sigma(level)*sigma(level) 192 | vector mvMaxError; 193 | 194 | }; 195 | 196 | } //namespace ORB_SLAM 197 | 198 | #endif //PNPSOLVER_H 199 | -------------------------------------------------------------------------------- /include/Semantic.h: -------------------------------------------------------------------------------- 1 | #ifndef SEMANTIC_H 2 | #define SEMANTIC_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | using namespace std; 8 | 9 | namespace ORB_SLAM2 10 | { 11 | 12 | struct semantic 13 | { 14 | int label; 15 | float prob; 16 | float x,y,w,h; 17 | cv::Mat mask; 18 | }; 19 | 20 | class Semantic{ 21 | 22 | public: 23 | //TO DO: extract semantic information online 24 | Semantic(const string &ParamFile, const string &WeightFile, const string &LabelFile); 25 | Semantic(); 26 | setmVaildObjLabelsKitti; 27 | setmVaildObjLabelsTUMRGBD; 28 | //setmVaildObjLabelsTUMRGBD = {0,2,13,56,58,62,64,73,77,75}; 29 | //setmVaildObjLabelsTUMRGBD = {62}; 30 | //Load semantic information offline 31 | int kitti_id = 0; 32 | void ReadSemanticKittiStereo(vector &semantics,string &path, double timestamp, float prob_threshold); 33 | void ReadSemanticTUMRGBD(vector &semantics,string &path, double timestamp, float prob_threshold); 34 | }; 35 | 36 | 37 | 38 | } 39 | 40 | #endif -------------------------------------------------------------------------------- /include/Sim3Solver.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #ifndef SIM3SOLVER_H 23 | #define SIM3SOLVER_H 24 | 25 | #include 26 | #include 27 | 28 | #include "KeyFrame.h" 29 | 30 | 31 | 32 | namespace ORB_SLAM2 33 | { 34 | 35 | class Sim3Solver 36 | { 37 | public: 38 | 39 | Sim3Solver(KeyFrame* pKF1, KeyFrame* pKF2, const std::vector &vpMatched12, const bool bFixScale = true); 40 | 41 | void SetRansacParameters(double probability = 0.99, int minInliers = 6 , int maxIterations = 300); 42 | 43 | cv::Mat find(std::vector &vbInliers12, int &nInliers); 44 | 45 | cv::Mat iterate(int nIterations, bool &bNoMore, std::vector &vbInliers, int &nInliers); 46 | 47 | cv::Mat GetEstimatedRotation(); 48 | cv::Mat GetEstimatedTranslation(); 49 | float GetEstimatedScale(); 50 | 51 | 52 | protected: 53 | 54 | void ComputeCentroid(cv::Mat &P, cv::Mat &Pr, cv::Mat &C); 55 | 56 | void ComputeSim3(cv::Mat &P1, cv::Mat &P2); 57 | 58 | void CheckInliers(); 59 | 60 | void Project(const std::vector &vP3Dw, std::vector &vP2D, cv::Mat Tcw, cv::Mat K); 61 | void FromCameraToImage(const std::vector &vP3Dc, std::vector &vP2D, cv::Mat K); 62 | 63 | 64 | protected: 65 | 66 | // KeyFrames and matches 67 | KeyFrame* mpKF1; 68 | KeyFrame* mpKF2; 69 | 70 | std::vector mvX3Dc1; 71 | std::vector mvX3Dc2; 72 | std::vector mvpMapPoints1; 73 | std::vector mvpMapPoints2; 74 | std::vector mvpMatches12; 75 | std::vector mvnIndices1; 76 | std::vector mvSigmaSquare1; 77 | std::vector mvSigmaSquare2; 78 | std::vector mvnMaxError1; 79 | std::vector mvnMaxError2; 80 | 81 | int N; 82 | int mN1; 83 | 84 | // Current Estimation 85 | cv::Mat mR12i; 86 | cv::Mat mt12i; 87 | float ms12i; 88 | cv::Mat mT12i; 89 | cv::Mat mT21i; 90 | std::vector mvbInliersi; 91 | int mnInliersi; 92 | 93 | // Current Ransac State 94 | int mnIterations; 95 | std::vector mvbBestInliers; 96 | int mnBestInliers; 97 | cv::Mat mBestT12; 98 | cv::Mat mBestRotation; 99 | cv::Mat mBestTranslation; 100 | float mBestScale; 101 | 102 | // Scale is fixed to 1 in the stereo/RGBD case 103 | bool mbFixScale; 104 | 105 | // Indices for random selection 106 | std::vector mvAllIndices; 107 | 108 | // Projections 109 | std::vector mvP1im1; 110 | std::vector mvP2im2; 111 | 112 | // RANSAC probability 113 | double mRansacProb; 114 | 115 | // RANSAC min inliers 116 | int mRansacMinInliers; 117 | 118 | // RANSAC max iterations 119 | int mRansacMaxIts; 120 | 121 | // Threshold inlier/outlier. e = dist(Pi,T_ij*Pj)^2 < 5.991*mSigma2 122 | float mTh; 123 | float mSigma2; 124 | 125 | // Calibration 126 | cv::Mat mK1; 127 | cv::Mat mK2; 128 | 129 | }; 130 | 131 | } //namespace ORB_SLAM 132 | 133 | #endif // SIM3SOLVER_H 134 | -------------------------------------------------------------------------------- /include/System.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #ifndef SYSTEM_H 23 | #define SYSTEM_H 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include "Tracking.h" 30 | #include "FrameDrawer.h" 31 | #include "MapDrawer.h" 32 | #include "Map.h" 33 | #include "LocalMapping.h" 34 | #include "LoopClosing.h" 35 | #include "KeyFrameDatabase.h" 36 | #include "ORBVocabulary.h" 37 | #include "Viewer.h" 38 | 39 | 40 | namespace ORB_SLAM2 41 | { 42 | 43 | class Viewer; 44 | class FrameDrawer; 45 | class Map; 46 | class Tracking; 47 | class LocalMapping; 48 | class LoopClosing; 49 | class MapDrawer; 50 | 51 | class System 52 | { 53 | public: 54 | // Input sensor 55 | enum eSensor{ 56 | MONOCULAR=0, 57 | STEREO=1, 58 | RGBD=2 59 | }; 60 | 61 | public: 62 | 63 | // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and Viewer threads. 64 | System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = true); 65 | 66 | // Proccess the given stereo frame. Images must be synchronized and rectified. 67 | // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. 68 | // Returns the camera pose (empty if tracking fails). 69 | cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp); 70 | 71 | // Process the given rgbd frame. Depthmap must be registered to the RGB frame. 72 | // Input image: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. 73 | // Input depthmap: Float (CV_32F). 74 | // Returns the camera pose (empty if tracking fails). 75 | cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp); 76 | 77 | // Proccess the given monocular frame 78 | // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. 79 | // Returns the camera pose (empty if tracking fails). 80 | cv::Mat TrackMonocular(const cv::Mat &im, const double ×tamp); 81 | 82 | // This stops local mapping thread (map building) and performs only camera tracking. 83 | void ActivateLocalizationMode(); 84 | // This resumes local mapping thread and performs SLAM again. 85 | void DeactivateLocalizationMode(); 86 | 87 | // Returns true if there have been a big map change (loop closure, global BA) 88 | // since last call to this function 89 | bool MapChanged(); 90 | 91 | // Reset the system (clear map) 92 | void Reset(); 93 | 94 | // All threads will be requested to finish. 95 | // It waits until all threads have finished. 96 | // This function must be called before saving the trajectory. 97 | void Shutdown(); 98 | 99 | // Save camera trajectory in the TUM RGB-D dataset format. 100 | // Only for stereo and RGB-D. This method does not work for monocular. 101 | // Call first Shutdown() 102 | // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset 103 | void SaveTrajectoryTUM(const string &filename); 104 | 105 | // Save keyframe poses in the TUM RGB-D dataset format. 106 | // This method works for all sensor input. 107 | // Call first Shutdown() 108 | // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset 109 | void SaveKeyFrameTrajectoryTUM(const string &filename); 110 | 111 | // Save camera trajectory in the KITTI dataset format. 112 | // Only for stereo and RGB-D. This method does not work for monocular. 113 | // Call first Shutdown() 114 | // See format details at: http://www.cvlibs.net/datasets/kitti/eval_odometry.php 115 | void SaveTrajectoryKITTI(const string &filename); 116 | 117 | // TODO: Save/Load functions 118 | // SaveMap(const string &filename); 119 | // LoadMap(const string &filename); 120 | 121 | // Information from most recent processed frame 122 | // You can call this right after TrackMonocular (or stereo or RGBD) 123 | int GetTrackingState(); 124 | std::vector GetTrackedMapPoints(); 125 | std::vector GetTrackedKeyPointsUn(); 126 | 127 | ///new codes/// 128 | ////semantic module//// 129 | Semantic* mpSemantic; 130 | void SaveObjctInfo(const string &filename); 131 | 132 | private: 133 | 134 | // Input sensor 135 | eSensor mSensor; 136 | 137 | // ORB vocabulary used for place recognition and feature matching. 138 | ORBVocabulary* mpVocabulary; 139 | 140 | // KeyFrame database for place recognition (relocalization and loop detection). 141 | KeyFrameDatabase* mpKeyFrameDatabase; 142 | 143 | // Map structure that stores the pointers to all KeyFrames and MapPoints. 144 | Map* mpMap; 145 | 146 | // Tracker. It receives a frame and computes the associated camera pose. 147 | // It also decides when to insert a new keyframe, create some new MapPoints and 148 | // performs relocalization if tracking fails. 149 | Tracking* mpTracker; 150 | 151 | // Local Mapper. It manages the local map and performs local bundle adjustment. 152 | LocalMapping* mpLocalMapper; 153 | 154 | // Loop Closer. It searches loops with every new keyframe. If there is a loop it performs 155 | // a pose graph optimization and full bundle adjustment (in a new thread) afterwards. 156 | LoopClosing* mpLoopCloser; 157 | 158 | // The viewer draws the map and the current camera pose. It uses Pangolin. 159 | Viewer* mpViewer; 160 | 161 | FrameDrawer* mpFrameDrawer; 162 | MapDrawer* mpMapDrawer; 163 | 164 | // System threads: Local Mapping, Loop Closing, Viewer. 165 | // The Tracking thread "lives" in the main execution thread that creates the System object. 166 | std::thread* mptLocalMapping; 167 | std::thread* mptLoopClosing; 168 | std::thread* mptViewer; 169 | 170 | // Reset flag 171 | std::mutex mMutexReset; 172 | bool mbReset; 173 | 174 | // Change mode flags 175 | std::mutex mMutexMode; 176 | bool mbActivateLocalizationMode; 177 | bool mbDeactivateLocalizationMode; 178 | 179 | // Tracking state 180 | int mTrackingState; 181 | std::vector mTrackedMapPoints; 182 | std::vector mTrackedKeyPointsUn; 183 | std::mutex mMutexState; 184 | }; 185 | 186 | }// namespace ORB_SLAM 187 | 188 | #endif // SYSTEM_H 189 | -------------------------------------------------------------------------------- /include/Tracking.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #ifndef TRACKING_H 23 | #define TRACKING_H 24 | 25 | #include 26 | #include 27 | 28 | #include"Viewer.h" 29 | #include"FrameDrawer.h" 30 | #include"Map.h" 31 | #include"LocalMapping.h" 32 | #include"LoopClosing.h" 33 | #include"Frame.h" 34 | #include "ORBVocabulary.h" 35 | #include"KeyFrameDatabase.h" 36 | #include"ORBextractor.h" 37 | #include "Initializer.h" 38 | #include "MapDrawer.h" 39 | #include "System.h" 40 | 41 | #include 42 | 43 | namespace ORB_SLAM2 44 | { 45 | 46 | class Viewer; 47 | class FrameDrawer; 48 | class Map; 49 | class LocalMapping; 50 | class LoopClosing; 51 | class System; 52 | 53 | class Tracking 54 | { 55 | 56 | public: 57 | Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap, 58 | KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor); 59 | 60 | // Preprocess the input and call Track(). Extract features and performs stereo matching. 61 | cv::Mat GrabImageStereo(const cv::Mat &imRectLeft,const cv::Mat &imRectRight, const double ×tamp); 62 | cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp); 63 | cv::Mat GrabImageMonocular(const cv::Mat &im, const double ×tamp); 64 | 65 | void SetLocalMapper(LocalMapping* pLocalMapper); 66 | void SetLoopClosing(LoopClosing* pLoopClosing); 67 | void SetViewer(Viewer* pViewer); 68 | 69 | // Load new settings 70 | // The focal lenght should be similar or scale prediction will fail when projecting points 71 | // TODO: Modify MapPoint::PredictScale to take into account focal lenght 72 | void ChangeCalibration(const string &strSettingPath); 73 | 74 | // Use this function if you have deactivated local mapping and you only want to localize the camera. 75 | void InformOnlyTracking(const bool &flag); 76 | ////////new codes///////// 77 | void SetSemantic(Semantic* pSemantic); 78 | Semantic* mpSemantic; 79 | string mSemanticDatasetPath; 80 | float mSemanticProbThreshold = 0.5; 81 | cv::Mat mImRGB; 82 | //vector mCurrentObject2Ds; 83 | void TrackObject(); 84 | void TrackObjectOverTwoFrame(); 85 | void TrackObjectMapToFrame(); 86 | void UpdateCurrentObject(bool bUpdate); 87 | void CheckReplacedObjInLastFrame(); 88 | Frame mLastFrame; 89 | 90 | bool mbStep; 91 | bool bStepByStep; 92 | void SetStepByStep(bool bSet); 93 | bool GetStepByStep(); 94 | 95 | 96 | public: 97 | 98 | // Tracking states 99 | enum eTrackingState{ 100 | SYSTEM_NOT_READY=-1, 101 | NO_IMAGES_YET=0, 102 | NOT_INITIALIZED=1, 103 | OK=2, 104 | LOST=3 105 | }; 106 | 107 | eTrackingState mState; 108 | eTrackingState mLastProcessedState; 109 | 110 | // Input sensor 111 | int mSensor; 112 | 113 | // Current Frame 114 | Frame mCurrentFrame; 115 | cv::Mat mImGray; 116 | 117 | // Initialization Variables (Monocular) 118 | std::vector mvIniLastMatches; 119 | std::vector mvIniMatches; 120 | std::vector mvbPrevMatched; 121 | std::vector mvIniP3D; 122 | Frame mInitialFrame; 123 | 124 | // Lists used to recover the full camera trajectory at the end of the execution. 125 | // Basically we store the reference keyframe for each frame and its relative transformation 126 | list mlRelativeFramePoses; 127 | list mlpReferences; 128 | list mlFrameTimes; 129 | list mlbLost; 130 | 131 | // True if local mapping is deactivated and we are performing only localization 132 | bool mbOnlyTracking; 133 | 134 | void Reset(); 135 | 136 | protected: 137 | 138 | // Main tracking function. It is independent of the input sensor. 139 | void Track(); 140 | 141 | // Map initialization for stereo and RGB-D 142 | void StereoInitialization(); 143 | 144 | // Map initialization for monocular 145 | void MonocularInitialization(); 146 | void CreateInitialMapMonocular(); 147 | 148 | void CheckReplacedInLastFrame(); 149 | bool TrackReferenceKeyFrame(); 150 | void UpdateLastFrame(); 151 | bool TrackWithMotionModel(); 152 | 153 | bool Relocalization(); 154 | 155 | void UpdateLocalMap(); 156 | void UpdateLocalPoints(); 157 | void UpdateLocalKeyFrames(); 158 | 159 | bool TrackLocalMap(); 160 | void SearchLocalPoints(); 161 | 162 | bool NeedNewKeyFrame(); 163 | void CreateNewKeyFrame(); 164 | 165 | // In case of performing only localization, this flag is true when there are no matches to 166 | // points in the map. Still tracking will continue if there are enough matches with temporal points. 167 | // In that case we are doing visual odometry. The system will try to do relocalization to recover 168 | // "zero-drift" localization to the map. 169 | bool mbVO; 170 | 171 | //Other Thread Pointers 172 | LocalMapping* mpLocalMapper; 173 | LoopClosing* mpLoopClosing; 174 | 175 | //ORB 176 | ORBextractor* mpORBextractorLeft, *mpORBextractorRight; 177 | ORBextractor* mpIniORBextractor; 178 | 179 | //BoW 180 | ORBVocabulary* mpORBVocabulary; 181 | KeyFrameDatabase* mpKeyFrameDB; 182 | 183 | // Initalization (only for monocular) 184 | Initializer* mpInitializer; 185 | 186 | //Local Map 187 | KeyFrame* mpReferenceKF; 188 | std::vector mvpLocalKeyFrames; 189 | std::vector mvpLocalMapPoints; 190 | 191 | // System 192 | System* mpSystem; 193 | 194 | //Drawers 195 | Viewer* mpViewer; 196 | FrameDrawer* mpFrameDrawer; 197 | MapDrawer* mpMapDrawer; 198 | 199 | //Map 200 | Map* mpMap; 201 | 202 | //Calibration matrix 203 | cv::Mat mK; 204 | cv::Mat mDistCoef; 205 | float mbf; 206 | 207 | //New KeyFrame rules (according to fps) 208 | int mMinFrames; 209 | int mMaxFrames; 210 | 211 | // Threshold close/far points 212 | // Points seen as close by the stereo/RGBD sensor are considered reliable 213 | // and inserted from just one frame. Far points requiere a match in two keyframes. 214 | float mThDepth; 215 | 216 | // For RGB-D inputs only. For some datasets (e.g. TUM) the depthmap values are scaled. 217 | float mDepthMapFactor; 218 | 219 | //Current matches in frame 220 | int mnMatchesInliers; 221 | 222 | //Last Frame, KeyFrame and Relocalisation Info 223 | KeyFrame* mpLastKeyFrame; 224 | 225 | unsigned int mnLastKeyFrameId; 226 | unsigned int mnLastRelocFrameId; 227 | 228 | //Motion Model 229 | cv::Mat mVelocity; 230 | 231 | //Color order (true RGB, false BGR, ignored if grayscale) 232 | bool mbRGB; 233 | 234 | list mlpTemporalPoints; 235 | }; 236 | 237 | } //namespace ORB_SLAM 238 | 239 | #endif // TRACKING_H 240 | -------------------------------------------------------------------------------- /include/Viewer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #ifndef VIEWER_H 23 | #define VIEWER_H 24 | 25 | #include "FrameDrawer.h" 26 | #include "MapDrawer.h" 27 | #include "Tracking.h" 28 | #include "System.h" 29 | 30 | #include 31 | 32 | namespace ORB_SLAM2 33 | { 34 | 35 | class Tracking; 36 | class FrameDrawer; 37 | class MapDrawer; 38 | class System; 39 | 40 | class Viewer 41 | { 42 | public: 43 | Viewer(System* pSystem, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Tracking *pTracking, const string &strSettingPath); 44 | 45 | // Main thread function. Draw points, keyframes, the current camera pose and the last processed 46 | // frame. Drawing is refreshed according to the camera fps. We use Pangolin. 47 | void Run(); 48 | 49 | void RequestFinish(); 50 | 51 | void RequestStop(); 52 | 53 | bool isFinished(); 54 | 55 | bool isStopped(); 56 | 57 | void Release(); 58 | 59 | //new codes// 60 | bool isStepByStep(); 61 | 62 | private: 63 | 64 | bool Stop(); 65 | 66 | System* mpSystem; 67 | FrameDrawer* mpFrameDrawer; 68 | MapDrawer* mpMapDrawer; 69 | Tracking* mpTracker; 70 | 71 | // 1/fps in ms 72 | double mT; 73 | float mImageWidth, mImageHeight; 74 | 75 | float mViewpointX, mViewpointY, mViewpointZ, mViewpointF; 76 | 77 | bool CheckFinish(); 78 | void SetFinish(); 79 | bool mbFinishRequested; 80 | bool mbFinished; 81 | std::mutex mMutexFinish; 82 | 83 | bool mbStopped; 84 | bool mbStopRequested; 85 | std::mutex mMutexStop; 86 | 87 | }; 88 | 89 | } 90 | 91 | 92 | #endif // VIEWER_H 93 | 94 | 95 | -------------------------------------------------------------------------------- /run_exp_kitti.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | dataset_path = "/home/zhiyu/DataSet/KITTI-Odometry-Full/" 4 | eval_path = "ExpResults/KITTI/" 5 | for i in range(0,1): 6 | seq_id = str(i).zfill(2) 7 | os.system(f'./Examples/Stereo/stereo_kitti Vocabulary/ORBvoc.txt {dataset_path}/{seq_id}/*.yaml {dataset_path}/{seq_id}/') 8 | os.system(f'mv CameraTrajectory.txt {eval_path}/results/CameraTrajectory_{seq_id}.txt') 9 | os.system(f'mv ObjectInfo.txt {eval_path}/results/ObjectInfo_{seq_id}.txt') 10 | 11 | #if(plot_flag): 12 | os.system(f'evo_ape kitti {eval_path}/groundtruth/{seq_id}.txt {eval_path}/results/CameraTrajectory_{seq_id}.txt --no_warnings --plot_mode=xz -va --save_plot {eval_path}/results/{seq_id}_ape --save_results {eval_path}/results/{seq_id}_ape.zip') 13 | os.system(f'evo_rpe kitti {eval_path}/groundtruth/{seq_id}.txt {eval_path}/results/CameraTrajectory_{seq_id}.txt --no_warnings -r trans_part -va --save_plot {eval_path}/results/{seq_id}_rpe_t --save_results {eval_path}/results/{seq_id}_rpe_t.zip') 14 | os.system(f'evo_rpe kitti {eval_path}/groundtruth/{seq_id}.txt {eval_path}/results/CameraTrajectory_{seq_id}.txt --no_warnings -r angle_deg -va --save_plot {eval_path}/results/{seq_id}_rpe_R --save_results {eval_path}/results/{seq_id}_rpe_R.zip') 15 | os.system(f'evo_traj kitti {eval_path}/results/CameraTrajectory_{seq_id}.txt --ref={eval_path}/groundtruth/{seq_id}.txt --no_warnings --plot_mode=xz --save_plot {eval_path}/results/{seq_id}_traj') 16 | # else: 17 | # os.system(f'evo_ape kitti {eval_path}/groundtruth/{seq_id}.txt {eval_path}/results/CameraTrajectory_{seq_id}.txt --align_origin --no_warnings -v --save_results {eval_path}/results/{seq_id}_ape.zip') 18 | # os.system(f'evo_rpe kitti {eval_path}/groundtruth/{seq_id}.txt {eval_path}/results/CameraTrajectory_{seq_id}.txt --align_origin --no_warnings -r trans_part -v --save_results {eval_path}/results/{seq_id}_rpe_t.zip') 19 | # os.system(f'evo_rpe kitti {eval_path}/groundtruth/{seq_id}.txt {eval_path}/results/CameraTrajectory_{seq_id}.txt --align_origin --no_warnings -r angle_deg -v --save_results {eval_path}/results/{seq_id}_rpe_R.zip') 20 | 21 | os.system(f'unzip -q -o {eval_path}/results/{seq_id}_ape.zip -d {eval_path}/results/{seq_id}_ape') 22 | os.system(f'unzip -q -o {eval_path}/results/{seq_id}_rpe_t.zip -d {eval_path}/results/{seq_id}_rpe_t') 23 | os.system(f'unzip -q -o {eval_path}/results/{seq_id}_rpe_R.zip -d {eval_path}/results/{seq_id}_rpe_R') -------------------------------------------------------------------------------- /run_exp_tum.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | dataset_path = "/home/zhiyu/DataSet/TUM/" 4 | seq_paths = os.listdir(dataset_path) 5 | #seq_paths = [path for path in seq_paths if(os.path.isdir(f'{dataset_path}/{path}'))] 6 | #seq_paths = ["rgbd_dataset_freiburg1_desk"] 7 | #seq_paths = ["rgbd_dataset_freiburg1_desk2"] 8 | #seq_paths = ["rgbd_dataset_freiburg1_room"] 9 | #seq_paths = ["rgbd_dataset_freiburg1_xyz"] 10 | #seq_paths = ["rgbd_dataset_freiburg1_teddy"] 11 | seq_paths = ["rgbd_dataset_freiburg2_desk"] 12 | #seq_paths = ["rgbd_dataset_freiburg3_long_office_household"] 13 | #seq_paths = ["rgbd_dataset_freiburg3_teddy"] 14 | 15 | loc_eval_path = "ExpResults/TUM/Localization" 16 | ass_eval_path = "ExpResults/TUM/ObjectDataAssociation" 17 | 18 | 19 | for seq_path in seq_paths: 20 | print(f'Run the system on {seq_path}...') 21 | os.system(f'./Examples/RGB-D/rgbd_tum Vocabulary/ORBvoc.txt {dataset_path}/{seq_path}/*.yaml {dataset_path}/{seq_path} {dataset_path}/{seq_path}/associate.txt') 22 | os.system(f'mv CameraTrajectory.txt {loc_eval_path}/results/CameraTrajectory_{seq_path}.txt') 23 | os.system(f'python2 {loc_eval_path}/evaluate_ate.py {dataset_path}/{seq_path}/groundtruth.txt {loc_eval_path}/results/CameraTrajectory_{seq_path}.txt --verbose --output_path={loc_eval_path}/results/ --output_name=ATE_{seq_path}.txt --plot={loc_eval_path}/results/ATE_{seq_path}.png') 24 | os.system(f'python2 {loc_eval_path}/evaluate_rpe.py {dataset_path}/{seq_path}/groundtruth.txt {loc_eval_path}/results/CameraTrajectory_{seq_path}.txt --verbose --output_path={loc_eval_path}/results/ --output_name=RPE_{seq_path}.txt') 25 | 26 | os.system(f'cp ObjectInfo.txt {ass_eval_path}/ObjectInfo_{seq_path}.txt') 27 | os.system(f'mv ObjectInfo.txt {loc_eval_path}/ObjectInfo_{seq_path}.txt') 28 | print(f'Run the system on {seq_path} done.') -------------------------------------------------------------------------------- /src/Converter.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | 23 | #include "Converter.h" 24 | 25 | namespace ORB_SLAM2 26 | { 27 | 28 | std::vector Converter::toDescriptorVector(const cv::Mat &Descriptors) 29 | { 30 | std::vector vDesc; 31 | vDesc.reserve(Descriptors.rows); 32 | for (int j=0;j R; 41 | R << cvT.at(0,0), cvT.at(0,1), cvT.at(0,2), 42 | cvT.at(1,0), cvT.at(1,1), cvT.at(1,2), 43 | cvT.at(2,0), cvT.at(2,1), cvT.at(2,2); 44 | 45 | Eigen::Matrix t(cvT.at(0,3), cvT.at(1,3), cvT.at(2,3)); 46 | 47 | return g2o::SE3Quat(R,t); 48 | } 49 | 50 | cv::Mat Converter::toCvMat(const g2o::SE3Quat &SE3) 51 | { 52 | Eigen::Matrix eigMat = SE3.to_homogeneous_matrix(); 53 | return toCvMat(eigMat); 54 | } 55 | 56 | cv::Mat Converter::toCvMat(const g2o::Sim3 &Sim3) 57 | { 58 | Eigen::Matrix3d eigR = Sim3.rotation().toRotationMatrix(); 59 | Eigen::Vector3d eigt = Sim3.translation(); 60 | double s = Sim3.scale(); 61 | return toCvSE3(s*eigR,eigt); 62 | } 63 | 64 | cv::Mat Converter::toCvMat(const Eigen::Matrix &m) 65 | { 66 | cv::Mat cvMat(4,4,CV_32F); 67 | for(int i=0;i<4;i++) 68 | for(int j=0; j<4; j++) 69 | cvMat.at(i,j)=m(i,j); 70 | 71 | return cvMat.clone(); 72 | } 73 | 74 | cv::Mat Converter::toCvMat(const Eigen::Matrix3d &m) 75 | { 76 | cv::Mat cvMat(3,3,CV_32F); 77 | for(int i=0;i<3;i++) 78 | for(int j=0; j<3; j++) 79 | cvMat.at(i,j)=m(i,j); 80 | 81 | return cvMat.clone(); 82 | } 83 | 84 | cv::Mat Converter::toCvMat(const Eigen::Matrix &m) 85 | { 86 | cv::Mat cvMat(3,1,CV_32F); 87 | for(int i=0;i<3;i++) 88 | cvMat.at(i)=m(i); 89 | 90 | return cvMat.clone(); 91 | } 92 | 93 | cv::Mat Converter::toCvSE3(const Eigen::Matrix &R, const Eigen::Matrix &t) 94 | { 95 | cv::Mat cvMat = cv::Mat::eye(4,4,CV_32F); 96 | for(int i=0;i<3;i++) 97 | { 98 | for(int j=0;j<3;j++) 99 | { 100 | cvMat.at(i,j)=R(i,j); 101 | } 102 | } 103 | for(int i=0;i<3;i++) 104 | { 105 | cvMat.at(i,3)=t(i); 106 | } 107 | 108 | return cvMat.clone(); 109 | } 110 | 111 | Eigen::Matrix Converter::toVector3d(const cv::Mat &cvVector) 112 | { 113 | Eigen::Matrix v; 114 | v << cvVector.at(0), cvVector.at(1), cvVector.at(2); 115 | 116 | return v; 117 | } 118 | 119 | Eigen::Matrix Converter::toVector3d(const cv::Point3f &cvPoint) 120 | { 121 | Eigen::Matrix v; 122 | v << cvPoint.x, cvPoint.y, cvPoint.z; 123 | 124 | return v; 125 | } 126 | 127 | Eigen::Matrix Converter::toMatrix3d(const cv::Mat &cvMat3) 128 | { 129 | Eigen::Matrix M; 130 | 131 | M << cvMat3.at(0,0), cvMat3.at(0,1), cvMat3.at(0,2), 132 | cvMat3.at(1,0), cvMat3.at(1,1), cvMat3.at(1,2), 133 | cvMat3.at(2,0), cvMat3.at(2,1), cvMat3.at(2,2); 134 | 135 | return M; 136 | } 137 | 138 | std::vector Converter::toQuaternion(const cv::Mat &M) 139 | { 140 | Eigen::Matrix eigMat = toMatrix3d(M); 141 | Eigen::Quaterniond q(eigMat); 142 | 143 | std::vector v(4); 144 | v[0] = q.x(); 145 | v[1] = q.y(); 146 | v[2] = q.z(); 147 | v[3] = q.w(); 148 | 149 | return v; 150 | } 151 | 152 | cv::Mat Converter::toCvMat(const Eigen::Matrix &Mat4) 153 | { 154 | cv::Mat cvMat = cv::Mat::eye(4,4,CV_32F); 155 | for(int i=0;i<4;i++) 156 | { 157 | for(int j=0;j<4;j++) 158 | { 159 | cvMat.at(i,j) = Mat4(i,j); 160 | } 161 | } 162 | return cvMat; 163 | 164 | } 165 | cv::Mat Converter::toCvMat(const Eigen::Matrix &Vec3) 166 | { 167 | cv::Mat cvMat = cv::Mat(3,1,CV_32F); 168 | for(int i=0;i<3;i++) 169 | { 170 | cvMat.at(i) = Vec3(i); 171 | 172 | } 173 | return cvMat; 174 | } 175 | cv::Mat Converter::toCvMat(const Eigen::Matrix &Mat3) 176 | { 177 | cv::Mat cvMat = cv::Mat::eye(4,4,CV_32F); 178 | for(int i=0;i<3;i++) 179 | { 180 | for(int j=0;j<3;j++) 181 | { 182 | cvMat.at(i,j) = Mat3(i,j); 183 | } 184 | } 185 | return cvMat; 186 | 187 | } 188 | 189 | Eigen::Vector3f Converter::toEigenVec3(const cv::Mat &Vec3f) 190 | { 191 | Eigen::Vector3f EigenMat; 192 | EigenMat << Vec3f.at(0), Vec3f.at(1), Vec3f.at(2); 193 | return EigenMat; 194 | } 195 | 196 | Eigen::Matrix Converter::toEigenMat3(const cv::Mat &Mat3f) 197 | { 198 | Eigen::Matrix EigenMat; 199 | EigenMat << Mat3f.at(0,0), Mat3f.at(0,1), Mat3f.at(0,2), 200 | Mat3f.at(1,0), Mat3f.at(1,1), Mat3f.at(1,2), 201 | Mat3f.at(2,0), Mat3f.at(2,1), Mat3f.at(2,2); 202 | return EigenMat; 203 | 204 | } 205 | 206 | Eigen::Matrix Converter::toEigenMat4(const cv::Mat &Mat4f) 207 | { 208 | Eigen::Matrix EigenMat; 209 | EigenMat << Mat4f.at(0,0), Mat4f.at(0,1), Mat4f.at(0,2), Mat4f.at(0,3), 210 | Mat4f.at(1,0), Mat4f.at(1,1), Mat4f.at(1,2), Mat4f.at(1,3), 211 | Mat4f.at(2,0), Mat4f.at(2,1), Mat4f.at(2,2), Mat4f.at(2,3), 212 | Mat4f.at(3,0), Mat4f.at(3,1), Mat4f.at(3,2), Mat4f.at(3,3); 213 | return EigenMat; 214 | } 215 | 216 | } //namespace ORB_SLAM 217 | -------------------------------------------------------------------------------- /src/Semantic.cc: -------------------------------------------------------------------------------- 1 | #include "Semantic.h" 2 | #include "Tracking.h" 3 | #include 4 | 5 | namespace ORB_SLAM2 6 | { 7 | 8 | Semantic::Semantic() 9 | { 10 | mVaildObjLabelsTUMRGBD = {0,39,41,56,58,62,63,64,65,66,73,77}; 11 | mVaildObjLabelsKitti = {2}; 12 | } 13 | 14 | void Semantic::ReadSemanticKittiStereo(vector &semantics,string &path, double timestamp, float SemanticProbThreshold) 15 | { 16 | stringstream filenametream; 17 | filenametream << setw(6) << setfill('0')<> filename; 20 | 21 | string semanticpath = path+filename; 22 | ifstream semanticfile((semanticpath+"/"+filename+".txt").c_str()); 23 | int k =0; 24 | while(!semanticfile.eof()) 25 | { 26 | string line; 27 | getline(semanticfile, line); 28 | if(!line.empty()) 29 | { 30 | stringstream ss; 31 | ss << line; 32 | int label, x, y, w, h, instance_id; 33 | float prob; 34 | ss >> label; 35 | ss >> prob; 36 | if(prob <= SemanticProbThreshold) 37 | continue; 38 | ss >> x; 39 | ss >> y; 40 | ss >> w; 41 | ss >> h; 42 | ss >> instance_id; 43 | if(count(mVaildObjLabelsKitti.begin(),mVaildObjLabelsKitti.end(),label)) 44 | { 45 | cv::Mat mask = cv::imread(semanticpath+"/"+to_string(instance_id)+".png",-1); 46 | semantic semantic_temp = {label,prob,x,y,w,h,mask}; 47 | semantics.push_back(semantic_temp); 48 | } 49 | 50 | } 51 | 52 | } 53 | kitti_id++; 54 | 55 | } 56 | 57 | void Semantic::ReadSemanticTUMRGBD(vector &semantics,string &path, double timestamp, float SemanticProbThreshold) 58 | { 59 | 60 | string filename = to_string(timestamp);; 61 | string semanticpath = path+filename; 62 | ifstream semanticfile((semanticpath+"/"+filename+".txt").c_str()); 63 | while(!semanticfile.eof()) 64 | { 65 | string line; 66 | getline(semanticfile, line); 67 | if(!line.empty()) 68 | { 69 | stringstream ss; 70 | ss << line; 71 | int label, x, y, w, h, instance_id; 72 | float prob; 73 | ss >> label; 74 | if(label == 63) 75 | { 76 | label = 62; 77 | } 78 | ss >> prob; 79 | if(prob <= SemanticProbThreshold) 80 | continue; 81 | ss >> x; 82 | ss >> y; 83 | ss >> w; 84 | ss >> h; 85 | ss >> instance_id; 86 | if(count(mVaildObjLabelsTUMRGBD.begin(),mVaildObjLabelsTUMRGBD.end(),label)) 87 | { 88 | cv::Mat mask = cv::imread(semanticpath+"/"+to_string(instance_id)+".png",-1); 89 | semantic semantic_temp = {label,prob,x,y,w,h,mask}; 90 | semantics.push_back(semantic_temp); 91 | } 92 | 93 | } 94 | 95 | } 96 | } 97 | 98 | } 99 | 100 | --------------------------------------------------------------------------------