├── .gitignore ├── CMakeLists.txt ├── README.md ├── configs ├── 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 ├── Latex.md ├── TUM1.yaml ├── TUM2.yaml ├── TUM3.yaml └── latex_pics │ ├── 01.png │ ├── 02.png │ ├── 03.png │ ├── 04.png │ └── 05.png ├── include ├── CeresOptimizer.h ├── Frame.h ├── FrameDrawer.h ├── Initializer.h ├── KeyFrame.h ├── KeyFrameDatabase.h ├── LocalMapping.h ├── LoopClosing.h ├── Map.h ├── MapDrawer.h ├── MapPoint.h ├── MatEigenConverter.h ├── MonoORBSlam.h ├── ORBVocabulary.h ├── ORBextractor.h ├── ORBmatcher.h ├── PnPsolver.h ├── Sim3Solver.h ├── Tracking.h └── Viewer.h ├── lib └── DBoW2 │ ├── CMakeLists.txt │ ├── DBoW2 │ ├── BowVector.cpp │ ├── BowVector.h │ ├── FClass.h │ ├── FORB.cpp │ ├── FORB.h │ ├── FeatureVector.cpp │ ├── FeatureVector.h │ ├── ScoringObject.cpp │ ├── ScoringObject.h │ └── TemplatedVocabulary.h │ ├── DUtils │ ├── Random.cpp │ ├── Random.h │ ├── Timestamp.cpp │ └── Timestamp.h │ ├── LICENSE.txt │ └── README.txt ├── package.xml ├── src ├── CeresOptimizer.cc ├── Frame.cc ├── FrameDrawer.cc ├── Initializer.cc ├── KeyFrame.cc ├── KeyFrameDatabase.cc ├── LocalMapping.cc ├── LoopClosing.cc ├── Map.cc ├── MapDrawer.cc ├── MapPoint.cc ├── MatEigenConverter.cc ├── MonoORBSlam.cc ├── ORBextractor.cc ├── ORBmatcher.cc ├── PnPsolver.cc ├── Sim3Solver.cc ├── Tracking.cc ├── Viewer.cc └── main.cc └── vocabulary └── ORBvoc.txt.tar.gz /.gitignore: -------------------------------------------------------------------------------- 1 | ####################### MacOS gitignore ###################### 2 | # General 3 | .DS_Store 4 | .AppleDouble 5 | .LSOverride 6 | 7 | # Icon must end with two \r 8 | Icon 9 | 10 | # Thumbnails 11 | ._* 12 | 13 | # Files that might appear in the root of a volume 14 | .DocumentRevisions-V100 15 | .fseventsd 16 | .Spotlight-V100 17 | .TemporaryItems 18 | .Trashes 19 | .VolumeIcon.icns 20 | .com.apple.timemachine.donotpresent 21 | 22 | # Directories potentially created on remote AFP share 23 | .AppleDB 24 | .AppleDesktop 25 | Network Trash Folder 26 | Temporary Items 27 | .apdisk 28 | 29 | ####################### C++ gitignore ######################## 30 | # Prerequisites 31 | *.d 32 | 33 | # Compiled Object files 34 | *.slo 35 | *.lo 36 | *.o 37 | *.obj 38 | 39 | # Precompiled Headers 40 | *.gch 41 | *.pch 42 | 43 | # Compiled Dynamic libraries 44 | *.so 45 | *.dylib 46 | *.dll 47 | 48 | # Fortran module files 49 | *.mod 50 | *.smod 51 | 52 | # Compiled Static libraries 53 | *.lai 54 | *.la 55 | *.a 56 | *.lib 57 | 58 | # Executables 59 | *.exe 60 | *.out 61 | *.app 62 | 63 | ####################### Python gitignore ##################### 64 | # Byte-compiled / optimized / DLL files 65 | __pycache__/ 66 | *.py[cod] 67 | *$py.class 68 | 69 | # C extensions 70 | #*.so 71 | 72 | # Distribution / packaging 73 | .Python 74 | build/ 75 | develop-eggs/ 76 | eggs/ 77 | .eggs/ 78 | lib64/ 79 | *.egg-info/ 80 | .installed.cfg 81 | *.egg 82 | 83 | *.swp 84 | cscope.* 85 | tags 86 | vocabulary/ORBvoc.txt 87 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ceres_mono_orb_slam2) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ######################################################################### 8 | if (NOT CMAKE_BUILD_TYPE) 9 | set(CMAKE_BUILD_TYPE "Release") 10 | message(STATUS "No build type selected, default to ${CMAKE_BUILD_TYPE}") 11 | endif() 12 | 13 | if(CMAKE_BUILD_TYPE STREQUAL "Release") 14 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -fPIC -march=native") 15 | else(CMAKE_BUILD_TYPE STREQUAL "Debug") 16 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -fPIC") 17 | endif(CMAKE_BUILD_TYPE STREQUAL "Release") 18 | ######################################################################### 19 | 20 | #find_package(catkin REQUIRED) 21 | find_package(OpenCV REQUIRED) 22 | find_package(Eigen3 REQUIRED) 23 | find_package(Pangolin REQUIRED) 24 | find_package(Ceres REQUIRED) 25 | find_package(Sophus REQUIRED) 26 | #find_package(OpenMP REQUIRED) 27 | 28 | #catkin_package( 29 | # INCLUDE_DIRS include 30 | # #CATKIN_DEPENDS 31 | #) 32 | 33 | ########### 34 | ## Build ## 35 | ########### 36 | 37 | ## Specify additional locations of header files 38 | ## Your package locations should be listed before other locations 39 | include_directories( 40 | include 41 | ${PROJECT_SOURCE_DIR} 42 | #${catkin_INCLUDE_DIRS} 43 | ${EIGEN3_INCLUDE_DIR} 44 | ${Pangolin_INCLUDE_DIRS} 45 | ${Sophus_INCLUDE_DIRS} 46 | ) 47 | 48 | add_library(${PROJECT_NAME} SHARED 49 | src/MonoORBSlam.cc 50 | src/Tracking.cc 51 | src/LocalMapping.cc 52 | src/LoopClosing.cc 53 | src/ORBextractor.cc 54 | src/ORBmatcher.cc 55 | src/FrameDrawer.cc 56 | src/MapPoint.cc 57 | src/KeyFrame.cc 58 | src/Map.cc 59 | src/MapDrawer.cc 60 | src/CeresOptimizer.cc 61 | src/PnPsolver.cc 62 | src/Frame.cc 63 | src/KeyFrameDatabase.cc 64 | src/Sim3Solver.cc 65 | src/Initializer.cc 66 | src/Viewer.cc 67 | src/MatEigenConverter.cc 68 | ) 69 | 70 | target_link_libraries(${PROJECT_NAME} PUBLIC 71 | #OpenMP::OpenMP_CXX 72 | #${catkin_LIBRARIES} 73 | ${OpenCV_LIBS} 74 | ${EIGEN3_LIBS} 75 | ${Pangolin_LIBRARIES} 76 | ${CERES_LIBRARIES} 77 | ${PROJECT_SOURCE_DIR}/lib/DBoW2/lib/libDBoW2.so 78 | glog 79 | gflags 80 | ) 81 | 82 | add_executable(mono_slam 83 | src/main.cc 84 | ) 85 | 86 | target_link_libraries(mono_slam 87 | ${PROJECT_NAME} 88 | ) 89 | -------------------------------------------------------------------------------- /configs/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 | -------------------------------------------------------------------------------- /configs/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: 15.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 | -------------------------------------------------------------------------------- /configs/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 | -------------------------------------------------------------------------------- /configs/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 | -------------------------------------------------------------------------------- /configs/Latex.md: -------------------------------------------------------------------------------- 1 | ### Some neccessary equations before Jacobian calculation 2 | **1. With the property of Lie Algebra Adjoint, [Reference](https://blog.csdn.net/heyijia0327/article/details/51773578)** 3 | $$\mathbf{S * exp(\hat{\zeta}) * S^{-1} = exp[(Adj(S) * \zeta)^{\Lambda}]}$$ 4 | We can get equations below 5 | $$\mathbf{S * exp(\hat{\zeta}) = exp[(Adj(S) * \zeta)^{\Lambda}] * S}$$ 6 | and 7 | $$\mathbf{exp(\hat{\zeta}) * S^{-1} = S^{-1} * exp[(Adj(S) * \zeta)^{\Lambda}]}$$ 8 | 9 | **2. Baker-Campbell-Hausdorf equations, [STATE ESTIMATION FOR ROBOTICS](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) P.234** 10 | 11 | $\hspace{2cm}\mathbf{ln(S_1 S_2)^{v} = ln(exp[\hat{\xi_1]}exp[\hat{\xi_2]})^{v}}$ 12 | 13 | $\hspace{4cm}\mathbf{ = \xi_1 + \xi_2 + \frac{1}{2} \xi_1^{\lambda} \xi_2 + \frac{1}{12}\xi_1^{\lambda} \xi_1^{\lambda} \xi_2 + \frac{1}{12}\xi_2^{\lambda} \xi_2^{\lambda} \xi_1 + \dots}$ 14 | 15 | $\hspace{4cm}\mathbf{\approx J_l(\xi_2)^{-1}\xi_1 + \xi_2}\hspace{2cm}$ (if $\xi_1 small$) 16 | 17 | $\hspace{4cm}\mathbf{\approx \xi_2 + J_r(\xi_1)^{-1}\xi_2}\hspace{2cm}$ (if $\xi_2 small$) 18 | 19 | With 20 | $\hspace{3cm}\mathbf{J_l(\xi)^{-1} = \sum_{n = 0}^{\infty} \frac{B_n}{n!} (\xi^{\lambda})^{n}}$ 21 | 22 | $\hspace{3cm}\mathbf{J_r(\xi)^{-1} = \sum_{n = 0}^{\infty} \frac{B_n}{n!} (-\xi^{\lambda})^{n}}$ 23 | 24 | With$\hspace{1cm}B_0 = 1, B_1 = -\frac{1}{2}, B_2 = \frac{1}{6}, B_3 = 0, B_4 = -\frac{1}{30}\dots$, $\hspace{5mm}\mathbf{\xi^{\lambda} = adj(\xi)}$, $\hspace{5mm}$ is adjoint matrix of $\xi$ 25 | 26 | **3. Adjoint Matrix of sim(3)** 27 | 28 | a) Main property of adjoint matrix on Lie Algebras, [Reference: LIE GROUPS AND LIE ALGEBRAS, 1.6](http://www.math.jhu.edu/~fspinu/423/7.pdf) 29 | $$\mathbf{[x, y] = adj(x)y}$$ 30 | 31 | b) sim3 Lie Brackets, [Reference: Local Accuracy and Global Consistency for Efficient Visual SLAM, P.184, A.3.4](https://www.doc.ic.ac.uk/~ajd/Publications/Strasdat-H-2012-PhD-Thesis.pdf): 32 | $$\mathbf{[x, y] = [\begin{bmatrix} \nu \newline \omega \newline \sigma\end{bmatrix} \begin{bmatrix} \tau \newline \varphi \newline \varsigma \end{bmatrix}] = \begin{bmatrix}\omega \times \tau + \nu \times \varphi + \sigma\tau - \varsigma\nu \newline \omega \times \varphi \newline 0 \end{bmatrix}}$$ 33 | $$\mathbf{= \begin{bmatrix}(\hat{\omega} + \sigma I)\tau + \nu \times \varphi - \varsigma\nu \newline \omega \times \varphi \newline 0 \end{bmatrix}}$$ 34 | $$\hspace{-4cm} = \mathbf{adj(x) y}$$ 35 | 36 | We can get $\hspace{4cm}\mathbf{\xi^{\lambda} = adj(\xi) = \begin{bmatrix} (\hat{\omega} + \sigma I) & \hat{\nu} & -{\nu} \newline 0 & \hat{\omega} & 0 \newline 0 & 0 & 0 \end{bmatrix}}$ 37 | 38 | ### Left multiplication/Right multiplication for pose update 39 | sim(3) update with **Left multiplication/Right multiplication** has affect on Jacobian calculation, Formula derivation below used Right multiplication as example 40 | 41 | ### Jacobian Calculation of sim(3) 42 | $$\mathbf{error = S_{ji} S_{iw} S_{jw}^{-1}}$$ 43 | 44 | Derivation of $\mathbf{Jacobian_i}$ 45 | 46 | $\hspace{6cm}\mathbf{ln(error(\xi_i + \delta_i))^v = ln(S_{ji}S_{iw}exp(\hat{\delta_i})S_{jw}^{-1})^{v}}$ 47 | 48 | $\hspace{10cm}\mathbf{= ln(S_{ji}S_{iw}S_{jw}^{-1}exp[(Adj(S_{jw}){\delta_i})^{\Lambda}])^{v}}$ 49 | 50 | $\hspace{10cm}\mathbf{= ln(exp(\xi_{error}) \cdot exp[(Adj(S_{jw}){\delta_i})^{\Lambda}])^{v}}$ 51 | 52 | $\hspace{10cm}\mathbf{= \xi_{error} + J_r(\xi_{error})^{-1} Adj(S_{jw}){\delta_i}}$ 53 | 54 | $$\mathbf{Jacobian_i = \frac{\partial ln(error)^v}{\partial \delta_i} = \lim_{\delta_i \to 0} \frac{ln(error(\xi_i + \delta_i))^v - ln(error)^v}{\delta_i} = J_r(\xi_{error})^{-1} Adj(S_{jw}) }$$ 55 | 56 | Same to $\mathbf{Jacobian_j}$ 57 | 58 | $\hspace{6cm}\mathbf{ln(error(\xi_i + \delta_j)) = ln(S_{ji}S_{iw}(S_{jw}exp(\hat{\delta_j}))^{-1})^{v}}$ 59 | $\hspace{10cm}\mathbf{= ln(S_{ji}S_{iw}exp(-\hat{\delta_j})S_{jw}^{-1})^{v}}$ 60 | 61 | $\hspace{10cm}\mathbf{= ln(exp(\xi_{error}) \cdot exp[(Adj(S_{jw})(-\delta_j))^{\Lambda}])^{v}}$ 62 | 63 | $\hspace{10cm}\mathbf{= \xi_{error} - J_r(\xi_{error})^{-1} Adj(S_{jw}){\delta_j}}$ 64 | 65 | $$\mathbf{Jacobian_j = \frac{\partial ln(error)^v}{\partial \delta_j} = \lim_{\delta_j \to 0} \frac{ln(error(\xi_i + \delta_j))^v - ln(error)^v}{\delta_j} = -J_r(\xi_{error})^{-1} Adj(S_{jw}) }$$ 66 | 67 | -------------------------------------------------------------------------------- /configs/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 | -------------------------------------------------------------------------------- /configs/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 | -------------------------------------------------------------------------------- /configs/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 | -------------------------------------------------------------------------------- /configs/latex_pics/01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/b51/ceres_mono_orb_slam2/831abd513c5620bf802c11069b5c450a5d6b30e4/configs/latex_pics/01.png -------------------------------------------------------------------------------- /configs/latex_pics/02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/b51/ceres_mono_orb_slam2/831abd513c5620bf802c11069b5c450a5d6b30e4/configs/latex_pics/02.png -------------------------------------------------------------------------------- /configs/latex_pics/03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/b51/ceres_mono_orb_slam2/831abd513c5620bf802c11069b5c450a5d6b30e4/configs/latex_pics/03.png -------------------------------------------------------------------------------- /configs/latex_pics/04.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/b51/ceres_mono_orb_slam2/831abd513c5620bf802c11069b5c450a5d6b30e4/configs/latex_pics/04.png -------------------------------------------------------------------------------- /configs/latex_pics/05.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/b51/ceres_mono_orb_slam2/831abd513c5620bf802c11069b5c450a5d6b30e4/configs/latex_pics/05.png -------------------------------------------------------------------------------- /include/Frame.h: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: Frame.h 6 | * 7 | * Created On: Wed 04 Sep 2019 03:46:11 PM CST 8 | * Licensed under The GPLv3 License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | /** 12 | * This file is part of ORB-SLAM2. 13 | * 14 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 15 | * of Zaragoza) For more information see 16 | * 17 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 18 | * it under the terms of the GNU General Public License as published by 19 | * the Free Software Foundation, either version 3 of the License, or 20 | * (at your option) any later version. 21 | * 22 | * ORB-SLAM2 is distributed in the hope that it will be useful, 23 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 24 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 25 | * GNU General Public License for more details. 26 | * 27 | * You should have received a copy of the GNU General Public License 28 | * along with ORB-SLAM2. If not, see . 29 | */ 30 | 31 | #ifndef FRAME_H 32 | #define FRAME_H 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | #include "KeyFrame.h" 39 | #include "MapPoint.h" 40 | #include "ORBVocabulary.h" 41 | #include "ORBextractor.h" 42 | #include "lib/DBoW2/DBoW2/BowVector.h" 43 | #include "lib/DBoW2/DBoW2/FeatureVector.h" 44 | 45 | #define FRAME_GRID_ROWS 48 46 | #define FRAME_GRID_COLS 64 47 | 48 | namespace ORB_SLAM2 { 49 | 50 | class MapPoint; 51 | class KeyFrame; 52 | 53 | class Frame { 54 | public: 55 | Frame(); 56 | 57 | // Copy constructor. 58 | Frame(const Frame& frame); 59 | 60 | // Constructor for Monocular cameras. 61 | Frame(const cv::Mat& imgGray, const double& timestamp, 62 | ORBextractor* extractor, ORBVocabulary* voc, cv::Mat& K, 63 | cv::Mat& distCoef, const float& bf, const float& thDepth); 64 | 65 | // Extract ORB on the image. 0 for left image and 1 for right image. 66 | void ExtractORB(int flag, const cv::Mat& img); 67 | 68 | // Compute Bag of Words representation. 69 | void ComputeBoW(); 70 | 71 | // Set the camera pose. 72 | void SetPose(Eigen::Matrix4d Tcw); 73 | 74 | // Computes rotation, translation and camera center matrices from the camera 75 | // pose. 76 | void UpdatePoseMatrices(); 77 | 78 | // Returns the camera center. 79 | inline Eigen::Vector3d GetCameraCenter() { return Ow_; } 80 | 81 | // Returns inverse of rotation 82 | inline Eigen::Matrix3d GetRotationInverse() { return Rwc_; } 83 | 84 | // Check if a MapPoint is in the frustum of the camera 85 | // and fill variables of the MapPoint to be used by the tracking 86 | bool isInFrustum(MapPoint* map_point, float viewingCosLimit); 87 | 88 | // Compute the cell of a keypoint (return false if outside the grid) 89 | bool PosInGrid(const cv::KeyPoint& kp, int& posX, int& posY); 90 | 91 | vector GetFeaturesInArea(const float& x, const float& y, 92 | const float& r, const int minLevel = -1, 93 | const int maxLevel = -1) const; 94 | 95 | // Search a match for each keypoint in the left image to a keypoint in the 96 | // right image. If there is a match, depth is computed and the right 97 | // coordinate associated to the left keypoint is stored. 98 | void ComputeStereoMatches(); 99 | 100 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 101 | 102 | public: 103 | // Vocabulary used for relocalization. 104 | ORBVocabulary* orb_vocabulary_; 105 | 106 | // Feature extractor. The right is used only in the stereo case. 107 | ORBextractor *orb_extractor_left_, *orb_extractor_right_; 108 | 109 | // Frame timestamp. 110 | double timestamp_; 111 | 112 | // Calibration matrix and OpenCV distortion parameters. 113 | cv::Mat K_; 114 | static float fx_; 115 | static float fy_; 116 | static float cx_; 117 | static float cy_; 118 | static float invfx_; 119 | static float invfy_; 120 | cv::Mat dist_coef_; 121 | 122 | // Stereo baseline multiplied by fx. 123 | float bf_; 124 | 125 | // Stereo baseline in meters. 126 | float mb_; 127 | 128 | // Threshold close/far points. Close points are inserted from 1 view. 129 | // Far points are inserted as in the monocular case from 2 views. 130 | float threshold_depth_; 131 | 132 | // Number of KeyPoints. 133 | int N_; 134 | 135 | // Vector of keypoints (original for visualization) and undistorted (actually 136 | // used by the system). In the stereo case, mvKeysUn is redundant as images 137 | // must be rectified. In the RGB-D case, RGB images can be distorted. 138 | std::vector keypoints_, right_keypoints_; 139 | std::vector undistort_keypoints_; 140 | 141 | // Corresponding stereo coordinate and depth for each keypoint. 142 | // "Monocular" keypoints have a negative value. 143 | std::vector mvuRight; 144 | std::vector depthes_; 145 | 146 | // Bag of Words Vector structures. 147 | DBoW2::BowVector bow_vector_; 148 | DBoW2::FeatureVector feature_vector_; 149 | 150 | // ORB descriptor, each row associated to a keypoint. 151 | cv::Mat descriptors_, right_descriptors_; 152 | 153 | // MapPoints associated to keypoints, NULL pointer if no association. 154 | std::vector map_points_; 155 | 156 | // Flag to identify outlier associations. 157 | std::vector is_outliers_; 158 | 159 | // Keypoints are assigned to cells in a grid to reduce matching complexity 160 | // when projecting MapPoints. 161 | static float grid_element_width_inv_; 162 | static float grid_element_height_inv_; 163 | std::vector grid_[FRAME_GRID_COLS][FRAME_GRID_ROWS]; 164 | 165 | // Camera pose. 166 | Eigen::Matrix4d Tcw_; 167 | 168 | // Current and Next Frame id. 169 | static long unsigned int next_id_; 170 | long unsigned int id_; 171 | 172 | // Reference Keyframe. 173 | KeyFrame* reference_keyframe_; 174 | 175 | // Scale pyramid info. 176 | int n_scale_levels_; 177 | float scale_factor_; 178 | float log_scale_factor_; 179 | vector scale_factors_; 180 | vector inv_scale_factors_; 181 | vector level_sigma2s_; 182 | vector inv_level_sigma2s_; 183 | 184 | // Undistorted Image Bounds (computed once). 185 | static float min_x_; 186 | static float max_x_; 187 | static float min_y_; 188 | static float max_y_; 189 | 190 | static bool do_initial_computations_; 191 | 192 | private: 193 | // Undistort keypoints given OpenCV distortion parameters. 194 | // Only for the RGB-D case. Stereo must be already rectified! 195 | // (called in the constructor). 196 | void UndistortKeyPoints(); 197 | 198 | // Computes image bounds for the undistorted image (called in the 199 | // constructor). 200 | void ComputeImageBounds(const cv::Mat& imgLeft); 201 | 202 | // Assign keypoints to the grid for speed up feature matching (called in the 203 | // constructor). 204 | void AssignFeaturesToGrid(); 205 | 206 | // Rotation, translation and camera center 207 | Eigen::Matrix3d Rcw_; 208 | Eigen::Vector3d tcw_; 209 | Eigen::Matrix3d Rwc_; 210 | Eigen::Vector3d Ow_; //==mtwc 211 | }; 212 | 213 | } // namespace ORB_SLAM2 214 | #endif // FRAME_H 215 | -------------------------------------------------------------------------------- /include/FrameDrawer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 5 | * of Zaragoza) 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 "Map.h" 25 | #include "MapPoint.h" 26 | #include "Tracking.h" 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | 33 | namespace ORB_SLAM2 { 34 | 35 | class Tracking; 36 | class Viewer; 37 | 38 | class FrameDrawer { 39 | public: 40 | FrameDrawer(Map* map); 41 | 42 | // Update info from the last processed frame. 43 | void Update(Tracking* tracker); 44 | 45 | // Draw last processed frame. 46 | cv::Mat DrawFrame(); 47 | 48 | protected: 49 | void DrawTextInfo(cv::Mat& img, int state, cv::Mat& imText); 50 | 51 | // Info of the frame to be drawn 52 | cv::Mat image_; 53 | int N_; 54 | vector current_keypoints_; 55 | vector do_map_, do_vo_; 56 | bool do_only_tracking_; 57 | int ntracked_, ntracked_vo_; 58 | vector init_keypoints_; 59 | vector init_matches_; 60 | int state_; 61 | 62 | Map* map_; 63 | 64 | std::mutex mutex_; 65 | }; 66 | } // namespace ORB_SLAM2 67 | 68 | #endif 69 | -------------------------------------------------------------------------------- /include/Initializer.h: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: Initializer.h 6 | * 7 | * Created On: Thu 05 Sep 2019 10:03:56 AM CST 8 | * Licensed under The GPLv3 License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | /** 12 | * This file is part of ORB-SLAM2. 13 | * 14 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 15 | * of Zaragoza) For more information see 16 | * 17 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 18 | * it under the terms of the GNU General Public License as published by 19 | * the Free Software Foundation, either version 3 of the License, or 20 | * (at your option) any later version. 21 | * 22 | * ORB-SLAM2 is distributed in the hope that it will be useful, 23 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 24 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 25 | * GNU General Public License for more details. 26 | * 27 | * You should have received a copy of the GNU General Public License 28 | * along with ORB-SLAM2. If not, see . 29 | */ 30 | 31 | #ifndef INITIALIZER_H_ 32 | #define INITIALIZER_H_ 33 | 34 | #include 35 | #include 36 | #include "Frame.h" 37 | 38 | namespace ORB_SLAM2 { 39 | 40 | // THIS IS THE INITIALIZER FOR MONOCULAR SLAM. NOT USED IN THE STEREO OR RGBD 41 | // CASE. 42 | class Initializer { 43 | typedef std::pair Match; 44 | 45 | public: 46 | // Fix the reference frame 47 | Initializer(const Frame& reference_frame, float sigma = 1.0, 48 | int iterations = 200); 49 | 50 | // Computes in parallel a fundamental matrix and a homography 51 | // Selects a model and tries to recover the motion and the structure from 52 | // motion 53 | bool Initialize(const Frame& current_frame, const vector& matches, 54 | Eigen::Matrix3d& R21, Eigen::Vector3d& t21, 55 | vector& vP3D, vector& is_triangulated); 56 | 57 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 58 | 59 | private: 60 | void FindHomography(vector& vbMatchesInliers, float& score, 61 | Eigen::Matrix3d& H21); 62 | void FindFundamental(vector& vbInliers, float& score, 63 | Eigen::Matrix3d& F21); 64 | 65 | Eigen::Matrix3d ComputeH21(const vector& vP1, 66 | const vector& vP2); 67 | Eigen::Matrix3d ComputeF21(const vector& vP1, 68 | const vector& vP2); 69 | 70 | float CheckHomography(const Eigen::Matrix3d& H21, const Eigen::Matrix3d& H12, 71 | vector& vbMatchesInliers, float sigma); 72 | 73 | float CheckFundamental(const Eigen::Matrix3d& F21, 74 | vector& vbMatchesInliers, float sigma); 75 | 76 | bool ReconstructF(vector& vbMatchesInliers, Eigen::Matrix3d& F21, 77 | Eigen::Matrix3d& K, Eigen::Matrix3d& R21, 78 | Eigen::Vector3d& t21, vector& vP3D, 79 | vector& is_triangulated, float minParallax, 80 | int minTriangulated); 81 | 82 | bool ReconstructH(vector& vbMatchesInliers, Eigen::Matrix3d& H21, 83 | Eigen::Matrix3d& K, Eigen::Matrix3d& R21, 84 | Eigen::Vector3d& t21, vector& vP3D, 85 | vector& is_triangulated, float minParallax, 86 | int minTriangulated); 87 | 88 | void Triangulate(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, 89 | const Eigen::Matrix& P1, 90 | const Eigen::Matrix& P2, Eigen::Vector3d& x3D); 91 | 92 | void Normalize(const vector& vKeys, 93 | vector& vNormalizedPoints, Eigen::Matrix3d& T); 94 | 95 | int CheckRT(const Eigen::Matrix3d& R, const Eigen::Vector3d& t, 96 | const vector& vKeys1, 97 | const vector& vKeys2, 98 | const vector& vMatches12, vector& vbInliers, 99 | const Eigen::Matrix3d& K, vector& vP3D, 100 | float th2, vector& vbGood, float& parallax); 101 | 102 | void DecomposeE(const Eigen::Matrix3d& E, Eigen::Matrix3d& R1, 103 | Eigen::Matrix3d& R2, Eigen::Vector3d& t); 104 | 105 | // Keypoints from Reference Frame (Frame 1) 106 | std::vector reference_keypoints_; 107 | 108 | // Keypoints from Current Frame (Frame 2) 109 | std::vector current_keypoints_; 110 | 111 | // Current Matches from Reference to Current 112 | std::vector index_matches_; 113 | std::vector is_index_matched_; 114 | 115 | // Calibration 116 | Eigen::Matrix3d K_; 117 | 118 | // Standard Deviation and Variance 119 | float sigma_, sigma2_; 120 | 121 | // Ransac max iterations 122 | int max_iterations_; 123 | 124 | // Ransac sets 125 | vector > ransac_sets_; 126 | }; 127 | 128 | } // namespace ORB_SLAM2 129 | 130 | #endif // INITIALIZER_H_ 131 | -------------------------------------------------------------------------------- /include/KeyFrame.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 5 | * of Zaragoza) 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 25 | #include 26 | 27 | #include "Frame.h" 28 | #include "KeyFrameDatabase.h" 29 | #include "MapPoint.h" 30 | #include "ORBVocabulary.h" 31 | #include "ORBextractor.h" 32 | #include "lib/DBoW2/DBoW2/BowVector.h" 33 | #include "lib/DBoW2/DBoW2/FeatureVector.h" 34 | 35 | namespace ORB_SLAM2 { 36 | class Map; 37 | class MapPoint; 38 | class Frame; 39 | class KeyFrameDatabase; 40 | 41 | class KeyFrame { 42 | public: 43 | KeyFrame(Frame& frame, Map* map, KeyFrameDatabase* keyframe_database); 44 | 45 | // Pose functions 46 | void SetPose(const Eigen::Matrix4d& Tcw); 47 | Eigen::Matrix4d GetPose(); 48 | Eigen::Matrix4d GetPoseInverse(); 49 | Eigen::Vector3d GetCameraCenter(); 50 | Eigen::Matrix3d GetRotation(); 51 | Eigen::Vector3d GetTranslation(); 52 | 53 | // Bag of Words Representation 54 | void ComputeBoW(); 55 | 56 | // Covisibility graph functions 57 | void AddConnection(KeyFrame* keyframe, const int& weight); 58 | void EraseConnection(KeyFrame* keyframe); 59 | void UpdateConnections(); 60 | void UpdateBestCovisibles(); 61 | std::set GetConnectedKeyFrames(); 62 | std::vector GetVectorCovisibleKeyFrames(); 63 | std::vector GetBestCovisibilityKeyFrames(const int& N); 64 | std::vector GetCovisiblesByWeight(const int& w); 65 | int GetWeight(KeyFrame* keyframe); 66 | 67 | // Spanning tree functions 68 | void AddChild(KeyFrame* keyframe); 69 | void EraseChild(KeyFrame* keyframe); 70 | void ChangeParent(KeyFrame* keyframe); 71 | std::set GetChilds(); 72 | KeyFrame* GetParent(); 73 | bool hasChild(KeyFrame* keyframe); 74 | 75 | // Loop Edges 76 | void AddLoopEdge(KeyFrame* keyframe); 77 | std::set GetLoopEdges(); 78 | 79 | // MapPoint observation functions 80 | void AddMapPoint(MapPoint* map_point, const size_t& index); 81 | void EraseMapPointMatch(const size_t& index); 82 | void EraseMapPointMatch(MapPoint* map_point); 83 | void ReplaceMapPointMatch(const size_t& index, MapPoint* map_point); 84 | std::set GetMapPoints(); 85 | std::vector GetMapPointMatches(); 86 | int TrackedMapPoints(const int& minObs); 87 | MapPoint* GetMapPoint(const size_t& index); 88 | 89 | // KeyPoint functions 90 | std::vector GetFeaturesInArea(const float& x, const float& y, 91 | const float& r) const; 92 | 93 | // Image 94 | bool IsInImage(const float& x, const float& y) const; 95 | 96 | // Enable/Disable bad flag changes 97 | void SetNotErase(); 98 | void SetErase(); 99 | 100 | // Set/check bad flag 101 | void SetBadFlag(); 102 | bool isBad(); 103 | 104 | // Compute Scene Depth (q=2 median). Used in monocular. 105 | float ComputeSceneMedianDepth(const int q); 106 | 107 | static bool weightComp(int a, int b) { return a > b; } 108 | 109 | static bool lId(KeyFrame* keyframe_1, KeyFrame* keyframe_2) { 110 | return keyframe_1->id_ < keyframe_2->id_; 111 | } 112 | 113 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 114 | 115 | // The following variables are accesed from only 1 thread or never change (no 116 | // mutex needed). 117 | public: 118 | static long unsigned int next_id_; 119 | long unsigned int id_; 120 | const long unsigned int frame_id_; 121 | 122 | const double timestamp_; 123 | 124 | // Grid (to speed up feature matching) 125 | const int grid_cols_; 126 | const int grid_rows_; 127 | const float grid_element_width_inv_; 128 | const float grid_element_height_inv_; 129 | 130 | // Variables used by the tracking 131 | long unsigned int track_reference_for_frame_; 132 | long unsigned int fuse_target_for_keyframe_; 133 | 134 | // Variables used by the local mapping 135 | long unsigned int n_BA_local_for_keyframe_; 136 | long unsigned int n_BA_fixed_for_keyframe_; 137 | 138 | // Variables used by the keyframe database 139 | long unsigned int n_loop_query_; 140 | int n_loop_words_; 141 | float loop_score_; 142 | long unsigned int reloc_query_; 143 | int n_reloc_words_; 144 | float reloc_score_; 145 | 146 | // Variables used by loop closing 147 | Eigen::Matrix4d global_BA_Tcw_; 148 | Eigen::Matrix4d global_BA_Bef_Tcw_; 149 | long unsigned int n_BA_global_for_keyframe_; 150 | 151 | // Calibration parameters 152 | const float fx_, fy_, cx_, cy_, invfx_, invfy_, bf_, mb_, threshold_depth_; 153 | 154 | // Number of KeyPoints 155 | const int N_; 156 | 157 | // KeyPoints, stereo coordinate and descriptors (all associated by an index) 158 | const std::vector keypoints_; 159 | const std::vector undistort_keypoints_; 160 | const std::vector mvuRight; // negative value for monocular points 161 | const std::vector depthes_; // negative value for monocular points 162 | const cv::Mat descriptors_; 163 | 164 | // BoW 165 | DBoW2::BowVector bow_vector_; 166 | DBoW2::FeatureVector feature_vector_; 167 | 168 | // Pose relative to parent (this is computed when bad flag is activated) 169 | Eigen::Matrix4d Tcp_; 170 | 171 | // Scale 172 | const int n_scale_levels_; 173 | const float scale_factor_; 174 | const float log_scale_factor_; 175 | const std::vector scale_factors_; 176 | const std::vector level_sigma2s_; 177 | const std::vector inv_level_sigma2s_; 178 | 179 | // Image bounds and calibration 180 | const int min_x_; 181 | const int min_y_; 182 | const int max_x_; 183 | const int max_y_; 184 | const cv::Mat K_; 185 | 186 | // The following variables need to be accessed trough a mutex to be thread 187 | // safe. 188 | protected: 189 | // SE3 Pose and camera center 190 | Eigen::Matrix4d Tcw_; 191 | Eigen::Matrix4d Twc_; 192 | Eigen::Vector3d Ow_; 193 | 194 | // MapPoints associated to keypoints 195 | std::vector map_points_; 196 | 197 | // BoW 198 | KeyFrameDatabase* keyframe_database_; 199 | ORBVocabulary* orb_vocabulary_; 200 | 201 | // Grid over the image to speed up feature matching 202 | std::vector > > grid_; 203 | 204 | std::map connected_keyframe_weights_; 205 | std::vector ordered_connected_keyframes_; 206 | std::vector ordered_weights_; 207 | 208 | // Spanning Tree and Loop Edges 209 | bool is_first_connection_; 210 | KeyFrame* parent_; 211 | std::set childrens_; 212 | std::set loop_edges_; 213 | 214 | // Bad flags 215 | bool do_not_erase_; 216 | bool do_to_be_erased_; 217 | bool is_bad_; 218 | 219 | float half_baseline_; // Only for visualization 220 | 221 | Map* map_; 222 | 223 | std::mutex mutex_pose_; 224 | std::mutex mutex_connections_; 225 | std::mutex mutex_features_; 226 | }; 227 | } // namespace ORB_SLAM2 228 | 229 | #endif // KEYFRAME_H_ 230 | -------------------------------------------------------------------------------- /include/KeyFrameDatabase.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 5 | * of Zaragoza) 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 KEY_FRAME_DATA_BASE_H_ 22 | #define KEY_FRAME_DATA_BASE_H_ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "Frame.h" 30 | #include "KeyFrame.h" 31 | #include "ORBVocabulary.h" 32 | 33 | namespace ORB_SLAM2 { 34 | class KeyFrame; 35 | class Frame; 36 | 37 | class KeyFrameDatabase { 38 | public: 39 | KeyFrameDatabase(const ORBVocabulary& voc); 40 | 41 | void add(KeyFrame* keyframe); 42 | 43 | void erase(KeyFrame* keyframe); 44 | 45 | void clear(); 46 | 47 | // Loop Detection 48 | std::vector DetectLoopCandidates(KeyFrame* keyframe, 49 | float minScore); 50 | 51 | // Relocalization 52 | std::vector DetectRelocalizationCandidates(Frame* frame); 53 | 54 | protected: 55 | // Associated vocabulary 56 | const ORBVocabulary* orb_vocabulary_; 57 | 58 | // Inverted file 59 | std::vector > inverted_files_; 60 | 61 | // Mutex 62 | std::mutex mutex_; 63 | }; 64 | } // namespace ORB_SLAM2 65 | 66 | #endif 67 | -------------------------------------------------------------------------------- /include/LocalMapping.h: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: LocalMapping.h 6 | * 7 | * Created On: Wed 04 Sep 2019 04:37:00 PM CST 8 | * Licensed under The GPLv3 License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | /** 12 | * This file is part of ORB-SLAM2. 13 | * 14 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 15 | * of Zaragoza) For more information see 16 | * 17 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 18 | * it under the terms of the GNU General Public License as published by 19 | * the Free Software Foundation, either version 3 of the License, or 20 | * (at your option) any later version. 21 | * 22 | * ORB-SLAM2 is distributed in the hope that it will be useful, 23 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 24 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 25 | * GNU General Public License for more details. 26 | * 27 | * You should have received a copy of the GNU General Public License 28 | * along with ORB-SLAM2. If not, see . 29 | */ 30 | 31 | #ifndef LOCALMAPPING_H_ 32 | #define LOCALMAPPING_H_ 33 | 34 | #include "KeyFrame.h" 35 | #include "KeyFrameDatabase.h" 36 | #include "LoopClosing.h" 37 | #include "Map.h" 38 | #include "Tracking.h" 39 | 40 | #include 41 | #include 42 | 43 | namespace ORB_SLAM2 { 44 | 45 | class Tracking; 46 | class LoopClosing; 47 | class Map; 48 | 49 | class LocalMapping { 50 | public: 51 | LocalMapping(Map* map, const float is_monocular); 52 | 53 | void SetLoopCloser(LoopClosing* loop_closer); 54 | 55 | void SetTracker(Tracking* tracker); 56 | 57 | // Main function 58 | void Run(); 59 | 60 | void InsertKeyFrame(KeyFrame* keyframe); 61 | 62 | // Thread Synch 63 | void RequestStop(); 64 | void RequestReset(); 65 | bool Stop(); 66 | void Release(); 67 | bool isStopped(); 68 | bool stopRequested(); 69 | bool AcceptKeyFrames(); 70 | void SetAcceptKeyFrames(bool flag); 71 | bool SetNotStop(bool flag); 72 | 73 | void InterruptBA(); 74 | 75 | void RequestFinish(); 76 | bool isFinished(); 77 | 78 | int KeyframesInQueue() { 79 | unique_lock lock(mutex_new_keyframes_); 80 | return new_keyframes_.size(); 81 | } 82 | 83 | protected: 84 | bool CheckNewKeyFrames(); 85 | void ProcessNewKeyFrame(); 86 | void CreateNewMapPoints(); 87 | 88 | void MapPointCulling(); 89 | void SearchInNeighbors(); 90 | 91 | void KeyFrameCulling(); 92 | 93 | Eigen::Matrix3d ComputeF12(KeyFrame*& pKF1, KeyFrame*& pKF2); 94 | 95 | Eigen::Matrix3d SkewSymmetricMatrix(const Eigen::Vector3d& v); 96 | 97 | bool is_monocular_; 98 | 99 | void ResetIfRequested(); 100 | bool is_reset_requested_; 101 | std::mutex mutex_reset_; 102 | 103 | bool CheckFinish(); 104 | void SetFinish(); 105 | bool is_finish_requested_; 106 | bool is_finished_; 107 | std::mutex mutex_finish_; 108 | 109 | Map* map_; 110 | 111 | LoopClosing* loop_closer_; 112 | Tracking* tracker_; 113 | 114 | std::list new_keyframes_; 115 | 116 | KeyFrame* current_keyframe_; 117 | 118 | std::list recent_added_map_points_; 119 | 120 | std::mutex mutex_new_keyframes_; 121 | 122 | bool is_abort_BA_; 123 | 124 | bool is_stopped_; 125 | bool is_stop_requested_; 126 | bool do_not_stop_; 127 | std::mutex mutex_stop_; 128 | 129 | bool do_accept_keyframes_; 130 | std::mutex mutex_accept_; 131 | }; 132 | } // namespace ORB_SLAM2 133 | 134 | #endif 135 | -------------------------------------------------------------------------------- /include/LoopClosing.h: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: LoopClosing.h 6 | * 7 | * Created On: Wed 04 Sep 2019 05:35:12 PM CST 8 | * Licensed under The GPLv3 License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | /** 12 | * This file is part of ORB-SLAM2. 13 | * 14 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 15 | * of Zaragoza) For more information see 16 | * 17 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 18 | * it under the terms of the GNU General Public License as published by 19 | * the Free Software Foundation, either version 3 of the License, or 20 | * (at your option) any later version. 21 | * 22 | * ORB-SLAM2 is distributed in the hope that it will be useful, 23 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 24 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 25 | * GNU General Public License for more details. 26 | * 27 | * You should have received a copy of the GNU General Public License 28 | * along with ORB-SLAM2. If not, see . 29 | */ 30 | 31 | #ifndef LOOPCLOSING_H_ 32 | #define LOOPCLOSING_H_ 33 | 34 | #include "KeyFrame.h" 35 | #include "LocalMapping.h" 36 | #include "Map.h" 37 | #include "ORBVocabulary.h" 38 | #include "Tracking.h" 39 | 40 | #include "KeyFrameDatabase.h" 41 | // #include "CeresSim3.h" 42 | #include 43 | 44 | #include 45 | #include 46 | 47 | namespace ORB_SLAM2 { 48 | 49 | class Tracking; 50 | class LocalMapping; 51 | class KeyFrameDatabase; 52 | 53 | class LoopClosing { 54 | public: 55 | typedef std::pair, int> ConsistentGroup; 56 | 57 | typedef std::map< 58 | KeyFrame*, Sophus::Sim3d, std::less, 59 | Eigen::aligned_allocator > > 60 | KeyFrameAndSim3; 61 | 62 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 63 | 64 | public: 65 | LoopClosing(Map* map, KeyFrameDatabase* keyframe_database, 66 | ORBVocabulary* orb_vocabulary, const bool is_fix_scale); 67 | 68 | void SetTracker(Tracking* tracker); 69 | 70 | void SetLocalMapper(LocalMapping* local_mapper); 71 | 72 | // Main function 73 | void Run(); 74 | 75 | void InsertKeyFrame(KeyFrame* keyframe); 76 | 77 | void RequestReset(); 78 | 79 | // This function will run in a separate thread 80 | void RunGlobalBundleAdjustment(unsigned long nLoopKF); 81 | 82 | bool isRunningGBA() { 83 | unique_lock lock(mutex_global_BA_); 84 | return is_running_global_BA_; 85 | } 86 | bool isFinishedGBA() { 87 | unique_lock lock(mutex_global_BA_); 88 | return is_finished_global_BA_; 89 | } 90 | 91 | void RequestFinish(); 92 | 93 | bool isFinished(); 94 | 95 | protected: 96 | bool CheckNewKeyFrames(); 97 | 98 | bool DetectLoop(); 99 | 100 | bool ComputeSim3(); 101 | 102 | void SearchAndFuse(const KeyFrameAndSim3& CorrectedPosesMap); 103 | 104 | void CorrectLoop(); 105 | 106 | void ResetIfRequested(); 107 | bool is_reset_requested_; 108 | std::mutex mutex_reset_; 109 | 110 | bool CheckFinish(); 111 | void SetFinish(); 112 | bool is_finish_requested_; 113 | bool is_finished_; 114 | std::mutex mutex_finish_; 115 | 116 | Map* map_; 117 | Tracking* tracker_; 118 | 119 | KeyFrameDatabase* keyframe_database_; 120 | ORBVocabulary* orb_vocabulary_; 121 | 122 | LocalMapping* local_mapper_; 123 | 124 | std::list loop_keyframe_queue_; 125 | 126 | std::mutex mutex_loop_queue_; 127 | 128 | // Loop detector parameters 129 | float covisibility_consistency_threshold_; 130 | 131 | // Loop detector variables 132 | KeyFrame* current_keyframe_; 133 | KeyFrame* matched_keyframe_; 134 | std::vector consistent_groups_; 135 | std::vector enough_consistent_candidates_; 136 | std::vector current_connected_keyframes_; 137 | std::vector current_matched_map_points_; 138 | std::vector loop_map_points_; 139 | Eigen::Matrix4d Scw_; 140 | Sophus::Sim3d sophus_sim3_Scw_; 141 | 142 | long unsigned int last_loop_keyframe_id_; 143 | 144 | // Variables related to Global Bundle Adjustment 145 | bool is_running_global_BA_; 146 | bool is_finished_global_BA_; 147 | bool is_stop_global_BA_; 148 | std::mutex mutex_global_BA_; 149 | std::thread* thread_global_BA_; 150 | 151 | // Fix scale in the stereo/RGB-D case 152 | bool is_fix_scale_; 153 | 154 | int full_BA_index_; 155 | }; 156 | } // namespace ORB_SLAM2 157 | 158 | #endif // LOOPCLOSING_H 159 | -------------------------------------------------------------------------------- /include/Map.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 5 | * of Zaragoza) 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 25 | #include 26 | 27 | #include "KeyFrame.h" 28 | #include "MapPoint.h" 29 | namespace ORB_SLAM2 { 30 | 31 | class MapPoint; 32 | class KeyFrame; 33 | 34 | class Map { 35 | public: 36 | Map(); 37 | 38 | void AddKeyFrame(KeyFrame* keyframe); 39 | void AddMapPoint(MapPoint* map_point); 40 | void EraseMapPoint(MapPoint* map_point); 41 | void EraseKeyFrame(KeyFrame* KeyFrame); 42 | void SetReferenceMapPoints(const std::vector& map_points); 43 | void InformNewBigChange(); 44 | int GetLastBigChangeIdx(); 45 | 46 | std::vector GetAllKeyFrames(); 47 | std::vector GetAllMapPoints(); 48 | std::vector GetReferenceMapPoints(); 49 | 50 | long unsigned int MapPointsInMap(); 51 | long unsigned KeyFramesInMap(); 52 | 53 | long unsigned int GetMaxKFid(); 54 | 55 | void clear(); 56 | 57 | std::vector keyframe_origins_; 58 | 59 | std::mutex mutex_map_update_; 60 | 61 | // This avoid that two points are created simultaneously in separate threads 62 | // (id conflict) 63 | std::mutex mutex_point_creation_; 64 | 65 | protected: 66 | std::set map_points_; 67 | std::set keyframes_; 68 | 69 | std::vector reference_map_points_; 70 | 71 | long unsigned int max_keyframe_id_; 72 | 73 | // Index related to a big change in the map (loop closure, global BA) 74 | int big_change_index_; 75 | 76 | std::mutex mutex_map_; 77 | }; 78 | } // namespace ORB_SLAM2 79 | 80 | #endif 81 | -------------------------------------------------------------------------------- /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 | 28 | #include 29 | #include 30 | 31 | namespace ORB_SLAM2 32 | { 33 | 34 | class MapDrawer 35 | { 36 | public: 37 | MapDrawer(Map* pMap, const string &strSettingPath); 38 | 39 | Map* mpMap; 40 | 41 | void DrawMapPoints(); 42 | void DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph); 43 | void DrawCurrentCamera(pangolin::OpenGlMatrix &Twc); 44 | void SetCurrentCameraPose(const Eigen::Matrix4d &Tcw); 45 | void SetReferenceKeyFrame(KeyFrame *pKF); 46 | void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M); 47 | 48 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 49 | 50 | private: 51 | 52 | float mKeyFrameSize; 53 | float mKeyFrameLineWidth; 54 | float mGraphLineWidth; 55 | float mPointSize; 56 | float mCameraSize; 57 | float mCameraLineWidth; 58 | 59 | Eigen::Matrix4d camera_pose_; 60 | 61 | std::mutex mMutexCamera; 62 | }; 63 | 64 | } //namespace ORB_SLAM 65 | 66 | #endif // MAPDRAWER_H 67 | -------------------------------------------------------------------------------- /include/MapPoint.h: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: MapPoint.h 6 | * 7 | * Created On: Thu 05 Sep 2019 10:38:24 AM CST 8 | * Licensed under The GPLv3 License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | /** 12 | * This file is part of ORB-SLAM2. 13 | * 14 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 15 | * of Zaragoza) For more information see 16 | * 17 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 18 | * it under the terms of the GNU General Public License as published by 19 | * the Free Software Foundation, either version 3 of the License, or 20 | * (at your option) any later version. 21 | * 22 | * ORB-SLAM2 is distributed in the hope that it will be useful, 23 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 24 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 25 | * GNU General Public License for more details. 26 | * 27 | * You should have received a copy of the GNU General Public License 28 | * along with ORB-SLAM2. If not, see . 29 | */ 30 | 31 | #ifndef MAPPOINT_H_ 32 | #define MAPPOINT_H_ 33 | 34 | #include "Frame.h" 35 | #include "KeyFrame.h" 36 | #include "Map.h" 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | namespace ORB_SLAM2 { 44 | 45 | class KeyFrame; 46 | class Map; 47 | class Frame; 48 | 49 | class MapPoint { 50 | public: 51 | MapPoint(const Eigen::Vector3d& pos, KeyFrame* reference_keyframe, Map* map); 52 | MapPoint(const Eigen::Vector3d& pos, Map* map, Frame* frame, const int& idxF); 53 | 54 | void SetWorldPos(const Eigen::Vector3d& pos); 55 | Eigen::Vector3d GetWorldPos(); 56 | 57 | Eigen::Vector3d GetNormal(); 58 | KeyFrame* GetReferenceKeyFrame(); 59 | 60 | std::map GetObservations(); 61 | int Observations(); 62 | 63 | void AddObservation(KeyFrame* keyframe, size_t index); 64 | void EraseObservation(KeyFrame* keyframe); 65 | 66 | int GetIndexInKeyFrame(KeyFrame* keyframe); 67 | bool IsInKeyFrame(KeyFrame* keyframe); 68 | 69 | void SetBadFlag(); 70 | bool isBad(); 71 | 72 | void Replace(MapPoint* pMP); 73 | MapPoint* GetReplaced(); 74 | 75 | void IncreaseVisible(int n = 1); 76 | void IncreaseFound(int n = 1); 77 | float GetFoundRatio(); 78 | inline int GetFound() { return n_found_; } 79 | 80 | void ComputeDistinctiveDescriptors(); 81 | 82 | cv::Mat GetDescriptor(); 83 | 84 | void UpdateNormalAndDepth(); 85 | 86 | float GetMinDistanceInvariance(); 87 | float GetMaxDistanceInvariance(); 88 | int PredictScale(const float& currentDist, KeyFrame* keyframe); 89 | int PredictScale(const float& currentDist, Frame* pF); 90 | 91 | static bool lId(MapPoint* map_point_1, MapPoint* map_point_2) { 92 | return map_point_1->id_ < map_point_2->id_; 93 | } 94 | 95 | public: 96 | long unsigned int id_; 97 | static long unsigned int next_id_; 98 | long int first_keyframe_id_; 99 | long int first_frame_id_; 100 | int n_observations_; 101 | 102 | // Variables used by the tracking 103 | float track_proj_x_; 104 | float track_proj_y_; 105 | float track_proj_x_r_; 106 | bool is_track_in_view_; 107 | int track_scale_level_; 108 | float track_view_cos_; 109 | long unsigned int track_reference_for_frame_; 110 | long unsigned int last_seen_frame_id_; 111 | 112 | // Variables used by local mapping 113 | long unsigned int n_BA_local_for_keyframe_; 114 | long unsigned int fuse_candidate_for_keyframe; 115 | 116 | // Variables used by loop closing 117 | long unsigned int loop_point_for_keyframe_; 118 | long unsigned int corrected_by_keyframe_; 119 | long unsigned int corrected_reference_; 120 | Eigen::Vector3d global_BA_pose_; 121 | long unsigned n_BA_global_for_keyframe_; 122 | 123 | static std::mutex global_mutex_; 124 | 125 | protected: 126 | // Position in absolute coordinates 127 | Eigen::Vector3d world_pose_; 128 | 129 | // Keyframes observing the point and associated index in keyframe 130 | std::map observations_; 131 | 132 | // Mean viewing direction 133 | Eigen::Vector3d normal_vector_; 134 | 135 | // Best descriptor to fast matching 136 | cv::Mat descriptor_; 137 | 138 | // Reference KeyFrame 139 | KeyFrame* reference_keyframe_; 140 | 141 | // Tracking counters 142 | int n_visible_; 143 | int n_found_; 144 | 145 | // Bad flag (we do not currently erase MapPoint from memory) 146 | bool is_bad_; 147 | MapPoint* replaced_map_point_; 148 | 149 | // Scale invariance distances 150 | float min_distance_; 151 | float max_distance_; 152 | 153 | Map* map_; 154 | 155 | std::mutex mutex_pose_; 156 | std::mutex mutex_features_; 157 | }; 158 | 159 | } // namespace ORB_SLAM2 160 | 161 | #endif // MAPPOINT_H 162 | -------------------------------------------------------------------------------- /include/MatEigenConverter.h: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: MatEigenConverter.h 6 | * 7 | * Created On: Fri 20 Sep 2019 06:40:20 PM CST 8 | * Licensed under The GPLv3 License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | 12 | #ifndef MAT_EIGEN_CONVERTER_H_ 13 | #define MAT_EIGEN_CONVERTER_H_ 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "LoopClosing.h" 20 | 21 | class MatEigenConverter { 22 | public: 23 | static std::vector toDescriptorVector(const cv::Mat& Descriptors); 24 | 25 | // Transpose 26 | static Eigen::Matrix4d MatToMatrix4d(const cv::Mat& T); 27 | static cv::Mat Matrix4dToMat(const Eigen::Matrix4d& T); 28 | 29 | // Rotation 30 | static Eigen::Matrix3d MatToMatrix3d(const cv::Mat& R); 31 | static cv::Mat Matrix3dToMat(const Eigen::Matrix3d& R); 32 | 33 | // translation 34 | static Eigen::Vector3d MatToVector3d(const cv::Mat& t); 35 | static cv::Mat Vector3dToMat(const Eigen::Vector3d& t); 36 | 37 | // Matrix_7_1 38 | static Eigen::Matrix Matrix4dToMatrix_7_1( 39 | const Eigen::Matrix4d& pose); 40 | static Eigen::Matrix4d Matrix_7_1_ToMatrix4d( 41 | const Eigen::Matrix& Tcw_7_1); 42 | }; 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /include/MonoORBSlam.h: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: MonoORBSlam.h 6 | * 7 | * Created On: Thu 05 Sep 2019 12:05:02 PM CST 8 | * Licensed under The GPLv3 License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | 12 | #ifndef MONO_ORB_SLAM_H_ 13 | #define MONO_ORB_SLAM_H_ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include "FrameDrawer.h" 22 | #include "KeyFrameDatabase.h" 23 | #include "LocalMapping.h" 24 | #include "LoopClosing.h" 25 | #include "Map.h" 26 | #include "MapDrawer.h" 27 | #include "ORBVocabulary.h" 28 | #include "Tracking.h" 29 | #include "Viewer.h" 30 | #include "MatEigenConverter.h" 31 | 32 | namespace ORB_SLAM2 { 33 | 34 | class Viewer; 35 | class FrameDrawer; 36 | class Map; 37 | class Tracking; 38 | class LocalMapping; 39 | class LoopClosing; 40 | 41 | class MonoORBSlam { 42 | public: 43 | MonoORBSlam(const string& voc_file, const string& string_setting_file, 44 | const bool is_use_viewer = true); 45 | 46 | Eigen::Matrix4d TrackMonocular(const cv::Mat& img, const double& timestamp); 47 | 48 | // This stops local mapping thread (map building) and performs only camera 49 | // tracking. 50 | void ActivateLocalizationMode(); 51 | // This resumes local mapping thread and performs SLAM again. 52 | void DeactivateLocalizationMode(); 53 | 54 | // Returns true if there have been a big map change (loop closure, global BA) 55 | // since last call to this function 56 | bool MapChanged(); 57 | 58 | // Reset the system (clear map) 59 | void Reset(); 60 | 61 | // All threads will be requested to finish. 62 | // It waits until all threads have finished. 63 | // This function must be called before saving the trajectory. 64 | void Shutdown(); 65 | 66 | // Save camera trajectory in the TUM RGB-D dataset format. 67 | // Only for stereo and RGB-D. This method does not work for monocular. 68 | // Call first Shutdown() 69 | // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset 70 | void SaveTrajectoryTUM(const string& filename); 71 | 72 | // Save keyframe poses in the TUM RGB-D dataset format. 73 | // This method works for all sensor input. 74 | // Call first Shutdown() 75 | // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset 76 | void SaveKeyFrameTrajectoryTUM(const string& filename); 77 | 78 | // TODO: Save/Load functions 79 | void SaveMap(const string &filename); 80 | // LoadMap(const string &filename); 81 | 82 | // Information from most recent processed frame 83 | // You can call this right after TrackMonocular (or stereo or RGBD) 84 | int GetTrackingState(); 85 | std::vector GetTrackedMapPoints(); 86 | std::vector GetTrackedKeyPointsUn(); 87 | 88 | private: 89 | // ORB vocabulary used for place recognition and feature matching. 90 | ORBVocabulary* vocabulary_; 91 | 92 | // KeyFrame database for place recognition (relocalization and loop 93 | // detection). 94 | KeyFrameDatabase* keyframe_database_; 95 | 96 | // Map structure that stores the pointers to all KeyFrames and MapPoints. 97 | Map* map_; 98 | 99 | // Tracker. It receives a frame and computes the associated camera pose. 100 | // It also decides when to insert a new keyframe, create some new MapPoints 101 | // and performs relocalization if tracking fails. 102 | Tracking* tracker_; 103 | 104 | // Local Mapper. It manages the local map and performs local bundle 105 | // adjustment. 106 | LocalMapping* local_mapper_; 107 | 108 | // Loop Closer. It searches loops with every new keyframe. If there is a 109 | // loop it performs a pose graph optimization and full bundle adjustment 110 | // (in a new thread) afterwards. 111 | LoopClosing* loop_closer_; 112 | 113 | // The viewer draws the map and the current camera pose. It uses Pangolin. 114 | Viewer* viewer_; 115 | 116 | FrameDrawer* frame_drawer_; 117 | MapDrawer* map_drawer_; 118 | 119 | // System threads: Local Mapping, Loop Closing, Viewer. 120 | // The Tracking thread "lives" in the main execution thread that creates the 121 | // System object. 122 | std::thread* thread_local_mapping_; 123 | std::thread* thread_loop_closing_; 124 | std::thread* thread_viewer_; 125 | 126 | // Reset flag 127 | std::mutex mutex_reset_; 128 | bool do_reset_; 129 | 130 | // Change mode flags 131 | std::mutex mutex_mode_; 132 | bool is_activate_localization_mode_; 133 | bool is_deactivate_localization_mode_; 134 | 135 | // Tracking state 136 | int tracking_state_; 137 | std::vector tracked_map_points_; 138 | std::vector tracked_undistort_keypoints_; 139 | std::mutex mutex_state_; 140 | }; 141 | 142 | } // namespace ORB_SLAM2 143 | 144 | #endif 145 | -------------------------------------------------------------------------------- /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"lib/DBoW2/DBoW2/FORB.h" 26 | #include"lib/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 | -------------------------------------------------------------------------------- /include/ORBmatcher.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 5 | * of Zaragoza) 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 ORBMATCHER_H 22 | #define ORBMATCHER_H 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include "Frame.h" 29 | #include "KeyFrame.h" 30 | #include "MapPoint.h" 31 | 32 | namespace ORB_SLAM2 { 33 | 34 | class ORBmatcher { 35 | public: 36 | ORBmatcher(float nnratio = 0.6, bool checkOri = true); 37 | 38 | // Computes the Hamming distance between two ORB descriptors 39 | static int DescriptorDistance(const cv::Mat& a, const cv::Mat& b); 40 | 41 | // Search matches between Frame keypoints and projected MapPoints. Returns 42 | // number of matches Used to track the local map (Tracking) 43 | int SearchByProjection(Frame& F, const std::vector& vpMapPoints, 44 | const float th = 3); 45 | 46 | // Project MapPoints tracked in last frame into the current frame and search 47 | // matches. Used to track from previous frame (Tracking) 48 | int SearchByProjection(Frame& CurrentFrame, const Frame& LastFrame, 49 | const float th); 50 | 51 | // Project MapPoints seen in KeyFrame into the Frame and search matches. 52 | // Used in relocalisation (Tracking) 53 | int SearchByProjection(Frame& CurrentFrame, KeyFrame* pKF, 54 | const std::set& sAlreadyFound, 55 | const float th, const int ORBdist); 56 | 57 | // Project MapPoints using a Similarity Transformation and search matches. 58 | // Used in loop detection (Loop Closing) 59 | int SearchByProjection(KeyFrame* pKF, const Eigen::Matrix4d& Scw, 60 | const std::vector& vpPoints, 61 | std::vector& vpMatched, int th); 62 | 63 | // Search matches between MapPoints in a KeyFrame and ORB in a Frame. 64 | // Brute force constrained to ORB that belong to the same vocabulary node (at 65 | // a certain level) Used in Relocalisation and Loop Detection 66 | int SearchByBoW(KeyFrame* pKF, Frame& F, 67 | std::vector& vpMapPointMatches); 68 | int SearchByBoW(KeyFrame* pKF1, KeyFrame* pKF2, 69 | std::vector& vpMatches12); 70 | 71 | // Matching for the Map Initialization (only used in the monocular case) 72 | int SearchForInitialization(Frame& F1, Frame& F2, 73 | std::vector& vbPrevMatched, 74 | std::vector& vnMatches12, 75 | int windowSize = 10); 76 | 77 | // Matching to triangulate new MapPoints. Check Epipolar Constraint. 78 | int SearchForTriangulation(KeyFrame* pKF1, KeyFrame* pKF2, 79 | const Eigen::Matrix3d& F12, 80 | std::vector >& vMatchedPairs, 81 | const bool bOnlyStereo); 82 | 83 | // Search matches between MapPoints seen in KF1 and KF2 transforming by a Sim3 84 | // [s12*R12|t12] In the stereo and RGB-D case, s12=1 85 | int SearchBySim3(KeyFrame* pKF1, KeyFrame* pKF2, 86 | std::vector& vpMatches12, const float& s12, 87 | const Eigen::Matrix3d& R12, const Eigen::Vector3d& t12, 88 | const float th); 89 | 90 | // Project MapPoints into KeyFrame and search for duplicated MapPoints. 91 | int Fuse(KeyFrame* pKF, const vector& vpMapPoints, 92 | const float th = 3.0); 93 | 94 | // Project MapPoints into KeyFrame using a given Sim3 and search for 95 | // duplicated MapPoints. 96 | int Fuse(KeyFrame* pKF, Eigen::Matrix4d Scw, const std::vector& vpPoints, 97 | float th, vector& vpReplacePoint); 98 | 99 | public: 100 | static const int TH_LOW; 101 | static const int TH_HIGH; 102 | static const int HISTO_LENGTH; 103 | 104 | protected: 105 | bool CheckDistEpipolarLine(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, 106 | const Eigen::Matrix3d& F12, const KeyFrame* pKF); 107 | 108 | float RadiusByViewingCos(const float& viewCos); 109 | 110 | void ComputeThreeMaxima(std::vector* histo, const int L, int& ind1, 111 | int& ind2, int& ind3); 112 | 113 | float mfNNratio; 114 | bool mbCheckOrientation; 115 | }; 116 | 117 | } // namespace ORB_SLAM2 118 | 119 | #endif // ORBMATCHER_H 120 | -------------------------------------------------------------------------------- /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 | Eigen::Matrix4d find(vector &vbInliers, int &nInliers); 71 | 72 | Eigen::Matrix4d iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers); 73 | 74 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 75 | private: 76 | 77 | void CheckInliers(); 78 | bool Refine(); 79 | 80 | // Functions from the original EPnP code 81 | void set_maximum_number_of_correspondences(const int n); 82 | void reset_correspondences(void); 83 | void add_correspondence(const double X, const double Y, const double Z, 84 | const double u, const double v); 85 | 86 | double compute_pose(double R[3][3], double T[3]); 87 | 88 | void relative_error(double & rot_err, double & transl_err, 89 | const double Rtrue[3][3], const double ttrue[3], 90 | const double Rest[3][3], const double test[3]); 91 | 92 | void print_pose(const double R[3][3], const double t[3]); 93 | double reprojection_error(const double R[3][3], const double t[3]); 94 | 95 | void choose_control_points(void); 96 | void compute_barycentric_coordinates(void); 97 | void fill_M(CvMat * M, const int row, const double * alphas, const double u, const double v); 98 | void compute_ccs(const double * betas, const double * ut); 99 | void compute_pcs(void); 100 | 101 | void solve_for_sign(void); 102 | 103 | void find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho, double * betas); 104 | void find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, double * betas); 105 | void find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho, double * betas); 106 | void qr_solve(CvMat * A, CvMat * b, CvMat * X); 107 | 108 | double dot(const double * v1, const double * v2); 109 | double dist2(const double * p1, const double * p2); 110 | 111 | void compute_rho(double * rho); 112 | void compute_L_6x10(const double * ut, double * l_6x10); 113 | 114 | void gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double current_betas[4]); 115 | void compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho, 116 | double cb[4], CvMat * A, CvMat * b); 117 | 118 | double compute_R_and_t(const double * ut, const double * betas, 119 | double R[3][3], double t[3]); 120 | 121 | void estimate_R_and_t(double R[3][3], double t[3]); 122 | 123 | void copy_R_and_t(const double R_dst[3][3], const double t_dst[3], 124 | double R_src[3][3], double t_src[3]); 125 | 126 | void mat_to_quat(const double R[3][3], double q[4]); 127 | 128 | 129 | double uc, vc, fu, fv; 130 | 131 | double * pws, * us, * alphas, * pcs; 132 | int maximum_number_of_correspondences; 133 | int number_of_correspondences; 134 | 135 | double cws[4][3], ccs[4][3]; 136 | double cws_determinant; 137 | 138 | vector mvpMapPointMatches; 139 | 140 | // 2D Points 141 | vector mvP2D; 142 | vector mvSigma2; 143 | 144 | // 3D Points 145 | vector mvP3Dw; 146 | 147 | // Index in Frame 148 | vector mvKeyPointIndices; 149 | 150 | // Current Estimation 151 | double mRi[3][3]; 152 | double mti[3]; 153 | Eigen::Matrix4d mTcwi; 154 | vector mvbInliersi; 155 | int mnInliersi; 156 | 157 | // Current Ransac State 158 | int mnIterations; 159 | vector mvbBestInliers; 160 | int mnBestInliers; 161 | Eigen::Matrix4d mBestTcw; 162 | 163 | // Refined 164 | Eigen::Matrix4d mRefinedTcw; 165 | vector mvbRefinedInliers; 166 | int mnRefinedInliers; 167 | 168 | // Number of Correspondences 169 | int N; 170 | 171 | // Indices for random selection [0 .. N-1] 172 | vector mvAllIndices; 173 | 174 | // RANSAC probability 175 | double mRansacProb; 176 | 177 | // RANSAC min inliers 178 | int mRansacMinInliers; 179 | 180 | // RANSAC max iterations 181 | int mRansacMaxIts; 182 | 183 | // RANSAC expected inliers/total ratio 184 | float mRansacEpsilon; 185 | 186 | // RANSAC Threshold inlier/outlier. Max error e = dist(P1,T_12*P2)^2 187 | float mRansacTh; 188 | 189 | // RANSAC Minimun Set used at each iteration 190 | int mRansacMinSet; 191 | 192 | // Max square error associated with scale level. Max error = th*th*sigma(level)*sigma(level) 193 | vector mvMaxError; 194 | 195 | }; 196 | 197 | } //namespace ORB_SLAM 198 | 199 | #endif //PNPSOLVER_H 200 | -------------------------------------------------------------------------------- /include/Sim3Solver.h: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: Sim3Solver.h 6 | * 7 | * Created On: Fri 20 Sep 2019 05:49:35 PM CST 8 | * Licensed under The GPLv3 License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | /** 12 | * This file is part of ORB-SLAM2. 13 | * 14 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 15 | * of Zaragoza) For more information see 16 | * 17 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 18 | * it under the terms of the GNU General Public License as published by 19 | * the Free Software Foundation, either version 3 of the License, or 20 | * (at your option) any later version. 21 | * 22 | * ORB-SLAM2 is distributed in the hope that it will be useful, 23 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 24 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 25 | * GNU General Public License for more details. 26 | * 27 | * You should have received a copy of the GNU General Public License 28 | * along with ORB-SLAM2. If not, see . 29 | */ 30 | 31 | #ifndef SIM3SOLVER_H 32 | #define SIM3SOLVER_H 33 | 34 | #include 35 | #include 36 | 37 | #include "KeyFrame.h" 38 | 39 | namespace ORB_SLAM2 { 40 | 41 | class Sim3Solver { 42 | public: 43 | Sim3Solver(KeyFrame* keyframe_1, KeyFrame* keyframe_2, 44 | const std::vector& matched_points_1_in_2, 45 | const bool is_fixed_scale = true); 46 | 47 | void SetRansacParameters(double probability = 0.99, int min_inliers = 6, 48 | int max_iterations = 300); 49 | 50 | Eigen::Matrix4d find(std::vector& vbInliers12, int& n_inliers); 51 | 52 | Eigen::Matrix4d iterate(int n_iterations, bool& is_no_more, 53 | std::vector& is_inliers, int& n_inliers); 54 | 55 | Eigen::Matrix3d GetEstimatedRotation(); 56 | Eigen::Vector3d GetEstimatedTranslation(); 57 | float GetEstimatedScale(); 58 | 59 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 60 | 61 | protected: 62 | void ComputeCentroid(Eigen::Matrix3d& P, Eigen::Matrix3d& Pr, 63 | Eigen::Vector3d& C); 64 | 65 | void ComputeSim3(Eigen::Matrix3d& P1, Eigen::Matrix3d& P2); 66 | 67 | void CheckInliers(); 68 | 69 | void Project(const std::vector& P3Dsw, 70 | std::vector& P2Ds, Eigen::Matrix4d Tcw, 71 | Eigen::Matrix3d& K); 72 | void FromCameraToImage(const std::vector& vP3Dc, 73 | std::vector& P2Ds, 74 | Eigen::Matrix3d& K); 75 | 76 | protected: 77 | // KeyFrames and matches 78 | KeyFrame* keyframe_1_; 79 | KeyFrame* keyframe_2_; 80 | 81 | std::vector X3Dsc1_; 82 | std::vector X3Dsc2_; 83 | std::vector map_points_1_; 84 | std::vector map_points_2_; 85 | std::vector matched_points_1_in_2_; 86 | std::vector matched_indices_1_; 87 | std::vector max_errors_1_; 88 | std::vector max_errors_2_; 89 | 90 | int N_; 91 | int N1_; 92 | 93 | // Current Estimation 94 | Eigen::Matrix3d R12i_; 95 | Eigen::Vector3d t12i_; 96 | float scale_12_i_; 97 | Eigen::Matrix4d T12i_; 98 | Eigen::Matrix4d T21i_; 99 | std::vector is_inliers_i_; 100 | int n_inliers_i_; 101 | 102 | // Current Ransac State 103 | int n_iterations_; 104 | std::vector is_best_inliers_; 105 | int n_best_inliers_; 106 | Eigen::Matrix4d best_T12_; 107 | Eigen::Matrix3d best_rotation_; 108 | Eigen::Vector3d best_translation_; 109 | float best_scale_; 110 | 111 | // Scale is fixed to 1 in the stereo/RGBD case 112 | bool is_fixed_scale_; 113 | 114 | // Indices for random selection 115 | std::vector all_indices_; 116 | 117 | // Projections 118 | std::vector points_1_in_im_1_; 119 | std::vector points_2_in_im_2_; 120 | 121 | // RANSAC probability 122 | double ransac_probability_; 123 | 124 | // RANSAC min inliers 125 | int ransac_min_inliers_; 126 | 127 | // RANSAC max iterations 128 | int ransac_max_iterations_; 129 | 130 | // Threshold inlier/outlier. e = dist(Pi,T_ij*Pj)^2 < 5.991*sigma2 131 | // float thres_; 132 | // float sigma2_; 133 | 134 | // Calibration 135 | Eigen::Matrix3d K1_; 136 | Eigen::Matrix3d K2_; 137 | }; 138 | 139 | } // namespace ORB_SLAM2 140 | 141 | #endif // SIM3SOLVER_H 142 | -------------------------------------------------------------------------------- /include/Tracking.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 5 | * of Zaragoza) 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 TRACKING_H_ 22 | #define TRACKING_H_ 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include "Frame.h" 29 | #include "FrameDrawer.h" 30 | #include "Initializer.h" 31 | #include "KeyFrameDatabase.h" 32 | #include "LocalMapping.h" 33 | #include "LoopClosing.h" 34 | #include "Map.h" 35 | #include "MapDrawer.h" 36 | #include "MonoORBSlam.h" 37 | #include "ORBVocabulary.h" 38 | #include "ORBextractor.h" 39 | #include "Viewer.h" 40 | 41 | namespace ORB_SLAM2 { 42 | 43 | class Viewer; 44 | class FrameDrawer; 45 | class Map; 46 | class LocalMapping; 47 | class LoopClosing; 48 | class MonoORBSlam; 49 | 50 | class Tracking { 51 | public: 52 | Tracking(MonoORBSlam* mono_orb_slam, ORBVocabulary* vocabulary, 53 | FrameDrawer* frame_drawer, MapDrawer* map_drawer, Map* map, 54 | KeyFrameDatabase* keyframe_database, 55 | const string& string_setting_file); 56 | 57 | // Preprocess the input and call Track(). Extract features and performs 58 | Eigen::Matrix4d GrabImageMonocular(const cv::Mat& img, const double& timestamp); 59 | 60 | void SetLocalMapper(LocalMapping* local_mapper); 61 | void SetLoopClosing(LoopClosing* loop_closing); 62 | void SetViewer(Viewer* viewer); 63 | 64 | // Use this function if you have deactivated local mapping and you only want 65 | // to localize the camera. 66 | void InformOnlyTracking(const bool& flag); 67 | 68 | public: 69 | // Tracking states 70 | enum eTrackingState { 71 | SYSTEM_NOT_READY = -1, 72 | NO_IMAGES_YET = 0, 73 | NOT_INITIALIZED = 1, 74 | OK = 2, 75 | LOST = 3 76 | }; 77 | 78 | eTrackingState state_; 79 | eTrackingState last_processed_state_; 80 | 81 | // Current Frame 82 | Frame current_frame_; 83 | cv::Mat img_gray_; 84 | 85 | // Initialization Variables (Monocular) 86 | std::vector init_last_matches_; 87 | std::vector init_matches_; 88 | std::vector pre_matched_keypoints_; 89 | std::vector init_P3Ds_; 90 | Frame init_frame_; 91 | 92 | // Lists used to recover the full camera trajectory at the end of the 93 | // execution. Basically we store the reference keyframe for each frame and its 94 | // relative transformation 95 | list relative_frame_poses_; 96 | list reference_keyframes_; 97 | list frame_times_; 98 | list do_lostes_; 99 | 100 | // True if local mapping is deactivated and we are performing only 101 | // localization 102 | bool do_only_tracking_; 103 | 104 | void Reset(); 105 | 106 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 107 | 108 | protected: 109 | bool Mapping(); 110 | bool TrackingWithKnownMap(); 111 | // Main tracking function. It is independent of the input sensor. 112 | void Track(); 113 | 114 | // Map initialization for stereo and RGB-D 115 | void StereoInitialization(); 116 | 117 | // Map initialization for monocular 118 | void MonocularInitialization(); 119 | void CreateInitialMapMonocular(); 120 | 121 | void CheckReplacedInLastFrame(); 122 | bool TrackReferenceKeyFrame(); 123 | void UpdateLastFrame(); 124 | bool TrackWithMotionModel(); 125 | 126 | bool Relocalization(); 127 | 128 | void UpdateLocalMap(); 129 | void UpdateLocalPoints(); 130 | void UpdateLocalKeyFrames(); 131 | 132 | bool TrackLocalMap(); 133 | void SearchLocalPoints(); 134 | 135 | bool NeedNewKeyFrame(); 136 | void CreateNewKeyFrame(); 137 | 138 | // In case of performing only localization, this flag is true when there are 139 | // no matches to points in the map. Still tracking will continue if there are 140 | // enough matches with temporal points. In that case we are doing visual 141 | // odometry. The system will try to do relocalization to recover "zero-drift" 142 | // localization to the map. 143 | bool do_vo_; 144 | 145 | // Other Thread Pointers 146 | LocalMapping* local_mapper_; 147 | LoopClosing* loop_closing_; 148 | 149 | // ORB 150 | ORBextractor *orb_extractor_left_, *orb_extractor_right_; 151 | ORBextractor* init_orb_extractor_; 152 | 153 | // BoW 154 | ORBVocabulary* orb_vocabulary_; 155 | KeyFrameDatabase* keyframe_database_; 156 | 157 | // Initalization (only for monocular) 158 | Initializer* initializer_; 159 | 160 | // Local Map 161 | KeyFrame* reference_keyframe_; 162 | std::vector local_keyframes_; 163 | std::vector local_map_points_; 164 | 165 | // MonoORBSlam 166 | MonoORBSlam* mono_orb_slam_; 167 | 168 | // Drawers 169 | Viewer* viewer_; 170 | FrameDrawer* frame_drawer_; 171 | MapDrawer* map_drawer_; 172 | 173 | // Map 174 | Map* map_; 175 | 176 | // Calibration matrix 177 | cv::Mat K_; 178 | cv::Mat dist_coef_; 179 | float bf_; 180 | 181 | // New KeyFrame rules (according to fps) 182 | int min_frames_; 183 | int max_frames_; 184 | 185 | // Threshold close/far points 186 | // Points seen as close by the stereo/RGBD sensor are considered reliable 187 | // and inserted from just one frame. Far points requiere a match in two 188 | // keyframes. 189 | float theshold_depth_; 190 | 191 | // Current matches in frame 192 | int n_matches_inliers_; 193 | 194 | // Last Frame, KeyFrame and Relocalisation Info 195 | KeyFrame* last_keyframe_; 196 | Frame last_frame_; 197 | unsigned int last_keyframe_id_; 198 | unsigned int last_reloc_frame_id_; 199 | 200 | // Motion Model 201 | Eigen::Matrix4d velocity_; 202 | 203 | // Color order (true RGB, false BGR, ignored if grayscale) 204 | bool is_rgb_; 205 | }; 206 | 207 | } // namespace ORB_SLAM2 208 | 209 | #endif // TRACKING_H 210 | -------------------------------------------------------------------------------- /include/Viewer.h: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: Viewer.h 6 | * 7 | * Created On: Wed 04 Sep 2019 06:57:52 PM CST 8 | * Licensed under The GPLv3 License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | /** 12 | * This file is part of ORB-SLAM2. 13 | * 14 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 15 | * of Zaragoza) For more information see 16 | * 17 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 18 | * it under the terms of the GNU General Public License as published by 19 | * the Free Software Foundation, either version 3 of the License, or 20 | * (at your option) any later version. 21 | * 22 | * ORB-SLAM2 is distributed in the hope that it will be useful, 23 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 24 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 25 | * GNU General Public License for more details. 26 | * 27 | * You should have received a copy of the GNU General Public License 28 | * along with ORB-SLAM2. If not, see . 29 | */ 30 | 31 | #ifndef VIEWER_H_ 32 | #define VIEWER_H_ 33 | 34 | #include "FrameDrawer.h" 35 | #include "MapDrawer.h" 36 | #include "MonoORBSlam.h" 37 | #include "Tracking.h" 38 | 39 | #include 40 | 41 | namespace ORB_SLAM2 { 42 | 43 | class Tracking; 44 | class FrameDrawer; 45 | class MapDrawer; 46 | class MonoORBSlam; 47 | 48 | class Viewer { 49 | public: 50 | Viewer(MonoORBSlam* mono_orb_slam, FrameDrawer* frame_drawer, MapDrawer* map_drawer, 51 | Tracking* tracker, const string& string_setting_file); 52 | 53 | // Main thread function. Draw points, keyframes, the current camera pose and 54 | // the last processed frame. Drawing is refreshed according to the camera fps. 55 | // We use Pangolin. 56 | void Run(); 57 | 58 | void RequestFinish(); 59 | 60 | void RequestStop(); 61 | 62 | bool isFinished(); 63 | 64 | bool isStopped(); 65 | 66 | void SetFollowCamera(); 67 | 68 | void Release(); 69 | 70 | private: 71 | bool Stop(); 72 | 73 | MonoORBSlam* mono_orb_slam_; 74 | FrameDrawer* frame_drawer_; 75 | MapDrawer* map_drawer_; 76 | Tracking* tracker_; 77 | 78 | // 1/fps in ms 79 | double T_; 80 | float img_width_, img_height_; 81 | 82 | float view_point_x_, view_point_y_, view_point_z_, view_point_f_; 83 | 84 | bool CheckFinish(); 85 | void SetFinish(); 86 | bool is_finish_requested_; 87 | bool is_finished_; 88 | bool is_follow_; 89 | std::mutex mutex_finish_; 90 | 91 | bool is_stopped_; 92 | bool is_stop_requested_; 93 | std::mutex mutex_stop_; 94 | }; 95 | 96 | } // namespace ORB_SLAM2 97 | #endif 98 | -------------------------------------------------------------------------------- /lib/DBoW2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(DBoW2) 3 | 4 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 6 | 7 | set(HDRS_DBOW2 8 | DBoW2/BowVector.h 9 | DBoW2/FORB.h 10 | DBoW2/FClass.h 11 | DBoW2/FeatureVector.h 12 | DBoW2/ScoringObject.h 13 | DBoW2/TemplatedVocabulary.h) 14 | set(SRCS_DBOW2 15 | DBoW2/BowVector.cpp 16 | DBoW2/FORB.cpp 17 | DBoW2/FeatureVector.cpp 18 | DBoW2/ScoringObject.cpp) 19 | 20 | set(HDRS_DUTILS 21 | DUtils/Random.h 22 | DUtils/Timestamp.h) 23 | set(SRCS_DUTILS 24 | DUtils/Random.cpp 25 | DUtils/Timestamp.cpp) 26 | 27 | find_package(OpenCV 3.0 QUIET) 28 | if(NOT OpenCV_FOUND) 29 | find_package(OpenCV 2.4.3 QUIET) 30 | if(NOT OpenCV_FOUND) 31 | message(FATAL_ERROR "OpenCV > 2.4.3 not found.") 32 | endif() 33 | endif() 34 | 35 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 36 | 37 | include_directories(${OpenCV_INCLUDE_DIRS}) 38 | add_library(DBoW2 SHARED ${SRCS_DBOW2} ${SRCS_DUTILS}) 39 | target_link_libraries(DBoW2 ${OpenCV_LIBS}) 40 | 41 | -------------------------------------------------------------------------------- /lib/DBoW2/DBoW2/BowVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.cpp 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "BowVector.h" 17 | 18 | namespace DBoW2 { 19 | 20 | // -------------------------------------------------------------------------- 21 | 22 | BowVector::BowVector(void) 23 | { 24 | } 25 | 26 | // -------------------------------------------------------------------------- 27 | 28 | BowVector::~BowVector(void) 29 | { 30 | } 31 | 32 | // -------------------------------------------------------------------------- 33 | 34 | void BowVector::addWeight(WordId id, WordValue v) 35 | { 36 | BowVector::iterator vit = this->lower_bound(id); 37 | 38 | if(vit != this->end() && !(this->key_comp()(id, vit->first))) 39 | { 40 | vit->second += v; 41 | } 42 | else 43 | { 44 | this->insert(vit, BowVector::value_type(id, v)); 45 | } 46 | } 47 | 48 | // -------------------------------------------------------------------------- 49 | 50 | void BowVector::addIfNotExist(WordId id, WordValue v) 51 | { 52 | BowVector::iterator vit = this->lower_bound(id); 53 | 54 | if(vit == this->end() || (this->key_comp()(id, vit->first))) 55 | { 56 | this->insert(vit, BowVector::value_type(id, v)); 57 | } 58 | } 59 | 60 | // -------------------------------------------------------------------------- 61 | 62 | void BowVector::normalize(LNorm norm_type) 63 | { 64 | double norm = 0.0; 65 | BowVector::iterator it; 66 | 67 | if(norm_type == DBoW2::L1) 68 | { 69 | for(it = begin(); it != end(); ++it) 70 | norm += fabs(it->second); 71 | } 72 | else 73 | { 74 | for(it = begin(); it != end(); ++it) 75 | norm += it->second * it->second; 76 | norm = sqrt(norm); 77 | } 78 | 79 | if(norm > 0.0) 80 | { 81 | for(it = begin(); it != end(); ++it) 82 | it->second /= norm; 83 | } 84 | } 85 | 86 | // -------------------------------------------------------------------------- 87 | 88 | std::ostream& operator<< (std::ostream &out, const BowVector &v) 89 | { 90 | BowVector::const_iterator vit; 91 | std::vector::const_iterator iit; 92 | unsigned int i = 0; 93 | const unsigned int N = v.size(); 94 | for(vit = v.begin(); vit != v.end(); ++vit, ++i) 95 | { 96 | out << "<" << vit->first << ", " << vit->second << ">"; 97 | 98 | if(i < N-1) out << ", "; 99 | } 100 | return out; 101 | } 102 | 103 | // -------------------------------------------------------------------------- 104 | 105 | void BowVector::saveM(const std::string &filename, size_t W) const 106 | { 107 | std::fstream f(filename.c_str(), std::ios::out); 108 | 109 | WordId last = 0; 110 | BowVector::const_iterator bit; 111 | for(bit = this->begin(); bit != this->end(); ++bit) 112 | { 113 | for(; last < bit->first; ++last) 114 | { 115 | f << "0 "; 116 | } 117 | f << bit->second << " "; 118 | 119 | last = bit->first + 1; 120 | } 121 | for(; last < (WordId)W; ++last) 122 | f << "0 "; 123 | 124 | f.close(); 125 | } 126 | 127 | // -------------------------------------------------------------------------- 128 | 129 | } // namespace DBoW2 130 | 131 | -------------------------------------------------------------------------------- /lib/DBoW2/DBoW2/BowVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.h 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_BOW_VECTOR__ 11 | #define __D_T_BOW_VECTOR__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Id of words 20 | typedef unsigned int WordId; 21 | 22 | /// Value of a word 23 | typedef double WordValue; 24 | 25 | /// Id of nodes in the vocabulary treee 26 | typedef unsigned int NodeId; 27 | 28 | /// L-norms for normalization 29 | enum LNorm 30 | { 31 | L1, 32 | L2 33 | }; 34 | 35 | /// Weighting type 36 | enum WeightingType 37 | { 38 | TF_IDF, 39 | TF, 40 | IDF, 41 | BINARY 42 | }; 43 | 44 | /// Scoring type 45 | enum ScoringType 46 | { 47 | L1_NORM, 48 | L2_NORM, 49 | CHI_SQUARE, 50 | KL, 51 | BHATTACHARYYA, 52 | DOT_PRODUCT, 53 | }; 54 | 55 | /// Vector of words to represent images 56 | class BowVector: 57 | public std::map 58 | { 59 | public: 60 | 61 | /** 62 | * Constructor 63 | */ 64 | BowVector(void); 65 | 66 | /** 67 | * Destructor 68 | */ 69 | ~BowVector(void); 70 | 71 | /** 72 | * Adds a value to a word value existing in the vector, or creates a new 73 | * word with the given value 74 | * @param id word id to look for 75 | * @param v value to create the word with, or to add to existing word 76 | */ 77 | void addWeight(WordId id, WordValue v); 78 | 79 | /** 80 | * Adds a word with a value to the vector only if this does not exist yet 81 | * @param id word id to look for 82 | * @param v value to give to the word if this does not exist 83 | */ 84 | void addIfNotExist(WordId id, WordValue v); 85 | 86 | /** 87 | * L1-Normalizes the values in the vector 88 | * @param norm_type norm used 89 | */ 90 | void normalize(LNorm norm_type); 91 | 92 | /** 93 | * Prints the content of the bow vector 94 | * @param out stream 95 | * @param v 96 | */ 97 | friend std::ostream& operator<<(std::ostream &out, const BowVector &v); 98 | 99 | /** 100 | * Saves the bow vector as a vector in a matlab file 101 | * @param filename 102 | * @param W number of words in the vocabulary 103 | */ 104 | void saveM(const std::string &filename, size_t W) const; 105 | }; 106 | 107 | } // namespace DBoW2 108 | 109 | #endif 110 | -------------------------------------------------------------------------------- /lib/DBoW2/DBoW2/FClass.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FClass.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: generic FClass to instantiate templated classes 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FCLASS__ 11 | #define __D_T_FCLASS__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Generic class to encapsulate functions to manage descriptors. 20 | /** 21 | * This class must be inherited. Derived classes can be used as the 22 | * parameter F when creating Templated structures 23 | * (TemplatedVocabulary, TemplatedDatabase, ...) 24 | */ 25 | class FClass 26 | { 27 | class TDescriptor; 28 | typedef const TDescriptor *pDescriptor; 29 | 30 | /** 31 | * Calculates the mean value of a set of descriptors 32 | * @param descriptors 33 | * @param mean mean descriptor 34 | */ 35 | virtual void meanValue(const std::vector &descriptors, 36 | TDescriptor &mean) = 0; 37 | 38 | /** 39 | * Calculates the distance between two descriptors 40 | * @param a 41 | * @param b 42 | * @return distance 43 | */ 44 | static double distance(const TDescriptor &a, const TDescriptor &b); 45 | 46 | /** 47 | * Returns a string version of the descriptor 48 | * @param a descriptor 49 | * @return string version 50 | */ 51 | static std::string toString(const TDescriptor &a); 52 | 53 | /** 54 | * Returns a descriptor from a string 55 | * @param a descriptor 56 | * @param s string version 57 | */ 58 | static void fromString(TDescriptor &a, const std::string &s); 59 | 60 | /** 61 | * Returns a mat with the descriptors in float format 62 | * @param descriptors 63 | * @param mat (out) NxL 32F matrix 64 | */ 65 | static void toMat32F(const std::vector &descriptors, 66 | cv::Mat &mat); 67 | }; 68 | 69 | } // namespace DBoW2 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /lib/DBoW2/DBoW2/FORB.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FORB.cpp 3 | * Date: June 2012 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for ORB descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | * Distance function has been modified 9 | * 10 | */ 11 | 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include "FORB.h" 19 | 20 | using namespace std; 21 | 22 | namespace DBoW2 { 23 | 24 | // -------------------------------------------------------------------------- 25 | 26 | const int FORB::L=32; 27 | 28 | void FORB::meanValue(const std::vector &descriptors, 29 | FORB::TDescriptor &mean) 30 | { 31 | if(descriptors.empty()) 32 | { 33 | mean.release(); 34 | return; 35 | } 36 | else if(descriptors.size() == 1) 37 | { 38 | mean = descriptors[0]->clone(); 39 | } 40 | else 41 | { 42 | vector sum(FORB::L * 8, 0); 43 | 44 | for(size_t i = 0; i < descriptors.size(); ++i) 45 | { 46 | const cv::Mat &d = *descriptors[i]; 47 | const unsigned char *p = d.ptr(); 48 | 49 | for(int j = 0; j < d.cols; ++j, ++p) 50 | { 51 | if(*p & (1 << 7)) ++sum[ j*8 ]; 52 | if(*p & (1 << 6)) ++sum[ j*8 + 1 ]; 53 | if(*p & (1 << 5)) ++sum[ j*8 + 2 ]; 54 | if(*p & (1 << 4)) ++sum[ j*8 + 3 ]; 55 | if(*p & (1 << 3)) ++sum[ j*8 + 4 ]; 56 | if(*p & (1 << 2)) ++sum[ j*8 + 5 ]; 57 | if(*p & (1 << 1)) ++sum[ j*8 + 6 ]; 58 | if(*p & (1)) ++sum[ j*8 + 7 ]; 59 | } 60 | } 61 | 62 | mean = cv::Mat::zeros(1, FORB::L, CV_8U); 63 | unsigned char *p = mean.ptr(); 64 | 65 | const int N2 = (int)descriptors.size() / 2 + descriptors.size() % 2; 66 | for(size_t i = 0; i < sum.size(); ++i) 67 | { 68 | if(sum[i] >= N2) 69 | { 70 | // set bit 71 | *p |= 1 << (7 - (i % 8)); 72 | } 73 | 74 | if(i % 8 == 7) ++p; 75 | } 76 | } 77 | } 78 | 79 | // -------------------------------------------------------------------------- 80 | 81 | int FORB::distance(const FORB::TDescriptor &a, 82 | const FORB::TDescriptor &b) 83 | { 84 | // Bit set count operation from 85 | // http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel 86 | 87 | const int *pa = a.ptr(); 88 | const int *pb = b.ptr(); 89 | 90 | int dist=0; 91 | 92 | for(int i=0; i<8; i++, pa++, pb++) 93 | { 94 | unsigned int v = *pa ^ *pb; 95 | v = v - ((v >> 1) & 0x55555555); 96 | v = (v & 0x33333333) + ((v >> 2) & 0x33333333); 97 | dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24; 98 | } 99 | 100 | return dist; 101 | } 102 | 103 | // -------------------------------------------------------------------------- 104 | 105 | std::string FORB::toString(const FORB::TDescriptor &a) 106 | { 107 | stringstream ss; 108 | const unsigned char *p = a.ptr(); 109 | 110 | for(int i = 0; i < a.cols; ++i, ++p) 111 | { 112 | ss << (int)*p << " "; 113 | } 114 | 115 | return ss.str(); 116 | } 117 | 118 | // -------------------------------------------------------------------------- 119 | 120 | void FORB::fromString(FORB::TDescriptor &a, const std::string &s) 121 | { 122 | a.create(1, FORB::L, CV_8U); 123 | unsigned char *p = a.ptr(); 124 | 125 | stringstream ss(s); 126 | for(int i = 0; i < FORB::L; ++i, ++p) 127 | { 128 | int n; 129 | ss >> n; 130 | 131 | if(!ss.fail()) 132 | *p = (unsigned char)n; 133 | } 134 | 135 | } 136 | 137 | // -------------------------------------------------------------------------- 138 | 139 | void FORB::toMat32F(const std::vector &descriptors, 140 | cv::Mat &mat) 141 | { 142 | if(descriptors.empty()) 143 | { 144 | mat.release(); 145 | return; 146 | } 147 | 148 | const size_t N = descriptors.size(); 149 | 150 | mat.create(N, FORB::L*8, CV_32F); 151 | float *p = mat.ptr(); 152 | 153 | for(size_t i = 0; i < N; ++i) 154 | { 155 | const int C = descriptors[i].cols; 156 | const unsigned char *desc = descriptors[i].ptr(); 157 | 158 | for(int j = 0; j < C; ++j, p += 8) 159 | { 160 | p[0] = (desc[j] & (1 << 7) ? 1 : 0); 161 | p[1] = (desc[j] & (1 << 6) ? 1 : 0); 162 | p[2] = (desc[j] & (1 << 5) ? 1 : 0); 163 | p[3] = (desc[j] & (1 << 4) ? 1 : 0); 164 | p[4] = (desc[j] & (1 << 3) ? 1 : 0); 165 | p[5] = (desc[j] & (1 << 2) ? 1 : 0); 166 | p[6] = (desc[j] & (1 << 1) ? 1 : 0); 167 | p[7] = desc[j] & (1); 168 | } 169 | } 170 | } 171 | 172 | // -------------------------------------------------------------------------- 173 | 174 | void FORB::toMat8U(const std::vector &descriptors, 175 | cv::Mat &mat) 176 | { 177 | mat.create(descriptors.size(), 32, CV_8U); 178 | 179 | unsigned char *p = mat.ptr(); 180 | 181 | for(size_t i = 0; i < descriptors.size(); ++i, p += 32) 182 | { 183 | const unsigned char *d = descriptors[i].ptr(); 184 | std::copy(d, d+32, p); 185 | } 186 | 187 | } 188 | 189 | // -------------------------------------------------------------------------- 190 | 191 | } // namespace DBoW2 192 | 193 | 194 | -------------------------------------------------------------------------------- /lib/DBoW2/DBoW2/FORB.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FORB.h 3 | * Date: June 2012 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for ORB descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_F_ORB__ 11 | #define __D_T_F_ORB__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "FClass.h" 18 | 19 | namespace DBoW2 { 20 | 21 | /// Functions to manipulate ORB descriptors 22 | class FORB: protected FClass 23 | { 24 | public: 25 | 26 | /// Descriptor type 27 | typedef cv::Mat TDescriptor; // CV_8U 28 | /// Pointer to a single descriptor 29 | typedef const TDescriptor *pDescriptor; 30 | /// Descriptor length (in bytes) 31 | static const int L; 32 | 33 | /** 34 | * Calculates the mean value of a set of descriptors 35 | * @param descriptors 36 | * @param mean mean descriptor 37 | */ 38 | static void meanValue(const std::vector &descriptors, 39 | TDescriptor &mean); 40 | 41 | /** 42 | * Calculates the distance between two descriptors 43 | * @param a 44 | * @param b 45 | * @return distance 46 | */ 47 | static int distance(const TDescriptor &a, const TDescriptor &b); 48 | 49 | /** 50 | * Returns a string version of the descriptor 51 | * @param a descriptor 52 | * @return string version 53 | */ 54 | static std::string toString(const TDescriptor &a); 55 | 56 | /** 57 | * Returns a descriptor from a string 58 | * @param a descriptor 59 | * @param s string version 60 | */ 61 | static void fromString(TDescriptor &a, const std::string &s); 62 | 63 | /** 64 | * Returns a mat with the descriptors in float format 65 | * @param descriptors 66 | * @param mat (out) NxL 32F matrix 67 | */ 68 | static void toMat32F(const std::vector &descriptors, 69 | cv::Mat &mat); 70 | 71 | static void toMat8U(const std::vector &descriptors, 72 | cv::Mat &mat); 73 | 74 | }; 75 | 76 | } // namespace DBoW2 77 | 78 | #endif 79 | 80 | -------------------------------------------------------------------------------- /lib/DBoW2/DBoW2/FeatureVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.cpp 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include "FeatureVector.h" 11 | #include 12 | #include 13 | #include 14 | 15 | namespace DBoW2 { 16 | 17 | // --------------------------------------------------------------------------- 18 | 19 | FeatureVector::FeatureVector(void) 20 | { 21 | } 22 | 23 | // --------------------------------------------------------------------------- 24 | 25 | FeatureVector::~FeatureVector(void) 26 | { 27 | } 28 | 29 | // --------------------------------------------------------------------------- 30 | 31 | void FeatureVector::addFeature(NodeId id, unsigned int i_feature) 32 | { 33 | FeatureVector::iterator vit = this->lower_bound(id); 34 | 35 | if(vit != this->end() && vit->first == id) 36 | { 37 | vit->second.push_back(i_feature); 38 | } 39 | else 40 | { 41 | vit = this->insert(vit, FeatureVector::value_type(id, 42 | std::vector() )); 43 | vit->second.push_back(i_feature); 44 | } 45 | } 46 | 47 | // --------------------------------------------------------------------------- 48 | 49 | std::ostream& operator<<(std::ostream &out, 50 | const FeatureVector &v) 51 | { 52 | if(!v.empty()) 53 | { 54 | FeatureVector::const_iterator vit = v.begin(); 55 | 56 | const std::vector* f = &vit->second; 57 | 58 | out << "<" << vit->first << ": ["; 59 | if(!f->empty()) out << (*f)[0]; 60 | for(unsigned int i = 1; i < f->size(); ++i) 61 | { 62 | out << ", " << (*f)[i]; 63 | } 64 | out << "]>"; 65 | 66 | for(++vit; vit != v.end(); ++vit) 67 | { 68 | f = &vit->second; 69 | 70 | out << ", <" << vit->first << ": ["; 71 | if(!f->empty()) out << (*f)[0]; 72 | for(unsigned int i = 1; i < f->size(); ++i) 73 | { 74 | out << ", " << (*f)[i]; 75 | } 76 | out << "]>"; 77 | } 78 | } 79 | 80 | return out; 81 | } 82 | 83 | // --------------------------------------------------------------------------- 84 | 85 | } // namespace DBoW2 86 | -------------------------------------------------------------------------------- /lib/DBoW2/DBoW2/FeatureVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FEATURE_VECTOR__ 11 | #define __D_T_FEATURE_VECTOR__ 12 | 13 | #include "BowVector.h" 14 | #include 15 | #include 16 | #include 17 | 18 | namespace DBoW2 { 19 | 20 | /// Vector of nodes with indexes of local features 21 | class FeatureVector: 22 | public std::map > 23 | { 24 | public: 25 | 26 | /** 27 | * Constructor 28 | */ 29 | FeatureVector(void); 30 | 31 | /** 32 | * Destructor 33 | */ 34 | ~FeatureVector(void); 35 | 36 | /** 37 | * Adds a feature to an existing node, or adds a new node with an initial 38 | * feature 39 | * @param id node id to add or to modify 40 | * @param i_feature index of feature to add to the given node 41 | */ 42 | void addFeature(NodeId id, unsigned int i_feature); 43 | 44 | /** 45 | * Sends a string versions of the feature vector through the stream 46 | * @param out stream 47 | * @param v feature vector 48 | */ 49 | friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v); 50 | 51 | }; 52 | 53 | } // namespace DBoW2 54 | 55 | #endif 56 | 57 | -------------------------------------------------------------------------------- /lib/DBoW2/DBoW2/ScoringObject.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: ScoringObject.cpp 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions to compute bow scores 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include 11 | #include "TemplatedVocabulary.h" 12 | #include "BowVector.h" 13 | 14 | using namespace DBoW2; 15 | 16 | // If you change the type of WordValue, make sure you change also the 17 | // epsilon value (this is needed by the KL method) 18 | const double GeneralScoring::LOG_EPS = log(DBL_EPSILON); // FLT_EPSILON 19 | 20 | // --------------------------------------------------------------------------- 21 | // --------------------------------------------------------------------------- 22 | 23 | double L1Scoring::score(const BowVector &v1, const BowVector &v2) const 24 | { 25 | BowVector::const_iterator v1_it, v2_it; 26 | const BowVector::const_iterator v1_end = v1.end(); 27 | const BowVector::const_iterator v2_end = v2.end(); 28 | 29 | v1_it = v1.begin(); 30 | v2_it = v2.begin(); 31 | 32 | double score = 0; 33 | 34 | while(v1_it != v1_end && v2_it != v2_end) 35 | { 36 | const WordValue& vi = v1_it->second; 37 | const WordValue& wi = v2_it->second; 38 | 39 | if(v1_it->first == v2_it->first) 40 | { 41 | score += fabs(vi - wi) - fabs(vi) - fabs(wi); 42 | 43 | // move v1 and v2 forward 44 | ++v1_it; 45 | ++v2_it; 46 | } 47 | else if(v1_it->first < v2_it->first) 48 | { 49 | // move v1 forward 50 | v1_it = v1.lower_bound(v2_it->first); 51 | // v1_it = (first element >= v2_it.id) 52 | } 53 | else 54 | { 55 | // move v2 forward 56 | v2_it = v2.lower_bound(v1_it->first); 57 | // v2_it = (first element >= v1_it.id) 58 | } 59 | } 60 | 61 | // ||v - w||_{L1} = 2 + Sum(|v_i - w_i| - |v_i| - |w_i|) 62 | // for all i | v_i != 0 and w_i != 0 63 | // (Nister, 2006) 64 | // scaled_||v - w||_{L1} = 1 - 0.5 * ||v - w||_{L1} 65 | score = -score/2.0; 66 | 67 | return score; // [0..1] 68 | } 69 | 70 | // --------------------------------------------------------------------------- 71 | // --------------------------------------------------------------------------- 72 | 73 | double L2Scoring::score(const BowVector &v1, const BowVector &v2) const 74 | { 75 | BowVector::const_iterator v1_it, v2_it; 76 | const BowVector::const_iterator v1_end = v1.end(); 77 | const BowVector::const_iterator v2_end = v2.end(); 78 | 79 | v1_it = v1.begin(); 80 | v2_it = v2.begin(); 81 | 82 | double score = 0; 83 | 84 | while(v1_it != v1_end && v2_it != v2_end) 85 | { 86 | const WordValue& vi = v1_it->second; 87 | const WordValue& wi = v2_it->second; 88 | 89 | if(v1_it->first == v2_it->first) 90 | { 91 | score += vi * wi; 92 | 93 | // move v1 and v2 forward 94 | ++v1_it; 95 | ++v2_it; 96 | } 97 | else if(v1_it->first < v2_it->first) 98 | { 99 | // move v1 forward 100 | v1_it = v1.lower_bound(v2_it->first); 101 | // v1_it = (first element >= v2_it.id) 102 | } 103 | else 104 | { 105 | // move v2 forward 106 | v2_it = v2.lower_bound(v1_it->first); 107 | // v2_it = (first element >= v1_it.id) 108 | } 109 | } 110 | 111 | // ||v - w||_{L2} = sqrt( 2 - 2 * Sum(v_i * w_i) ) 112 | // for all i | v_i != 0 and w_i != 0 ) 113 | // (Nister, 2006) 114 | if(score >= 1) // rounding errors 115 | score = 1.0; 116 | else 117 | score = 1.0 - sqrt(1.0 - score); // [0..1] 118 | 119 | return score; 120 | } 121 | 122 | // --------------------------------------------------------------------------- 123 | // --------------------------------------------------------------------------- 124 | 125 | double ChiSquareScoring::score(const BowVector &v1, const BowVector &v2) 126 | const 127 | { 128 | BowVector::const_iterator v1_it, v2_it; 129 | const BowVector::const_iterator v1_end = v1.end(); 130 | const BowVector::const_iterator v2_end = v2.end(); 131 | 132 | v1_it = v1.begin(); 133 | v2_it = v2.begin(); 134 | 135 | double score = 0; 136 | 137 | // all the items are taken into account 138 | 139 | while(v1_it != v1_end && v2_it != v2_end) 140 | { 141 | const WordValue& vi = v1_it->second; 142 | const WordValue& wi = v2_it->second; 143 | 144 | if(v1_it->first == v2_it->first) 145 | { 146 | // (v-w)^2/(v+w) - v - w = -4 vw/(v+w) 147 | // we move the -4 out 148 | if(vi + wi != 0.0) score += vi * wi / (vi + wi); 149 | 150 | // move v1 and v2 forward 151 | ++v1_it; 152 | ++v2_it; 153 | } 154 | else if(v1_it->first < v2_it->first) 155 | { 156 | // move v1 forward 157 | v1_it = v1.lower_bound(v2_it->first); 158 | } 159 | else 160 | { 161 | // move v2 forward 162 | v2_it = v2.lower_bound(v1_it->first); 163 | } 164 | } 165 | 166 | // this takes the -4 into account 167 | score = 2. * score; // [0..1] 168 | 169 | return score; 170 | } 171 | 172 | // --------------------------------------------------------------------------- 173 | // --------------------------------------------------------------------------- 174 | 175 | double KLScoring::score(const BowVector &v1, const BowVector &v2) const 176 | { 177 | BowVector::const_iterator v1_it, v2_it; 178 | const BowVector::const_iterator v1_end = v1.end(); 179 | const BowVector::const_iterator v2_end = v2.end(); 180 | 181 | v1_it = v1.begin(); 182 | v2_it = v2.begin(); 183 | 184 | double score = 0; 185 | 186 | // all the items or v are taken into account 187 | 188 | while(v1_it != v1_end && v2_it != v2_end) 189 | { 190 | const WordValue& vi = v1_it->second; 191 | const WordValue& wi = v2_it->second; 192 | 193 | if(v1_it->first == v2_it->first) 194 | { 195 | if(vi != 0 && wi != 0) score += vi * log(vi/wi); 196 | 197 | // move v1 and v2 forward 198 | ++v1_it; 199 | ++v2_it; 200 | } 201 | else if(v1_it->first < v2_it->first) 202 | { 203 | // move v1 forward 204 | score += vi * (log(vi) - LOG_EPS); 205 | ++v1_it; 206 | } 207 | else 208 | { 209 | // move v2_it forward, do not add any score 210 | v2_it = v2.lower_bound(v1_it->first); 211 | // v2_it = (first element >= v1_it.id) 212 | } 213 | } 214 | 215 | // sum rest of items of v 216 | for(; v1_it != v1_end; ++v1_it) 217 | if(v1_it->second != 0) 218 | score += v1_it->second * (log(v1_it->second) - LOG_EPS); 219 | 220 | return score; // cannot be scaled 221 | } 222 | 223 | // --------------------------------------------------------------------------- 224 | // --------------------------------------------------------------------------- 225 | 226 | double BhattacharyyaScoring::score(const BowVector &v1, 227 | const BowVector &v2) const 228 | { 229 | BowVector::const_iterator v1_it, v2_it; 230 | const BowVector::const_iterator v1_end = v1.end(); 231 | const BowVector::const_iterator v2_end = v2.end(); 232 | 233 | v1_it = v1.begin(); 234 | v2_it = v2.begin(); 235 | 236 | double score = 0; 237 | 238 | while(v1_it != v1_end && v2_it != v2_end) 239 | { 240 | const WordValue& vi = v1_it->second; 241 | const WordValue& wi = v2_it->second; 242 | 243 | if(v1_it->first == v2_it->first) 244 | { 245 | score += sqrt(vi * wi); 246 | 247 | // move v1 and v2 forward 248 | ++v1_it; 249 | ++v2_it; 250 | } 251 | else if(v1_it->first < v2_it->first) 252 | { 253 | // move v1 forward 254 | v1_it = v1.lower_bound(v2_it->first); 255 | // v1_it = (first element >= v2_it.id) 256 | } 257 | else 258 | { 259 | // move v2 forward 260 | v2_it = v2.lower_bound(v1_it->first); 261 | // v2_it = (first element >= v1_it.id) 262 | } 263 | } 264 | 265 | return score; // already scaled 266 | } 267 | 268 | // --------------------------------------------------------------------------- 269 | // --------------------------------------------------------------------------- 270 | 271 | double DotProductScoring::score(const BowVector &v1, 272 | const BowVector &v2) const 273 | { 274 | BowVector::const_iterator v1_it, v2_it; 275 | const BowVector::const_iterator v1_end = v1.end(); 276 | const BowVector::const_iterator v2_end = v2.end(); 277 | 278 | v1_it = v1.begin(); 279 | v2_it = v2.begin(); 280 | 281 | double score = 0; 282 | 283 | while(v1_it != v1_end && v2_it != v2_end) 284 | { 285 | const WordValue& vi = v1_it->second; 286 | const WordValue& wi = v2_it->second; 287 | 288 | if(v1_it->first == v2_it->first) 289 | { 290 | score += vi * wi; 291 | 292 | // move v1 and v2 forward 293 | ++v1_it; 294 | ++v2_it; 295 | } 296 | else if(v1_it->first < v2_it->first) 297 | { 298 | // move v1 forward 299 | v1_it = v1.lower_bound(v2_it->first); 300 | // v1_it = (first element >= v2_it.id) 301 | } 302 | else 303 | { 304 | // move v2 forward 305 | v2_it = v2.lower_bound(v1_it->first); 306 | // v2_it = (first element >= v1_it.id) 307 | } 308 | } 309 | 310 | return score; // cannot scale 311 | } 312 | 313 | // --------------------------------------------------------------------------- 314 | // --------------------------------------------------------------------------- 315 | 316 | -------------------------------------------------------------------------------- /lib/DBoW2/DBoW2/ScoringObject.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: ScoringObject.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions to compute bow scores 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_SCORING_OBJECT__ 11 | #define __D_T_SCORING_OBJECT__ 12 | 13 | #include "BowVector.h" 14 | 15 | namespace DBoW2 { 16 | 17 | /// Base class of scoring functions 18 | class GeneralScoring 19 | { 20 | public: 21 | /** 22 | * Computes the score between two vectors. Vectors must be sorted and 23 | * normalized if necessary 24 | * @param v (in/out) 25 | * @param w (in/out) 26 | * @return score 27 | */ 28 | virtual double score(const BowVector &v, const BowVector &w) const = 0; 29 | 30 | /** 31 | * Returns whether a vector must be normalized before scoring according 32 | * to the scoring scheme 33 | * @param norm norm to use 34 | * @return true iff must normalize 35 | */ 36 | virtual bool mustNormalize(LNorm &norm) const = 0; 37 | 38 | /// Log of epsilon 39 | static const double LOG_EPS; 40 | // If you change the type of WordValue, make sure you change also the 41 | // epsilon value (this is needed by the KL method) 42 | 43 | virtual ~GeneralScoring() {} //!< Required for virtual base classes 44 | 45 | }; 46 | 47 | /** 48 | * Macro for defining Scoring classes 49 | * @param NAME name of class 50 | * @param MUSTNORMALIZE if vectors must be normalized to compute the score 51 | * @param NORM type of norm to use when MUSTNORMALIZE 52 | */ 53 | #define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ 54 | NAME: public GeneralScoring \ 55 | { public: \ 56 | /** \ 57 | * Computes score between two vectors \ 58 | * @param v \ 59 | * @param w \ 60 | * @return score between v and w \ 61 | */ \ 62 | virtual double score(const BowVector &v, const BowVector &w) const; \ 63 | \ 64 | /** \ 65 | * Says if a vector must be normalized according to the scoring function \ 66 | * @param norm (out) if true, norm to use 67 | * @return true iff vectors must be normalized \ 68 | */ \ 69 | virtual inline bool mustNormalize(LNorm &norm) const \ 70 | { norm = NORM; return MUSTNORMALIZE; } \ 71 | } 72 | 73 | /// L1 Scoring object 74 | class __SCORING_CLASS(L1Scoring, true, L1); 75 | 76 | /// L2 Scoring object 77 | class __SCORING_CLASS(L2Scoring, true, L2); 78 | 79 | /// Chi square Scoring object 80 | class __SCORING_CLASS(ChiSquareScoring, true, L1); 81 | 82 | /// KL divergence Scoring object 83 | class __SCORING_CLASS(KLScoring, true, L1); 84 | 85 | /// Bhattacharyya Scoring object 86 | class __SCORING_CLASS(BhattacharyyaScoring, true, L1); 87 | 88 | /// Dot product Scoring object 89 | class __SCORING_CLASS(DotProductScoring, false, L1); 90 | 91 | #undef __SCORING_CLASS 92 | 93 | } // namespace DBoW2 94 | 95 | #endif 96 | 97 | -------------------------------------------------------------------------------- /lib/DBoW2/DUtils/Random.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Random.cpp 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: April 2010 6 | * Description: manages pseudo-random numbers 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #include "Random.h" 12 | #include "Timestamp.h" 13 | #include 14 | using namespace std; 15 | 16 | bool DUtils::Random::m_already_seeded = false; 17 | 18 | void DUtils::Random::SeedRand(){ 19 | Timestamp time; 20 | time.setToCurrentTime(); 21 | srand((unsigned)time.getFloatTime()); 22 | } 23 | 24 | void DUtils::Random::SeedRandOnce() 25 | { 26 | if(!m_already_seeded) 27 | { 28 | DUtils::Random::SeedRand(); 29 | m_already_seeded = true; 30 | } 31 | } 32 | 33 | void DUtils::Random::SeedRand(int seed) 34 | { 35 | srand(seed); 36 | } 37 | 38 | void DUtils::Random::SeedRandOnce(int seed) 39 | { 40 | if(!m_already_seeded) 41 | { 42 | DUtils::Random::SeedRand(seed); 43 | m_already_seeded = true; 44 | } 45 | } 46 | 47 | int DUtils::Random::RandomInt(int min, int max){ 48 | int d = max - min + 1; 49 | return int(((double)rand()/((double)RAND_MAX + 1.0)) * d) + min; 50 | } 51 | 52 | // --------------------------------------------------------------------------- 53 | // --------------------------------------------------------------------------- 54 | 55 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer(int min, int max) 56 | { 57 | if(min <= max) 58 | { 59 | m_min = min; 60 | m_max = max; 61 | } 62 | else 63 | { 64 | m_min = max; 65 | m_max = min; 66 | } 67 | 68 | createValues(); 69 | } 70 | 71 | // --------------------------------------------------------------------------- 72 | 73 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer 74 | (const DUtils::Random::UnrepeatedRandomizer& rnd) 75 | { 76 | *this = rnd; 77 | } 78 | 79 | // --------------------------------------------------------------------------- 80 | 81 | int DUtils::Random::UnrepeatedRandomizer::get() 82 | { 83 | if(empty()) createValues(); 84 | 85 | DUtils::Random::SeedRandOnce(); 86 | 87 | int k = DUtils::Random::RandomInt(0, m_values.size()-1); 88 | int ret = m_values[k]; 89 | m_values[k] = m_values.back(); 90 | m_values.pop_back(); 91 | 92 | return ret; 93 | } 94 | 95 | // --------------------------------------------------------------------------- 96 | 97 | void DUtils::Random::UnrepeatedRandomizer::createValues() 98 | { 99 | int n = m_max - m_min + 1; 100 | 101 | m_values.resize(n); 102 | for(int i = 0; i < n; ++i) m_values[i] = m_min + i; 103 | } 104 | 105 | // --------------------------------------------------------------------------- 106 | 107 | void DUtils::Random::UnrepeatedRandomizer::reset() 108 | { 109 | if((int)m_values.size() != m_max - m_min + 1) createValues(); 110 | } 111 | 112 | // --------------------------------------------------------------------------- 113 | 114 | DUtils::Random::UnrepeatedRandomizer& 115 | DUtils::Random::UnrepeatedRandomizer::operator= 116 | (const DUtils::Random::UnrepeatedRandomizer& rnd) 117 | { 118 | if(this != &rnd) 119 | { 120 | this->m_min = rnd.m_min; 121 | this->m_max = rnd.m_max; 122 | this->m_values = rnd.m_values; 123 | } 124 | return *this; 125 | } 126 | 127 | // --------------------------------------------------------------------------- 128 | 129 | 130 | -------------------------------------------------------------------------------- /lib/DBoW2/DUtils/Random.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Random.h 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: April 2010, November 2011 6 | * Description: manages pseudo-random numbers 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #pragma once 12 | #ifndef __D_RANDOM__ 13 | #define __D_RANDOM__ 14 | 15 | #include 16 | #include 17 | 18 | namespace DUtils { 19 | 20 | /// Functions to generate pseudo-random numbers 21 | class Random 22 | { 23 | public: 24 | class UnrepeatedRandomizer; 25 | 26 | public: 27 | /** 28 | * Sets the random number seed to the current time 29 | */ 30 | static void SeedRand(); 31 | 32 | /** 33 | * Sets the random number seed to the current time only the first 34 | * time this function is called 35 | */ 36 | static void SeedRandOnce(); 37 | 38 | /** 39 | * Sets the given random number seed 40 | * @param seed 41 | */ 42 | static void SeedRand(int seed); 43 | 44 | /** 45 | * Sets the given random number seed only the first time this function 46 | * is called 47 | * @param seed 48 | */ 49 | static void SeedRandOnce(int seed); 50 | 51 | /** 52 | * Returns a random number in the range [0..1] 53 | * @return random T number in [0..1] 54 | */ 55 | template 56 | static T RandomValue(){ 57 | return (T)rand()/(T)RAND_MAX; 58 | } 59 | 60 | /** 61 | * Returns a random number in the range [min..max] 62 | * @param min 63 | * @param max 64 | * @return random T number in [min..max] 65 | */ 66 | template 67 | static T RandomValue(T min, T max){ 68 | return Random::RandomValue() * (max - min) + min; 69 | } 70 | 71 | /** 72 | * Returns a random int in the range [min..max] 73 | * @param min 74 | * @param max 75 | * @return random int in [min..max] 76 | */ 77 | static int RandomInt(int min, int max); 78 | 79 | /** 80 | * Returns a random number from a gaussian distribution 81 | * @param mean 82 | * @param sigma standard deviation 83 | */ 84 | template 85 | static T RandomGaussianValue(T mean, T sigma) 86 | { 87 | // Box-Muller transformation 88 | T x1, x2, w, y1; 89 | 90 | do { 91 | x1 = (T)2. * RandomValue() - (T)1.; 92 | x2 = (T)2. * RandomValue() - (T)1.; 93 | w = x1 * x1 + x2 * x2; 94 | } while ( w >= (T)1. || w == (T)0. ); 95 | 96 | w = sqrt( ((T)-2.0 * log( w ) ) / w ); 97 | y1 = x1 * w; 98 | 99 | return( mean + y1 * sigma ); 100 | } 101 | 102 | private: 103 | 104 | /// If SeedRandOnce() or SeedRandOnce(int) have already been called 105 | static bool m_already_seeded; 106 | 107 | }; 108 | 109 | // --------------------------------------------------------------------------- 110 | 111 | /// Provides pseudo-random numbers with no repetitions 112 | class Random::UnrepeatedRandomizer 113 | { 114 | public: 115 | 116 | /** 117 | * Creates a randomizer that returns numbers in the range [min, max] 118 | * @param min 119 | * @param max 120 | */ 121 | UnrepeatedRandomizer(int min, int max); 122 | ~UnrepeatedRandomizer(){} 123 | 124 | /** 125 | * Copies a randomizer 126 | * @param rnd 127 | */ 128 | UnrepeatedRandomizer(const UnrepeatedRandomizer& rnd); 129 | 130 | /** 131 | * Copies a randomizer 132 | * @param rnd 133 | */ 134 | UnrepeatedRandomizer& operator=(const UnrepeatedRandomizer& rnd); 135 | 136 | /** 137 | * Returns a random number not given before. If all the possible values 138 | * were already given, the process starts again 139 | * @return unrepeated random number 140 | */ 141 | int get(); 142 | 143 | /** 144 | * Returns whether all the possible values between min and max were 145 | * already given. If get() is called when empty() is true, the behaviour 146 | * is the same than after creating the randomizer 147 | * @return true iff all the values were returned 148 | */ 149 | inline bool empty() const { return m_values.empty(); } 150 | 151 | /** 152 | * Returns the number of values still to be returned 153 | * @return amount of values to return 154 | */ 155 | inline unsigned int left() const { return m_values.size(); } 156 | 157 | /** 158 | * Resets the randomizer as it were just created 159 | */ 160 | void reset(); 161 | 162 | protected: 163 | 164 | /** 165 | * Creates the vector with available values 166 | */ 167 | void createValues(); 168 | 169 | protected: 170 | 171 | /// Min of range of values 172 | int m_min; 173 | /// Max of range of values 174 | int m_max; 175 | 176 | /// Available values 177 | std::vector m_values; 178 | 179 | }; 180 | 181 | } 182 | 183 | #endif 184 | 185 | -------------------------------------------------------------------------------- /lib/DBoW2/DUtils/Timestamp.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Timestamp.cpp 3 | * Author: Dorian Galvez-Lopez 4 | * Date: March 2009 5 | * Description: timestamping functions 6 | * 7 | * Note: in windows, this class has a 1ms resolution 8 | * 9 | * License: see the LICENSE.txt file 10 | * 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #ifdef _WIN32 21 | #ifndef WIN32 22 | #define WIN32 23 | #endif 24 | #endif 25 | 26 | #ifdef WIN32 27 | #include 28 | #define sprintf sprintf_s 29 | #else 30 | #include 31 | #endif 32 | 33 | #include "Timestamp.h" 34 | 35 | using namespace std; 36 | 37 | using namespace DUtils; 38 | 39 | Timestamp::Timestamp(Timestamp::tOptions option) 40 | { 41 | if(option & CURRENT_TIME) 42 | setToCurrentTime(); 43 | else if(option & ZERO) 44 | setTime(0.); 45 | } 46 | 47 | Timestamp::~Timestamp(void) 48 | { 49 | } 50 | 51 | bool Timestamp::empty() const 52 | { 53 | return m_secs == 0 && m_usecs == 0; 54 | } 55 | 56 | void Timestamp::setToCurrentTime(){ 57 | 58 | #ifdef WIN32 59 | struct __timeb32 timebuffer; 60 | _ftime32_s( &timebuffer ); // C4996 61 | // Note: _ftime is deprecated; consider using _ftime_s instead 62 | m_secs = timebuffer.time; 63 | m_usecs = timebuffer.millitm * 1000; 64 | #else 65 | struct timeval now; 66 | gettimeofday(&now, NULL); 67 | m_secs = now.tv_sec; 68 | m_usecs = now.tv_usec; 69 | #endif 70 | 71 | } 72 | 73 | void Timestamp::setTime(const string &stime){ 74 | string::size_type p = stime.find('.'); 75 | if(p == string::npos){ 76 | m_secs = atol(stime.c_str()); 77 | m_usecs = 0; 78 | }else{ 79 | m_secs = atol(stime.substr(0, p).c_str()); 80 | 81 | string s_usecs = stime.substr(p+1, 6); 82 | m_usecs = atol(stime.substr(p+1).c_str()); 83 | m_usecs *= (unsigned long)pow(10.0, double(6 - s_usecs.length())); 84 | } 85 | } 86 | 87 | void Timestamp::setTime(double s) 88 | { 89 | m_secs = (unsigned long)s; 90 | m_usecs = (s - (double)m_secs) * 1e6; 91 | } 92 | 93 | double Timestamp::getFloatTime() const { 94 | return double(m_secs) + double(m_usecs)/1000000.0; 95 | } 96 | 97 | string Timestamp::getStringTime() const { 98 | char buf[32]; 99 | sprintf(buf, "%.6lf", this->getFloatTime()); 100 | return string(buf); 101 | } 102 | 103 | double Timestamp::operator- (const Timestamp &t) const { 104 | return this->getFloatTime() - t.getFloatTime(); 105 | } 106 | 107 | Timestamp& Timestamp::operator+= (double s) 108 | { 109 | *this = *this + s; 110 | return *this; 111 | } 112 | 113 | Timestamp& Timestamp::operator-= (double s) 114 | { 115 | *this = *this - s; 116 | return *this; 117 | } 118 | 119 | Timestamp Timestamp::operator+ (double s) const 120 | { 121 | unsigned long secs = (long)floor(s); 122 | unsigned long usecs = (long)((s - (double)secs) * 1e6); 123 | 124 | return this->plus(secs, usecs); 125 | } 126 | 127 | Timestamp Timestamp::plus(unsigned long secs, unsigned long usecs) const 128 | { 129 | Timestamp t; 130 | 131 | const unsigned long max = 1000000ul; 132 | 133 | if(m_usecs + usecs >= max) 134 | t.setTime(m_secs + secs + 1, m_usecs + usecs - max); 135 | else 136 | t.setTime(m_secs + secs, m_usecs + usecs); 137 | 138 | return t; 139 | } 140 | 141 | Timestamp Timestamp::operator- (double s) const 142 | { 143 | unsigned long secs = (long)floor(s); 144 | unsigned long usecs = (long)((s - (double)secs) * 1e6); 145 | 146 | return this->minus(secs, usecs); 147 | } 148 | 149 | Timestamp Timestamp::minus(unsigned long secs, unsigned long usecs) const 150 | { 151 | Timestamp t; 152 | 153 | const unsigned long max = 1000000ul; 154 | 155 | if(m_usecs < usecs) 156 | t.setTime(m_secs - secs - 1, max - (usecs - m_usecs)); 157 | else 158 | t.setTime(m_secs - secs, m_usecs - usecs); 159 | 160 | return t; 161 | } 162 | 163 | bool Timestamp::operator> (const Timestamp &t) const 164 | { 165 | if(m_secs > t.m_secs) return true; 166 | else if(m_secs == t.m_secs) return m_usecs > t.m_usecs; 167 | else return false; 168 | } 169 | 170 | bool Timestamp::operator>= (const Timestamp &t) const 171 | { 172 | if(m_secs > t.m_secs) return true; 173 | else if(m_secs == t.m_secs) return m_usecs >= t.m_usecs; 174 | else return false; 175 | } 176 | 177 | bool Timestamp::operator< (const Timestamp &t) const 178 | { 179 | if(m_secs < t.m_secs) return true; 180 | else if(m_secs == t.m_secs) return m_usecs < t.m_usecs; 181 | else return false; 182 | } 183 | 184 | bool Timestamp::operator<= (const Timestamp &t) const 185 | { 186 | if(m_secs < t.m_secs) return true; 187 | else if(m_secs == t.m_secs) return m_usecs <= t.m_usecs; 188 | else return false; 189 | } 190 | 191 | bool Timestamp::operator== (const Timestamp &t) const 192 | { 193 | return(m_secs == t.m_secs && m_usecs == t.m_usecs); 194 | } 195 | 196 | 197 | string Timestamp::Format(bool machine_friendly) const 198 | { 199 | struct tm tm_time; 200 | 201 | time_t t = (time_t)getFloatTime(); 202 | 203 | #ifdef WIN32 204 | localtime_s(&tm_time, &t); 205 | #else 206 | localtime_r(&t, &tm_time); 207 | #endif 208 | 209 | char buffer[128]; 210 | 211 | if(machine_friendly) 212 | { 213 | strftime(buffer, 128, "%Y%m%d_%H%M%S", &tm_time); 214 | } 215 | else 216 | { 217 | strftime(buffer, 128, "%c", &tm_time); // Thu Aug 23 14:55:02 2001 218 | } 219 | 220 | return string(buffer); 221 | } 222 | 223 | string Timestamp::Format(double s) { 224 | int days = int(s / (24. * 3600.0)); 225 | s -= days * (24. * 3600.0); 226 | int hours = int(s / 3600.0); 227 | s -= hours * 3600; 228 | int minutes = int(s / 60.0); 229 | s -= minutes * 60; 230 | int seconds = int(s); 231 | int ms = int((s - seconds)*1e6); 232 | 233 | stringstream ss; 234 | ss.fill('0'); 235 | bool b; 236 | if((b = (days > 0))) ss << days << "d "; 237 | if((b = (b || hours > 0))) ss << setw(2) << hours << ":"; 238 | if((b = (b || minutes > 0))) ss << setw(2) << minutes << ":"; 239 | if(b) ss << setw(2); 240 | ss << seconds; 241 | if(!b) ss << "." << setw(6) << ms; 242 | 243 | return ss.str(); 244 | } 245 | 246 | 247 | -------------------------------------------------------------------------------- /lib/DBoW2/DUtils/Timestamp.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Timestamp.h 3 | * Author: Dorian Galvez-Lopez 4 | * Date: March 2009 5 | * Description: timestamping functions 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_TIMESTAMP__ 11 | #define __D_TIMESTAMP__ 12 | 13 | #include 14 | using namespace std; 15 | 16 | namespace DUtils { 17 | 18 | /// Timestamp 19 | class Timestamp 20 | { 21 | public: 22 | 23 | /// Options to initiate a timestamp 24 | enum tOptions 25 | { 26 | NONE = 0, 27 | CURRENT_TIME = 0x1, 28 | ZERO = 0x2 29 | }; 30 | 31 | public: 32 | 33 | /** 34 | * Creates a timestamp 35 | * @param option option to set the initial time stamp 36 | */ 37 | Timestamp(Timestamp::tOptions option = NONE); 38 | 39 | /** 40 | * Destructor 41 | */ 42 | virtual ~Timestamp(void); 43 | 44 | /** 45 | * Says if the timestamp is "empty": seconds and usecs are both 0, as 46 | * when initiated with the ZERO flag 47 | * @return true iif secs == usecs == 0 48 | */ 49 | bool empty() const; 50 | 51 | /** 52 | * Sets this instance to the current time 53 | */ 54 | void setToCurrentTime(); 55 | 56 | /** 57 | * Sets the timestamp from seconds and microseconds 58 | * @param secs: seconds 59 | * @param usecs: microseconds 60 | */ 61 | inline void setTime(unsigned long secs, unsigned long usecs){ 62 | m_secs = secs; 63 | m_usecs = usecs; 64 | } 65 | 66 | /** 67 | * Returns the timestamp in seconds and microseconds 68 | * @param secs seconds 69 | * @param usecs microseconds 70 | */ 71 | inline void getTime(unsigned long &secs, unsigned long &usecs) const 72 | { 73 | secs = m_secs; 74 | usecs = m_usecs; 75 | } 76 | 77 | /** 78 | * Sets the timestamp from a string with the time in seconds 79 | * @param stime: string such as "1235603336.036609" 80 | */ 81 | void setTime(const string &stime); 82 | 83 | /** 84 | * Sets the timestamp from a number of seconds from the epoch 85 | * @param s seconds from the epoch 86 | */ 87 | void setTime(double s); 88 | 89 | /** 90 | * Returns this timestamp as the number of seconds in (long) float format 91 | */ 92 | double getFloatTime() const; 93 | 94 | /** 95 | * Returns this timestamp as the number of seconds in fixed length string format 96 | */ 97 | string getStringTime() const; 98 | 99 | /** 100 | * Returns the difference in seconds between this timestamp (greater) and t (smaller) 101 | * If the order is swapped, a negative number is returned 102 | * @param t: timestamp to subtract from this timestamp 103 | * @return difference in seconds 104 | */ 105 | double operator- (const Timestamp &t) const; 106 | 107 | /** 108 | * Returns a copy of this timestamp + s seconds + us microseconds 109 | * @param s seconds 110 | * @param us microseconds 111 | */ 112 | Timestamp plus(unsigned long s, unsigned long us) const; 113 | 114 | /** 115 | * Returns a copy of this timestamp - s seconds - us microseconds 116 | * @param s seconds 117 | * @param us microseconds 118 | */ 119 | Timestamp minus(unsigned long s, unsigned long us) const; 120 | 121 | /** 122 | * Adds s seconds to this timestamp and returns a reference to itself 123 | * @param s seconds 124 | * @return reference to this timestamp 125 | */ 126 | Timestamp& operator+= (double s); 127 | 128 | /** 129 | * Substracts s seconds to this timestamp and returns a reference to itself 130 | * @param s seconds 131 | * @return reference to this timestamp 132 | */ 133 | Timestamp& operator-= (double s); 134 | 135 | /** 136 | * Returns a copy of this timestamp + s seconds 137 | * @param s: seconds 138 | */ 139 | Timestamp operator+ (double s) const; 140 | 141 | /** 142 | * Returns a copy of this timestamp - s seconds 143 | * @param s: seconds 144 | */ 145 | Timestamp operator- (double s) const; 146 | 147 | /** 148 | * Returns whether this timestamp is at the future of t 149 | * @param t 150 | */ 151 | bool operator> (const Timestamp &t) const; 152 | 153 | /** 154 | * Returns whether this timestamp is at the future of (or is the same as) t 155 | * @param t 156 | */ 157 | bool operator>= (const Timestamp &t) const; 158 | 159 | /** 160 | * Returns whether this timestamp and t represent the same instant 161 | * @param t 162 | */ 163 | bool operator== (const Timestamp &t) const; 164 | 165 | /** 166 | * Returns whether this timestamp is at the past of t 167 | * @param t 168 | */ 169 | bool operator< (const Timestamp &t) const; 170 | 171 | /** 172 | * Returns whether this timestamp is at the past of (or is the same as) t 173 | * @param t 174 | */ 175 | bool operator<= (const Timestamp &t) const; 176 | 177 | /** 178 | * Returns the timestamp in a human-readable string 179 | * @param machine_friendly if true, the returned string is formatted 180 | * to yyyymmdd_hhmmss, without weekday or spaces 181 | * @note This has not been tested under Windows 182 | * @note The timestamp is truncated to seconds 183 | */ 184 | string Format(bool machine_friendly = false) const; 185 | 186 | /** 187 | * Returns a string version of the elapsed time in seconds, with the format 188 | * xd hh:mm:ss, hh:mm:ss, mm:ss or s.us 189 | * @param s: elapsed seconds (given by getFloatTime) to format 190 | */ 191 | static string Format(double s); 192 | 193 | 194 | protected: 195 | /// Seconds 196 | unsigned long m_secs; // seconds 197 | /// Microseconds 198 | unsigned long m_usecs; // microseconds 199 | }; 200 | 201 | } 202 | 203 | #endif 204 | 205 | -------------------------------------------------------------------------------- /lib/DBoW2/LICENSE.txt: -------------------------------------------------------------------------------- 1 | DBoW2: bag-of-words library for C++ with generic descriptors 2 | 3 | Copyright (c) 2015 Dorian Galvez-Lopez (Universidad de Zaragoza) 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions 8 | are met: 9 | 1. Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 2. Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | 3. Neither the name of copyright holders nor the names of its 15 | contributors may be used to endorse or promote products derived 16 | from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 19 | ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 20 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 21 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS 22 | BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | POSSIBILITY OF SUCH DAMAGE. 29 | 30 | If you use it in an academic work, please cite: 31 | 32 | @ARTICLE{GalvezTRO12, 33 | author={G\'alvez-L\'opez, Dorian and Tard\'os, J. D.}, 34 | journal={IEEE Transactions on Robotics}, 35 | title={Bags of Binary Words for Fast Place Recognition in Image Sequences}, 36 | year={2012}, 37 | month={October}, 38 | volume={28}, 39 | number={5}, 40 | pages={1188--1197}, 41 | doi={10.1109/TRO.2012.2197158}, 42 | ISSN={1552-3098} 43 | } 44 | 45 | -------------------------------------------------------------------------------- /lib/DBoW2/README.txt: -------------------------------------------------------------------------------- 1 | You should have received this DBoW2 version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2). 2 | See the original DBoW2 library at: https://github.com/dorian3d/DBoW2 3 | All files included in this version are BSD, see LICENSE.txt 4 | 5 | We also use Random.h, Random.cpp, Timestamp.pp and Timestamp.h from DLib/DUtils. 6 | See the original DLib library at: https://github.com/dorian3d/DLib 7 | All files included in this version are BSD, see LICENSE.txt 8 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ceres_mono_orb_slam2 4 | 0.0.0 5 | The ceres_mono_orb_slam2 package 6 | 7 | 8 | b51 9 | 10 | GPLv3 11 | 12 | catkin 13 | 14 | 15 | -------------------------------------------------------------------------------- /src/FrameDrawer.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 5 | * of Zaragoza) 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 | #include "FrameDrawer.h" 22 | #include "Tracking.h" 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | namespace ORB_SLAM2 { 30 | FrameDrawer::FrameDrawer(Map* map) : map_(map) { 31 | state_ = Tracking::SYSTEM_NOT_READY; 32 | image_ = cv::Mat(480, 640, CV_8UC3, cv::Scalar(0, 0, 0)); 33 | } 34 | 35 | cv::Mat FrameDrawer::DrawFrame() { 36 | cv::Mat img; 37 | // Initialization: KeyPoints in reference frame 38 | std::vector init_keypoints; 39 | // Initialization: correspondeces with reference keypoints 40 | std::vector matches; 41 | // KeyPoints in current frame 42 | std::vector current_keypoints; 43 | // Tracked MapPoints in current frame 44 | std::vector do_vo, do_map; 45 | // Tracking state 46 | int state; 47 | 48 | // Copy variables within scoped mutex 49 | { 50 | unique_lock lock(mutex_); 51 | state = state_; 52 | if (state_ == Tracking::SYSTEM_NOT_READY) state_ = Tracking::NO_IMAGES_YET; 53 | 54 | image_.copyTo(img); 55 | 56 | if (state_ == Tracking::NOT_INITIALIZED) { 57 | current_keypoints = current_keypoints_; 58 | init_keypoints = init_keypoints_; 59 | matches = init_matches_; 60 | } else if (state_ == Tracking::OK) { 61 | current_keypoints = current_keypoints_; 62 | do_vo = do_vo_; 63 | do_map = do_map_; 64 | } else if (state_ == Tracking::LOST) { 65 | current_keypoints = current_keypoints_; 66 | } 67 | } // destroy scoped mutex -> release mutex 68 | 69 | if (img.channels() < 3) // this should be always true 70 | cv::cvtColor(img, img, CV_GRAY2BGR); 71 | 72 | // Draw 73 | if (state == Tracking::NOT_INITIALIZED) { 74 | // INITIALIZING 75 | for (unsigned int i = 0; i < matches.size(); i++) { 76 | if (matches[i] >= 0) { 77 | cv::line(img, init_keypoints[i].pt, current_keypoints[matches[i]].pt, 78 | cv::Scalar(0, 255, 0)); 79 | } 80 | } 81 | } else if (state == Tracking::OK) { 82 | // TRACKING 83 | ntracked_ = 0; 84 | ntracked_vo_ = 0; 85 | const float r = 5; 86 | const int n = current_keypoints.size(); 87 | for (int i = 0; i < n; i++) { 88 | if (do_vo[i] || do_map[i]) { 89 | cv::Point2f pt1, pt2; 90 | pt1.x = current_keypoints[i].pt.x - r; 91 | pt1.y = current_keypoints[i].pt.y - r; 92 | pt2.x = current_keypoints[i].pt.x + r; 93 | pt2.y = current_keypoints[i].pt.y + r; 94 | 95 | // This is a match to a MapPoint in the map 96 | if (do_map[i]) { 97 | cv::rectangle(img, pt1, pt2, cv::Scalar(0, 255, 0)); 98 | cv::circle(img, current_keypoints[i].pt, 2, cv::Scalar(0, 255, 0), 99 | -1); 100 | ntracked_++; 101 | } else // This is match to a "visual odometry" MapPoint created in the 102 | // last frame 103 | { 104 | cv::rectangle(img, pt1, pt2, cv::Scalar(255, 0, 0)); 105 | cv::circle(img, current_keypoints[i].pt, 2, cv::Scalar(255, 0, 0), 106 | -1); 107 | ntracked_vo_++; 108 | } 109 | } 110 | } 111 | } 112 | 113 | cv::Mat imgWithInfo; 114 | DrawTextInfo(img, state, imgWithInfo); 115 | 116 | return imgWithInfo; 117 | } 118 | 119 | void FrameDrawer::DrawTextInfo(cv::Mat& img, int state, cv::Mat& imgText) { 120 | stringstream s; 121 | if (state == Tracking::NO_IMAGES_YET) 122 | s << " WAITING FOR IMAGES"; 123 | else if (state == Tracking::NOT_INITIALIZED) 124 | s << " TRYING TO INITIALIZE "; 125 | else if (state == Tracking::OK) { 126 | if (!do_only_tracking_) 127 | s << "SLAM MODE | "; 128 | else 129 | s << "LOCALIZATION | "; 130 | int nKFs = map_->KeyFramesInMap(); 131 | int nMPs = map_->MapPointsInMap(); 132 | s << "KFs: " << nKFs << ", MPs: " << nMPs << ", Matches: " << ntracked_; 133 | if (ntracked_vo_ > 0) { 134 | s << ", + VO matches: " << ntracked_vo_; 135 | } 136 | } else if (state == Tracking::LOST) { 137 | s << " TRACK LOST. TRYING TO RELOCALIZE "; 138 | } else if (state == Tracking::SYSTEM_NOT_READY) { 139 | s << " LOADING ORB VOCABULARY. PLEASE WAIT..."; 140 | } 141 | 142 | int baseline = 0; 143 | cv::Size textSize = 144 | cv::getTextSize(s.str(), cv::FONT_HERSHEY_PLAIN, 1, 1, &baseline); 145 | 146 | imgText = cv::Mat(img.rows + textSize.height + 10, img.cols, img.type()); 147 | img.copyTo(imgText.rowRange(0, img.rows).colRange(0, img.cols)); 148 | imgText.rowRange(img.rows, imgText.rows) = 149 | cv::Mat::zeros(textSize.height + 10, img.cols, img.type()); 150 | cv::putText(imgText, s.str(), cv::Point(5, imgText.rows - 5), 151 | cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(255, 255, 255), 1, 8); 152 | } 153 | 154 | void FrameDrawer::Update(Tracking* tracker) { 155 | unique_lock lock(mutex_); 156 | tracker->img_gray_.copyTo(image_); 157 | current_keypoints_ = tracker->current_frame_.keypoints_; 158 | N_ = current_keypoints_.size(); 159 | do_vo_ = std::vector(N_, false); 160 | do_map_ = std::vector(N_, false); 161 | do_only_tracking_ = tracker->do_only_tracking_; 162 | 163 | if (tracker->last_processed_state_ == Tracking::NOT_INITIALIZED) { 164 | init_keypoints_ = tracker->init_frame_.keypoints_; 165 | init_matches_ = tracker->init_matches_; 166 | } else if (tracker->last_processed_state_ == Tracking::OK) { 167 | for (int i = 0; i < N_; i++) { 168 | MapPoint* map_point = tracker->current_frame_.map_points_[i]; 169 | if (map_point) { 170 | if (!tracker->current_frame_.is_outliers_[i]) { 171 | if (map_point->Observations() > 0) 172 | do_map_[i] = true; 173 | else 174 | do_vo_[i] = true; 175 | } 176 | } 177 | } 178 | } 179 | state_ = static_cast(tracker->last_processed_state_); 180 | } 181 | } // namespace ORB_SLAM2 182 | -------------------------------------------------------------------------------- /src/KeyFrameDatabase.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 5 | * of Zaragoza) 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 | #include "KeyFrameDatabase.h" 22 | 23 | #include 24 | 25 | #include "KeyFrame.h" 26 | #include "ORBVocabulary.h" 27 | #include "lib/DBoW2/DBoW2/BowVector.h" 28 | 29 | 30 | namespace ORB_SLAM2 { 31 | 32 | KeyFrameDatabase::KeyFrameDatabase(const ORBVocabulary& voc) 33 | : orb_vocabulary_(&voc) { 34 | inverted_files_.resize(voc.size()); 35 | } 36 | 37 | void KeyFrameDatabase::add(KeyFrame* keyframe) { 38 | unique_lock lock(mutex_); 39 | 40 | for (DBoW2::BowVector::const_iterator vit = keyframe->bow_vector_.begin(), 41 | vend = keyframe->bow_vector_.end(); 42 | vit != vend; vit++) 43 | inverted_files_[vit->first].push_back(keyframe); 44 | } 45 | 46 | void KeyFrameDatabase::erase(KeyFrame* keyframe) { 47 | unique_lock lock(mutex_); 48 | 49 | // Erase elements in the Inverse File for the entry 50 | for (DBoW2::BowVector::const_iterator vit = keyframe->bow_vector_.begin(), 51 | vend = keyframe->bow_vector_.end(); 52 | vit != vend; vit++) { 53 | // List of keyframes that share the word 54 | std::list& keyframes = inverted_files_[vit->first]; 55 | 56 | for (std::list::iterator lit = keyframes.begin(), 57 | lend = keyframes.end(); 58 | lit != lend; lit++) { 59 | if (keyframe == *lit) { 60 | keyframes.erase(lit); 61 | break; 62 | } 63 | } 64 | } 65 | } 66 | 67 | void KeyFrameDatabase::clear() { 68 | inverted_files_.clear(); 69 | inverted_files_.resize(orb_vocabulary_->size()); 70 | } 71 | 72 | std::vector KeyFrameDatabase::DetectLoopCandidates( 73 | KeyFrame* keyframe, float minScore) { 74 | std::set connected_keyframes = keyframe->GetConnectedKeyFrames(); 75 | std::list keyframes_sharing_words; 76 | 77 | // Search all keyframes that share a word with current keyframes 78 | // Discard keyframes connected to the query keyframe 79 | { 80 | unique_lock lock(mutex_); 81 | 82 | for (DBoW2::BowVector::const_iterator vit = keyframe->bow_vector_.begin(), 83 | vend = keyframe->bow_vector_.end(); 84 | vit != vend; vit++) { 85 | list& keyframes = inverted_files_[vit->first]; 86 | 87 | for (list::iterator lit = keyframes.begin(), 88 | lend = keyframes.end(); 89 | lit != lend; lit++) { 90 | KeyFrame* ikeyframe = *lit; 91 | if (ikeyframe->n_loop_query_ != keyframe->id_) { 92 | ikeyframe->n_loop_words_ = 0; 93 | if (!connected_keyframes.count(ikeyframe)) { 94 | ikeyframe->n_loop_query_ = keyframe->id_; 95 | keyframes_sharing_words.push_back(ikeyframe); 96 | } 97 | } 98 | ikeyframe->n_loop_words_++; 99 | } 100 | } 101 | } 102 | 103 | if (keyframes_sharing_words.empty()) { 104 | return std::vector(); 105 | } 106 | 107 | std::list > score_and_matches; 108 | 109 | // Only compare against those keyframes that share enough words 110 | int maxCommonWords = 0; 111 | for (list::iterator lit = keyframes_sharing_words.begin(), 112 | lend = keyframes_sharing_words.end(); 113 | lit != lend; lit++) { 114 | if ((*lit)->n_loop_words_ > maxCommonWords) 115 | maxCommonWords = (*lit)->n_loop_words_; 116 | } 117 | 118 | int minCommonWords = maxCommonWords * 0.8f; 119 | 120 | int nscores = 0; 121 | 122 | // Compute similarity score. Retain the matches whose score is higher than 123 | // minScore 124 | for (list::iterator lit = keyframes_sharing_words.begin(), 125 | lend = keyframes_sharing_words.end(); 126 | lit != lend; lit++) { 127 | KeyFrame* ikeyframe = *lit; 128 | 129 | if (ikeyframe->n_loop_words_ > minCommonWords) { 130 | nscores++; 131 | 132 | float score = 133 | orb_vocabulary_->score(keyframe->bow_vector_, ikeyframe->bow_vector_); 134 | 135 | ikeyframe->loop_score_ = score; 136 | if (score >= minScore) 137 | score_and_matches.push_back(std::make_pair(score, ikeyframe)); 138 | } 139 | } 140 | 141 | if (score_and_matches.empty()) { 142 | return std::vector(); 143 | } 144 | 145 | std::list > acc_score_and_matches; 146 | float bestAccScore = minScore; 147 | 148 | // Lets now accumulate score by covisibility 149 | for (std::list >::iterator 150 | it = score_and_matches.begin(), 151 | itend = score_and_matches.end(); 152 | it != itend; it++) { 153 | KeyFrame* ikeyframe = it->second; 154 | std::vector neighs = ikeyframe->GetBestCovisibilityKeyFrames(10); 155 | 156 | float bestScore = it->first; 157 | float accScore = it->first; 158 | KeyFrame* best_keyframe = ikeyframe; 159 | for (std::vector::iterator vit = neighs.begin(), 160 | vend = neighs.end(); 161 | vit != vend; vit++) { 162 | KeyFrame* jkeyframe = *vit; 163 | if (jkeyframe->n_loop_query_ == keyframe->id_ && 164 | jkeyframe->n_loop_words_ > minCommonWords) { 165 | accScore += jkeyframe->loop_score_; 166 | if (jkeyframe->loop_score_ > bestScore) { 167 | best_keyframe = jkeyframe; 168 | bestScore = jkeyframe->loop_score_; 169 | } 170 | } 171 | } 172 | 173 | acc_score_and_matches.push_back(std::make_pair(accScore, best_keyframe)); 174 | if (accScore > bestAccScore) { 175 | bestAccScore = accScore; 176 | } 177 | } 178 | 179 | // Return all those keyframes with a score higher than 0.75*bestScore 180 | float minScoreToRetain = 0.75f * bestAccScore; 181 | 182 | std::set alread_added_keyframes; 183 | std::vector loop_candidates_; 184 | loop_candidates_.reserve(acc_score_and_matches.size()); 185 | 186 | for (list >::iterator 187 | it = acc_score_and_matches.begin(), 188 | itend = acc_score_and_matches.end(); 189 | it != itend; it++) { 190 | if (it->first > minScoreToRetain) { 191 | KeyFrame* ikeyframe = it->second; 192 | if (!alread_added_keyframes.count(ikeyframe)) { 193 | loop_candidates_.push_back(ikeyframe); 194 | alread_added_keyframes.insert(ikeyframe); 195 | } 196 | } 197 | } 198 | 199 | return loop_candidates_; 200 | } 201 | 202 | vector KeyFrameDatabase::DetectRelocalizationCandidates( 203 | Frame* frame) { 204 | std::list keyframes_sharing_words; 205 | 206 | // Search all keyframes that share a word with current frame 207 | { 208 | unique_lock lock(mutex_); 209 | 210 | for (DBoW2::BowVector::const_iterator vit = frame->bow_vector_.begin(), 211 | vend = frame->bow_vector_.end(); 212 | vit != vend; vit++) { 213 | std::list& keyframes = inverted_files_[vit->first]; 214 | 215 | for (std::list::iterator lit = keyframes.begin(), 216 | lend = keyframes.end(); 217 | lit != lend; lit++) { 218 | KeyFrame* ikeyframe = *lit; 219 | if (ikeyframe->reloc_query_ != frame->id_) { 220 | ikeyframe->n_reloc_words_ = 0; 221 | ikeyframe->reloc_query_ = frame->id_; 222 | keyframes_sharing_words.push_back(ikeyframe); 223 | } 224 | ikeyframe->n_reloc_words_++; 225 | } 226 | } 227 | } 228 | if (keyframes_sharing_words.empty()) { 229 | return std::vector(); 230 | } 231 | 232 | // Only compare against those keyframes that share enough words 233 | int maxCommonWords = 0; 234 | for (list::iterator lit = keyframes_sharing_words.begin(), 235 | lend = keyframes_sharing_words.end(); 236 | lit != lend; lit++) { 237 | if ((*lit)->n_reloc_words_ > maxCommonWords) 238 | maxCommonWords = (*lit)->n_reloc_words_; 239 | } 240 | 241 | int minCommonWords = maxCommonWords * 0.8f; 242 | 243 | std::list > score_and_matches; 244 | 245 | int nscores = 0; 246 | 247 | // Compute similarity score. 248 | for (list::iterator lit = keyframes_sharing_words.begin(), 249 | lend = keyframes_sharing_words.end(); 250 | lit != lend; lit++) { 251 | KeyFrame* ikeyframe = *lit; 252 | 253 | if (ikeyframe->n_reloc_words_ > minCommonWords) { 254 | nscores++; 255 | float score = 256 | orb_vocabulary_->score(frame->bow_vector_, ikeyframe->bow_vector_); 257 | ikeyframe->reloc_score_ = score; 258 | score_and_matches.push_back(std::make_pair(score, ikeyframe)); 259 | } 260 | } 261 | 262 | if (score_and_matches.empty()) { 263 | return std::vector(); 264 | } 265 | 266 | std::list > acc_score_and_matches; 267 | float bestAccScore = 0; 268 | 269 | // Lets now accumulate score by covisibility 270 | for (list >::iterator it = score_and_matches.begin(), 271 | itend = score_and_matches.end(); 272 | it != itend; it++) { 273 | KeyFrame* ikeyframe = it->second; 274 | std::vector neighs = ikeyframe->GetBestCovisibilityKeyFrames(10); 275 | 276 | float bestScore = it->first; 277 | float accScore = bestScore; 278 | KeyFrame* best_keyframe = ikeyframe; 279 | for (vector::iterator vit = neighs.begin(), vend = neighs.end(); 280 | vit != vend; vit++) { 281 | KeyFrame* jkeyframe = *vit; 282 | if (jkeyframe->reloc_query_ != frame->id_) continue; 283 | 284 | accScore += jkeyframe->reloc_score_; 285 | if (jkeyframe->reloc_score_ > bestScore) { 286 | best_keyframe = jkeyframe; 287 | bestScore = jkeyframe->reloc_score_; 288 | } 289 | } 290 | acc_score_and_matches.push_back(std::make_pair(accScore, best_keyframe)); 291 | if (accScore > bestAccScore) { 292 | bestAccScore = accScore; 293 | } 294 | } 295 | 296 | // Return all those keyframes with a score higher than 0.75*bestScore 297 | float minScoreToRetain = 0.75f * bestAccScore; 298 | std::set alread_added_keyframes; 299 | std::vector reloc_candidates; 300 | reloc_candidates.reserve(acc_score_and_matches.size()); 301 | for (std::list >::iterator 302 | it = acc_score_and_matches.begin(), 303 | itend = acc_score_and_matches.end(); 304 | it != itend; it++) { 305 | const float& score = it->first; 306 | if (score > minScoreToRetain) { 307 | KeyFrame* ikeyframe = it->second; 308 | if (!alread_added_keyframes.count(ikeyframe)) { 309 | reloc_candidates.push_back(ikeyframe); 310 | alread_added_keyframes.insert(ikeyframe); 311 | } 312 | } 313 | } 314 | 315 | return reloc_candidates; 316 | } 317 | } // namespace ORB_SLAM2 318 | -------------------------------------------------------------------------------- /src/Map.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 5 | * of Zaragoza) 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 | #include "Map.h" 22 | 23 | #include 24 | 25 | namespace ORB_SLAM2 { 26 | 27 | Map::Map() : max_keyframe_id_(0), big_change_index_(0) {} 28 | 29 | void Map::AddKeyFrame(KeyFrame* keyframe) { 30 | unique_lock lock(mutex_map_); 31 | keyframes_.insert(keyframe); 32 | if (keyframe->id_ > max_keyframe_id_) { 33 | max_keyframe_id_ = keyframe->id_; 34 | } 35 | } 36 | 37 | void Map::AddMapPoint(MapPoint* map_point) { 38 | unique_lock lock(mutex_map_); 39 | map_points_.insert(map_point); 40 | } 41 | 42 | void Map::EraseMapPoint(MapPoint* map_point) { 43 | unique_lock lock(mutex_map_); 44 | map_points_.erase(map_point); 45 | 46 | // TODO: This only erase the pointer. 47 | // Delete the MapPoint 48 | } 49 | 50 | void Map::EraseKeyFrame(KeyFrame* keyframe) { 51 | unique_lock lock(mutex_map_); 52 | keyframes_.erase(keyframe); 53 | 54 | // TODO: This only erase the pointer. 55 | // Delete the MapPoint 56 | } 57 | 58 | void Map::SetReferenceMapPoints(const vector& map_points) { 59 | unique_lock lock(mutex_map_); 60 | reference_map_points_ = map_points; 61 | } 62 | 63 | void Map::InformNewBigChange() { 64 | unique_lock lock(mutex_map_); 65 | big_change_index_++; 66 | } 67 | 68 | int Map::GetLastBigChangeIdx() { 69 | unique_lock lock(mutex_map_); 70 | return big_change_index_; 71 | } 72 | 73 | vector Map::GetAllKeyFrames() { 74 | unique_lock lock(mutex_map_); 75 | return std::vector(keyframes_.begin(), keyframes_.end()); 76 | } 77 | 78 | vector Map::GetAllMapPoints() { 79 | unique_lock lock(mutex_map_); 80 | return std::vector(map_points_.begin(), map_points_.end()); 81 | } 82 | 83 | long unsigned int Map::MapPointsInMap() { 84 | unique_lock lock(mutex_map_); 85 | return map_points_.size(); 86 | } 87 | 88 | long unsigned int Map::KeyFramesInMap() { 89 | unique_lock lock(mutex_map_); 90 | return keyframes_.size(); 91 | } 92 | 93 | vector Map::GetReferenceMapPoints() { 94 | unique_lock lock(mutex_map_); 95 | return reference_map_points_; 96 | } 97 | 98 | long unsigned int Map::GetMaxKFid() { 99 | unique_lock lock(mutex_map_); 100 | return max_keyframe_id_; 101 | } 102 | 103 | void Map::clear() { 104 | for (std::set::iterator sit = map_points_.begin(), 105 | send = map_points_.end(); 106 | sit != send; sit++) 107 | delete *sit; 108 | 109 | for (std::set::iterator sit = keyframes_.begin(), 110 | send = keyframes_.end(); 111 | sit != send; sit++) 112 | delete *sit; 113 | 114 | map_points_.clear(); 115 | keyframes_.clear(); 116 | max_keyframe_id_ = 0; 117 | reference_map_points_.clear(); 118 | keyframe_origins_.clear(); 119 | } 120 | } // namespace ORB_SLAM2 121 | -------------------------------------------------------------------------------- /src/MapDrawer.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 5 | * of Zaragoza) 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 | #include "MapDrawer.h" 22 | #include 23 | #include 24 | #include "KeyFrame.h" 25 | #include "MapPoint.h" 26 | #include "MatEigenConverter.h" 27 | 28 | namespace ORB_SLAM2 { 29 | 30 | MapDrawer::MapDrawer(Map* pMap, const string& strSettingPath) : mpMap(pMap) { 31 | cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ); 32 | 33 | mKeyFrameSize = fSettings["Viewer.KeyFrameSize"]; 34 | mKeyFrameLineWidth = fSettings["Viewer.KeyFrameLineWidth"]; 35 | mGraphLineWidth = fSettings["Viewer.GraphLineWidth"]; 36 | mPointSize = fSettings["Viewer.PointSize"]; 37 | mCameraSize = fSettings["Viewer.CameraSize"]; 38 | mCameraLineWidth = fSettings["Viewer.CameraLineWidth"]; 39 | } 40 | 41 | void MapDrawer::DrawMapPoints() { 42 | const vector& vpMPs = mpMap->GetAllMapPoints(); 43 | const vector& vpRefMPs = mpMap->GetReferenceMapPoints(); 44 | 45 | set spRefMPs(vpRefMPs.begin(), vpRefMPs.end()); 46 | 47 | if (vpMPs.empty()) return; 48 | 49 | glPointSize(mPointSize); 50 | glBegin(GL_POINTS); 51 | glColor3f(0.0, 0.0, 0.0); 52 | 53 | for (size_t i = 0, iend = vpMPs.size(); i < iend; i++) { 54 | if (vpMPs[i]->isBad() || spRefMPs.count(vpMPs[i])) continue; 55 | Eigen::Vector3d pos = vpMPs[i]->GetWorldPos(); 56 | glVertex3f(pos[0], pos[1], pos[2]); 57 | } 58 | glEnd(); 59 | 60 | glPointSize(mPointSize); 61 | glBegin(GL_POINTS); 62 | glColor3f(1.0, 0.0, 0.0); 63 | 64 | for (set::iterator sit = spRefMPs.begin(), send = spRefMPs.end(); 65 | sit != send; sit++) { 66 | if ((*sit)->isBad()) continue; 67 | Eigen::Vector3d pos = (*sit)->GetWorldPos(); 68 | glVertex3f(pos[0], pos[1], pos[2]); 69 | } 70 | 71 | glEnd(); 72 | } 73 | 74 | void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph) { 75 | const float& w = mKeyFrameSize; 76 | const float h = w * 0.75; 77 | const float z = w * 0.6; 78 | 79 | const vector vpKFs = mpMap->GetAllKeyFrames(); 80 | 81 | if (bDrawKF) { 82 | for (size_t i = 0; i < vpKFs.size(); i++) { 83 | KeyFrame* pKF = vpKFs[i]; 84 | // Eigen Matrix data is column major, not row major 85 | Eigen::Matrix Twc = 86 | pKF->GetPoseInverse().cast(); 87 | 88 | glPushMatrix(); 89 | 90 | glMultMatrixf(Twc.data()); 91 | 92 | glLineWidth(mKeyFrameLineWidth); 93 | glColor3f(0.0f, 0.0f, 1.0f); 94 | glBegin(GL_LINES); 95 | glVertex3f(0, 0, 0); 96 | glVertex3f(w, h, z); 97 | glVertex3f(0, 0, 0); 98 | glVertex3f(w, -h, z); 99 | glVertex3f(0, 0, 0); 100 | glVertex3f(-w, -h, z); 101 | glVertex3f(0, 0, 0); 102 | glVertex3f(-w, h, z); 103 | 104 | glVertex3f(w, h, z); 105 | glVertex3f(w, -h, z); 106 | 107 | glVertex3f(-w, h, z); 108 | glVertex3f(-w, -h, z); 109 | 110 | glVertex3f(-w, h, z); 111 | glVertex3f(w, h, z); 112 | 113 | glVertex3f(-w, -h, z); 114 | glVertex3f(w, -h, z); 115 | glEnd(); 116 | 117 | glPopMatrix(); 118 | } 119 | } 120 | 121 | if (bDrawGraph) { 122 | glLineWidth(mGraphLineWidth); 123 | glColor4f(0.0f, 1.0f, 0.0f, 0.6f); 124 | glBegin(GL_LINES); 125 | 126 | for (size_t i = 0; i < vpKFs.size(); i++) { 127 | // Covisibility Graph 128 | const vector vCovKFs = vpKFs[i]->GetCovisiblesByWeight(100); 129 | Eigen::Vector3d Ow = vpKFs[i]->GetCameraCenter(); 130 | if (!vCovKFs.empty()) { 131 | for (vector::const_iterator vit = vCovKFs.begin(), 132 | vend = vCovKFs.end(); 133 | vit != vend; vit++) { 134 | if ((*vit)->id_ < vpKFs[i]->id_) continue; 135 | Eigen::Vector3d Ow2 = (*vit)->GetCameraCenter(); 136 | glVertex3f(Ow[0], Ow[1], Ow[2]); 137 | glVertex3f(Ow2[0], Ow2[1], Ow2[2]); 138 | } 139 | } 140 | 141 | // Spanning tree 142 | KeyFrame* pParent = vpKFs[i]->GetParent(); 143 | if (pParent) { 144 | Eigen::Vector3d Owp = pParent->GetCameraCenter(); 145 | glVertex3f(Ow[0], Ow[1], Ow[2]); 146 | glVertex3f(Owp[0], Owp[1], Owp[2]); 147 | } 148 | 149 | // Loops 150 | set sLoopKFs = vpKFs[i]->GetLoopEdges(); 151 | for (set::iterator sit = sLoopKFs.begin(), 152 | send = sLoopKFs.end(); 153 | sit != send; sit++) { 154 | if ((*sit)->id_ < vpKFs[i]->id_) continue; 155 | Eigen::Vector3d Owl = (*sit)->GetCameraCenter(); 156 | glVertex3f(Ow[0], Ow[1], Ow[2]); 157 | glVertex3f(Owl[0], Owl[1], Owl[2]); 158 | } 159 | } 160 | 161 | glEnd(); 162 | } 163 | } 164 | 165 | void MapDrawer::DrawCurrentCamera(pangolin::OpenGlMatrix& Twc) { 166 | const float& w = mCameraSize; 167 | const float h = w * 0.75; 168 | const float z = w * 0.6; 169 | 170 | glPushMatrix(); 171 | 172 | #ifdef HAVE_GLES 173 | glMultMatrixf(Twc.m); 174 | #else 175 | glMultMatrixd(Twc.m); 176 | #endif 177 | 178 | glLineWidth(mCameraLineWidth); 179 | glColor3f(0.0f, 1.0f, 0.0f); 180 | glBegin(GL_LINES); 181 | glVertex3f(0, 0, 0); 182 | glVertex3f(w, h, z); 183 | glVertex3f(0, 0, 0); 184 | glVertex3f(w, -h, z); 185 | glVertex3f(0, 0, 0); 186 | glVertex3f(-w, -h, z); 187 | glVertex3f(0, 0, 0); 188 | glVertex3f(-w, h, z); 189 | 190 | glVertex3f(w, h, z); 191 | glVertex3f(w, -h, z); 192 | 193 | glVertex3f(-w, h, z); 194 | glVertex3f(-w, -h, z); 195 | 196 | glVertex3f(-w, h, z); 197 | glVertex3f(w, h, z); 198 | 199 | glVertex3f(-w, -h, z); 200 | glVertex3f(w, -h, z); 201 | glEnd(); 202 | 203 | glPopMatrix(); 204 | } 205 | 206 | void MapDrawer::SetCurrentCameraPose(const Eigen::Matrix4d& Tcw) { 207 | unique_lock lock(mMutexCamera); 208 | camera_pose_ = Tcw; 209 | } 210 | 211 | void MapDrawer::GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix& M) { 212 | if (!camera_pose_.isIdentity()) { 213 | Eigen::Matrix3d Rwc = Eigen::Matrix3d::Identity(); 214 | Eigen::Vector3d twc = Eigen::Vector3d::Zero(); 215 | { 216 | unique_lock lock(mMutexCamera); 217 | Rwc = camera_pose_.block<3, 3>(0, 0).transpose(); 218 | twc = -Rwc * camera_pose_.block<3, 1>(0, 3); 219 | } 220 | 221 | M.m[0] = Rwc(0, 0); 222 | M.m[1] = Rwc(1, 0); 223 | M.m[2] = Rwc(2, 0); 224 | M.m[3] = 0.0; 225 | 226 | M.m[4] = Rwc(0, 1); 227 | M.m[5] = Rwc(1, 1); 228 | M.m[6] = Rwc(2, 1); 229 | M.m[7] = 0.0; 230 | 231 | M.m[8] = Rwc(0, 2); 232 | M.m[9] = Rwc(1, 2); 233 | M.m[10] = Rwc(2, 2); 234 | M.m[11] = 0.0; 235 | 236 | M.m[12] = twc[0]; 237 | M.m[13] = twc[1]; 238 | M.m[14] = twc[2]; 239 | M.m[15] = 1.0; 240 | } else 241 | M.SetIdentity(); 242 | } 243 | 244 | } // namespace ORB_SLAM2 245 | -------------------------------------------------------------------------------- /src/MatEigenConverter.cc: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: MatEigenConverter.cc 6 | * 7 | * Created On: Fri 20 Sep 2019 06:22:54 PM CST 8 | * Licensed under The GPLv3 License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | 12 | #include "MatEigenConverter.h" 13 | 14 | Eigen::Matrix4d MatEigenConverter::MatToMatrix4d(const cv::Mat& T) { 15 | Eigen::Matrix4d eigen_T = Eigen::Matrix4d::Identity(); 16 | for (int i = 0; i < 4; i++) 17 | for (int j = 0; j < 4; j++) { 18 | eigen_T(i, j) = T.at(i, j); 19 | } 20 | return eigen_T; 21 | } 22 | 23 | cv::Mat MatEigenConverter::Matrix4dToMat(const Eigen::Matrix4d& T) { 24 | cv::Mat cv_T = cv::Mat(4, 4, CV_32F); 25 | for (int i = 0; i < 4; i++) 26 | for (int j = 0; j < 4; j++) { 27 | cv_T.at(i, j) = T(i, j); 28 | } 29 | return cv_T.clone(); 30 | } 31 | 32 | Eigen::Matrix3d MatEigenConverter::MatToMatrix3d(const cv::Mat& R) { 33 | Eigen::Matrix3d eigen_R = Eigen::Matrix3d::Identity(); 34 | for (int i = 0; i < 3; i++) 35 | for (int j = 0; j < 3; j++) { 36 | eigen_R(i, j) = R.at(i, j); 37 | } 38 | return eigen_R; 39 | } 40 | 41 | cv::Mat MatEigenConverter::Matrix3dToMat(const Eigen::Matrix3d& R) { 42 | cv::Mat cv_R = cv::Mat(3, 3, CV_32F); 43 | for (int i = 0; i < 3; i++) 44 | for (int j = 0; j < 3; j++) { 45 | cv_R.at(i, j) = R(i, j); 46 | } 47 | return cv_R.clone(); 48 | } 49 | 50 | Eigen::Vector3d MatEigenConverter::MatToVector3d(const cv::Mat& t) { 51 | Eigen::Vector3d eigen_t = Eigen::Vector3d::Zero(); 52 | for (int i = 0; i < 3; i++) { 53 | eigen_t[i] = t.at(i, 0); 54 | } 55 | return eigen_t; 56 | } 57 | 58 | cv::Mat MatEigenConverter::Vector3dToMat(const Eigen::Vector3d& t) { 59 | cv::Mat cv_t = cv::Mat(3, 1, CV_32F); 60 | for (int i = 0; i < 3; i++) { 61 | cv_t.at(i, 0) = t[i]; 62 | } 63 | return cv_t.clone(); 64 | } 65 | 66 | Eigen::Matrix MatEigenConverter::Matrix4dToMatrix_7_1( 67 | const Eigen::Matrix4d& pose) { 68 | Eigen::Matrix Tcw_7_1; 69 | Eigen::Matrix3d R; 70 | R = pose.block<3, 3>(0, 0); 71 | // Eigen Quaternion coeffs output [x, y, z, w] 72 | Tcw_7_1.block<3, 1>(0, 0) = pose.block<3, 1>(0, 3); 73 | Tcw_7_1.block<4, 1>(3, 0) = Eigen::Quaterniond(R).coeffs(); 74 | return Tcw_7_1; 75 | } 76 | 77 | Eigen::Matrix4d MatEigenConverter::Matrix_7_1_ToMatrix4d( 78 | const Eigen::Matrix& Tcw_7_1) { 79 | Eigen::Quaterniond q(Tcw_7_1[6], Tcw_7_1[3], Tcw_7_1[4], Tcw_7_1[5]); 80 | Eigen::Matrix3d R = q.normalized().toRotationMatrix(); 81 | Eigen::Matrix4d pose = Eigen::Matrix4d::Identity(); 82 | pose.block<3, 3>(0, 0) = R; 83 | pose.block<3, 1>(0, 3) = Tcw_7_1.block<3, 1>(0, 0); 84 | return pose; 85 | } 86 | 87 | std::vector MatEigenConverter::toDescriptorVector( 88 | const cv::Mat& Descriptors) { 89 | std::vector vDesc; 90 | vDesc.reserve(Descriptors.rows); 91 | for (int j = 0; j < Descriptors.rows; j++) 92 | vDesc.push_back(Descriptors.row(j)); 93 | 94 | return vDesc; 95 | } 96 | -------------------------------------------------------------------------------- /src/MonoORBSlam.cc: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: MonoORBSlam.cc 6 | * 7 | * Created On: Tue 03 Sep 2019 06:27:56 PM CST 8 | * Licensed under The GPLv3 License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | 12 | #include "MonoORBSlam.h" 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | namespace ORB_SLAM2 { 20 | 21 | MonoORBSlam::MonoORBSlam(const string& voc_file, 22 | const string& string_setting_file, 23 | const bool is_use_viewer) 24 | : do_reset_(false), 25 | is_activate_localization_mode_(false), 26 | is_deactivate_localization_mode_(false) { 27 | std::cout 28 | << std::endl 29 | << "ORB-SLAM2 Copyright (C) 2014-2016 Raul Mur-Artal, University of " 30 | "Zaragoza." 31 | << std::endl 32 | << "ceres_mono_orb_slam2 Copyright (C) 2019 b51live" 33 | << std::endl 34 | << "This program comes with ABSOLUTELY NO WARRANTY;" << std::endl 35 | << "This is free software, and you are welcome to redistribute it" 36 | << std::endl 37 | << "under certain conditions. See LICENSE.txt." << std::endl 38 | << std::endl; 39 | 40 | // Check settings file 41 | cv::FileStorage fsSettings(string_setting_file.c_str(), 42 | cv::FileStorage::READ); 43 | if (!fsSettings.isOpened()) { 44 | LOG(FATAL) << "Failed to open settings file at: " << string_setting_file; 45 | } 46 | 47 | // Load ORB Vocabulary 48 | std::cout << std::endl 49 | << "Loading ORB Vocabulary. This could take a while..." 50 | << std::endl; 51 | 52 | vocabulary_ = new ORBVocabulary(); 53 | bool voc_loaded = vocabulary_->loadFromTextFile(voc_file); 54 | if (!voc_loaded) { 55 | LOG(ERROR) << "Wrong path to vocabulary."; 56 | LOG(FATAL) << "Failed to open at: " << voc_file; 57 | } 58 | 59 | LOG(INFO) << "Vocabulary loaded!"; 60 | 61 | // Create KeyFrame Database 62 | keyframe_database_ = new KeyFrameDatabase(*vocabulary_); 63 | 64 | // Create the Map 65 | map_ = new Map(); 66 | 67 | // Create Drawers. These are used by the Viewer 68 | frame_drawer_ = new FrameDrawer(map_); 69 | map_drawer_ = new MapDrawer(map_, string_setting_file); 70 | 71 | // Initialize the Tracking thread 72 | //(it will live in the main thread of execution, the one that called this 73 | // constructor) 74 | tracker_ = new Tracking(this, vocabulary_, frame_drawer_, map_drawer_, map_, 75 | keyframe_database_, string_setting_file); 76 | 77 | // Initialize the Local Mapping thread and launch 78 | local_mapper_ = new LocalMapping(map_, true); 79 | thread_local_mapping_ = 80 | new thread(&ORB_SLAM2::LocalMapping::Run, local_mapper_); 81 | 82 | // Initialize the Loop Closing thread and launch 83 | loop_closer_ = new LoopClosing(map_, keyframe_database_, vocabulary_, false); 84 | thread_loop_closing_ = new thread(&ORB_SLAM2::LoopClosing::Run, loop_closer_); 85 | 86 | // Initialize the Viewer thread and launch 87 | viewer_ = new Viewer(this, frame_drawer_, map_drawer_, tracker_, 88 | string_setting_file); 89 | thread_viewer_ = new thread(&Viewer::Run, viewer_); 90 | tracker_->SetViewer(viewer_); 91 | 92 | // Set pointers between threads 93 | tracker_->SetLocalMapper(local_mapper_); 94 | tracker_->SetLoopClosing(loop_closer_); 95 | 96 | local_mapper_->SetTracker(tracker_); 97 | local_mapper_->SetLoopCloser(loop_closer_); 98 | 99 | loop_closer_->SetTracker(tracker_); 100 | loop_closer_->SetLocalMapper(local_mapper_); 101 | } 102 | 103 | Eigen::Matrix4d MonoORBSlam::TrackMonocular(const cv::Mat& img, 104 | const double& timestamp) { 105 | { 106 | unique_lock lock(mutex_mode_); 107 | if (is_activate_localization_mode_) { 108 | local_mapper_->RequestStop(); 109 | 110 | // Wait until Local Mapping has effectively stopped 111 | while (!local_mapper_->isStopped()) { 112 | usleep(1000); 113 | } 114 | tracker_->InformOnlyTracking(true); 115 | is_activate_localization_mode_ = false; 116 | } 117 | 118 | if (is_deactivate_localization_mode_) { 119 | tracker_->InformOnlyTracking(false); 120 | local_mapper_->Release(); 121 | is_deactivate_localization_mode_ = false; 122 | } 123 | } 124 | 125 | { 126 | unique_lock lock(mutex_reset_); 127 | if (do_reset_) { 128 | tracker_->Reset(); 129 | do_reset_ = false; 130 | } 131 | } 132 | 133 | Eigen::Matrix4d Tcw = tracker_->GrabImageMonocular(img, timestamp); 134 | 135 | unique_lock lock2(mutex_state_); 136 | tracking_state_ = tracker_->state_; 137 | tracked_map_points_ = tracker_->current_frame_.map_points_; 138 | tracked_undistort_keypoints_ = tracker_->current_frame_.undistort_keypoints_; 139 | 140 | return Tcw; 141 | } 142 | 143 | void MonoORBSlam::ActivateLocalizationMode() { 144 | unique_lock lock(mutex_mode_); 145 | is_activate_localization_mode_ = true; 146 | } 147 | 148 | void MonoORBSlam::DeactivateLocalizationMode() { 149 | unique_lock lock(mutex_mode_); 150 | is_deactivate_localization_mode_ = true; 151 | } 152 | 153 | bool MonoORBSlam::MapChanged() { 154 | static int n = 0; 155 | int curn = map_->GetLastBigChangeIdx(); 156 | if (n < curn) { 157 | n = curn; 158 | return true; 159 | } else { 160 | return false; 161 | } 162 | } 163 | 164 | void MonoORBSlam::Reset() { 165 | unique_lock lock(mutex_reset_); 166 | do_reset_ = true; 167 | } 168 | 169 | void MonoORBSlam::Shutdown() { 170 | local_mapper_->RequestFinish(); 171 | loop_closer_->RequestFinish(); 172 | if (viewer_) { 173 | viewer_->RequestFinish(); 174 | while (!viewer_->isFinished()) { 175 | usleep(5000); 176 | } 177 | } 178 | // Wait until all thread have effectively stopped 179 | while (!local_mapper_->isFinished() || !loop_closer_->isFinished() || 180 | loop_closer_->isRunningGBA()) { 181 | usleep(5000); 182 | } 183 | 184 | if (viewer_) { 185 | pangolin::BindToContext("ORB-SLAM2: Map Viewer"); 186 | } 187 | } 188 | 189 | void MonoORBSlam::SaveTrajectoryTUM(const string& filename) { 190 | LOG(INFO) << "Cannot save camera trajectory for mono"; 191 | return; 192 | } 193 | 194 | void MonoORBSlam::SaveMap(const string& filename) { 195 | LOG(INFO) << "Saving map to " << filename << "..."; 196 | 197 | cv::FileStorage f(filename, cv::FileStorage::WRITE); 198 | 199 | LOG(INFO) << "Saving map points" << "..."; 200 | f << "MapPoints" << "["; 201 | 202 | std::vector map_points = map_->GetAllMapPoints(); 203 | sort(map_points.begin(), map_points.end(), MapPoint::lId); 204 | for (const auto& map_point : map_points) { 205 | if (map_point->isBad()) 206 | continue; 207 | cv::Mat pos = MatEigenConverter::Vector3dToMat(map_point->GetWorldPos()); 208 | f << "{:" 209 | << "id" << std::to_string(map_point->id_) 210 | << "pos" << pos 211 | << "descriptor" << map_point->GetDescriptor() 212 | << "}"; 213 | } 214 | f << "]"; 215 | LOG(INFO) << "map points saved!"; 216 | 217 | LOG(INFO) << "Saving keyframes " << filename << " ..."; 218 | std::vector keyframes = map_->GetAllKeyFrames(); 219 | sort(keyframes.begin(), keyframes.end(), KeyFrame::lId); 220 | f << "KeyFrames" << "["; 221 | for (const auto& keyframe : keyframes) { 222 | if (keyframe->isBad()) 223 | continue; 224 | cv::Mat R = 225 | MatEigenConverter::Matrix3dToMat(keyframe->GetRotation().transpose()); 226 | cv::Mat t = MatEigenConverter::Vector3dToMat(keyframe->GetCameraCenter()); 227 | std::set map_point_set = keyframe->GetMapPoints(); 228 | cv::Mat map_point_indices(1, map_point_set.size(), CV_32F); 229 | int count = 0; 230 | for (auto it = map_point_set.begin(); it != map_point_set.end(); it++) { 231 | map_point_indices.at(0, count) = (*it)->id_; 232 | count++; 233 | } 234 | 235 | f << "{:" 236 | << "id" << std::to_string(keyframe->id_) 237 | << "timestamp" << keyframe->timestamp_ 238 | << "R" << R 239 | << "t" << t 240 | << "map_point indices" << map_point_indices 241 | << "}"; 242 | } 243 | f << "]"; 244 | 245 | f.release(); 246 | LOG(INFO) << "Map saved!"; 247 | } 248 | 249 | void MonoORBSlam::SaveKeyFrameTrajectoryTUM(const string& filename) { 250 | LOG(INFO) << "Saving keyframe trajectory to " << filename << " ..."; 251 | 252 | std::vector keyframes = map_->GetAllKeyFrames(); 253 | sort(keyframes.begin(), keyframes.end(), KeyFrame::lId); 254 | 255 | // Transform all keyframes so that the first keyframe is at the origin. 256 | // After a loop closure the first keyframe might not be at the origin. 257 | // cv::Mat Two = vpKFs[0]->GetPoseInverse(); 258 | ofstream f; 259 | f.open(filename.c_str()); 260 | f << fixed; 261 | 262 | for (size_t i = 0; i < keyframes.size(); i++) { 263 | KeyFrame* keyframe = keyframes[i]; 264 | 265 | if (keyframe->isBad()) { 266 | continue; 267 | } 268 | 269 | Eigen::Matrix3d R = keyframe->GetRotation().transpose(); 270 | Eigen::Quaterniond q(R); 271 | Eigen::Vector3d t = keyframe->GetCameraCenter(); 272 | f << setprecision(6) << keyframe->timestamp_ << setprecision(7) << " " 273 | << t[0] << " " << t[1] << " " << t[2] << " " << q.x() << " " << q.y() 274 | << " " << q.z() << " " << q.w() << endl; 275 | } 276 | f.close(); 277 | LOG(INFO) << "trajectory saved!"; 278 | } 279 | 280 | int MonoORBSlam::GetTrackingState() { 281 | unique_lock lock(mutex_state_); 282 | return tracking_state_; 283 | } 284 | 285 | std::vector MonoORBSlam::GetTrackedMapPoints() { 286 | unique_lock lock(mutex_state_); 287 | return tracked_map_points_; 288 | } 289 | 290 | std::vector MonoORBSlam::GetTrackedKeyPointsUn() { 291 | unique_lock lock(mutex_state_); 292 | return tracked_undistort_keypoints_; 293 | } 294 | 295 | } // namespace ORB_SLAM2 296 | -------------------------------------------------------------------------------- /src/Viewer.cc: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: Viewer.cc 6 | * 7 | * Created On: Wed 04 Sep 2019 06:59:03 PM CST 8 | * Licensed under The GPLv3 License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | /** 12 | * This file is part of ORB-SLAM2. 13 | * 14 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 15 | * of Zaragoza) For more information see 16 | * 17 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 18 | * it under the terms of the GNU General Public License as published by 19 | * the Free Software Foundation, either version 3 of the License, or 20 | * (at your option) any later version. 21 | * 22 | * ORB-SLAM2 is distributed in the hope that it will be useful, 23 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 24 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 25 | * GNU General Public License for more details. 26 | * 27 | * You should have received a copy of the GNU General Public License 28 | * along with ORB-SLAM2. If not, see . 29 | */ 30 | 31 | #include "Viewer.h" 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | namespace ORB_SLAM2 { 38 | 39 | Viewer::Viewer(MonoORBSlam* mono_orb_slam, FrameDrawer* frame_drawer, 40 | MapDrawer* map_drawer, Tracking* tracker, 41 | const string& string_setting_file) 42 | : mono_orb_slam_(mono_orb_slam), 43 | frame_drawer_(frame_drawer), 44 | map_drawer_(map_drawer), 45 | tracker_(tracker), 46 | is_finish_requested_(false), 47 | is_finished_(true), 48 | is_follow_(false), 49 | is_stopped_(true), 50 | is_stop_requested_(false) { 51 | cv::FileStorage fSettings(string_setting_file, cv::FileStorage::READ); 52 | float fps = fSettings["Camera.fps"]; 53 | 54 | if (fps < 1) fps = 30; 55 | T_ = 1e3 / fps; 56 | 57 | img_width_ = fSettings["Camera.width"]; 58 | img_height_ = fSettings["Camera.height"]; 59 | if (img_width_ < 1 || img_height_ < 1) { 60 | img_width_ = 640; 61 | img_height_ = 480; 62 | } 63 | 64 | view_point_x_ = fSettings["Viewer.ViewpointX"]; 65 | view_point_y_ = fSettings["Viewer.ViewpointY"]; 66 | view_point_z_ = fSettings["Viewer.ViewpointZ"]; 67 | view_point_f_ = fSettings["Viewer.ViewpointF"]; 68 | } 69 | 70 | void Viewer::Run() { 71 | is_finished_ = false; 72 | is_stopped_ = false; 73 | 74 | pangolin::CreateWindowAndBind("ORB-SLAM2: Map Viewer", 1024, 768); 75 | 76 | // 3D Mouse handler requires depth testing to be enabled 77 | glEnable(GL_DEPTH_TEST); 78 | 79 | // Issue specific OpenGl we might need 80 | glEnable(GL_BLEND); 81 | glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); 82 | 83 | pangolin::CreatePanel("menu").SetBounds(0.0, 1.0, 0.0, 84 | pangolin::Attach::Pix(175)); 85 | pangolin::Var menuFollowCamera("menu.Follow Camera", false, true); 86 | pangolin::Var menuShowPoints("menu.Show Points", true, true); 87 | pangolin::Var menuShowKeyFrames("menu.Show KeyFrames", true, true); 88 | pangolin::Var menuShowGraph("menu.Show Graph", true, true); 89 | pangolin::Var menuLocalizationMode("menu.Localization Mode", false, 90 | true); 91 | pangolin::Var menuReset("menu.Reset", false, false); 92 | 93 | // Define Camera Render Object (for view / scene browsing) 94 | pangolin::OpenGlRenderState s_cam( 95 | pangolin::ProjectionMatrix(1024, 768, view_point_f_, view_point_f_, 512, 96 | 389, 0.1, 1000), 97 | pangolin::ModelViewLookAt(view_point_x_, view_point_y_, view_point_z_, 0, 98 | 0, 0, 0.0, -1.0, 0.0)); 99 | 100 | // Add named OpenGL viewport to window and provide 3D Handler 101 | pangolin::View& d_cam = pangolin::CreateDisplay() 102 | .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 103 | 1.0, -1024.0f / 768.0f) 104 | .SetHandler(new pangolin::Handler3D(s_cam)); 105 | 106 | pangolin::OpenGlMatrix Twc; 107 | Twc.SetIdentity(); 108 | 109 | cv::namedWindow("ORB-SLAM2: Current Frame"); 110 | 111 | bool is_localization_mode = false; 112 | 113 | while (true) { 114 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 115 | 116 | map_drawer_->GetCurrentOpenGLCameraMatrix(Twc); 117 | 118 | if (menuFollowCamera && is_follow_) { 119 | s_cam.Follow(Twc); 120 | } else if (menuFollowCamera && !is_follow_) { 121 | s_cam.SetModelViewMatrix( 122 | pangolin::ModelViewLookAt(view_point_x_, view_point_y_, view_point_z_, 123 | 0, 0, 0, 0.0, -1.0, 0.0)); 124 | s_cam.Follow(Twc); 125 | is_follow_ = true; 126 | } else if (!menuFollowCamera && is_follow_) { 127 | is_follow_ = false; 128 | } 129 | 130 | if (menuLocalizationMode && !is_localization_mode) { 131 | mono_orb_slam_->ActivateLocalizationMode(); 132 | is_localization_mode = true; 133 | } else if (!menuLocalizationMode && is_localization_mode) { 134 | mono_orb_slam_->DeactivateLocalizationMode(); 135 | is_localization_mode = false; 136 | } 137 | 138 | d_cam.Activate(s_cam); 139 | glClearColor(1.0f, 1.0f, 1.0f, 1.0f); 140 | map_drawer_->DrawCurrentCamera(Twc); 141 | if (menuShowKeyFrames || menuShowGraph) 142 | map_drawer_->DrawKeyFrames(menuShowKeyFrames, menuShowGraph); 143 | if (menuShowPoints) map_drawer_->DrawMapPoints(); 144 | 145 | pangolin::FinishFrame(); 146 | 147 | cv::Mat img = frame_drawer_->DrawFrame(); 148 | cv::imshow("ORB-SLAM2: Current Frame", img); 149 | cv::waitKey(T_); 150 | 151 | if (menuReset) { 152 | menuShowGraph = true; 153 | menuShowKeyFrames = true; 154 | menuShowPoints = true; 155 | menuLocalizationMode = false; 156 | if (is_localization_mode) { 157 | mono_orb_slam_->DeactivateLocalizationMode(); 158 | } 159 | is_localization_mode = false; 160 | is_follow_ = true; 161 | menuFollowCamera = true; 162 | mono_orb_slam_->Reset(); 163 | menuReset = false; 164 | } 165 | 166 | if (Stop()) { 167 | while (isStopped()) { 168 | usleep(3000); 169 | } 170 | } 171 | 172 | if (CheckFinish()) { 173 | break; 174 | } 175 | } 176 | 177 | SetFinish(); 178 | } 179 | 180 | void Viewer::RequestFinish() { 181 | unique_lock lock(mutex_finish_); 182 | is_finish_requested_ = true; 183 | } 184 | 185 | bool Viewer::CheckFinish() { 186 | unique_lock lock(mutex_finish_); 187 | return is_finish_requested_; 188 | } 189 | 190 | void Viewer::SetFinish() { 191 | unique_lock lock(mutex_finish_); 192 | is_finished_ = true; 193 | } 194 | 195 | bool Viewer::isFinished() { 196 | unique_lock lock(mutex_finish_); 197 | return is_finished_; 198 | } 199 | 200 | void Viewer::RequestStop() { 201 | unique_lock lock(mutex_stop_); 202 | if (!is_stopped_) is_stop_requested_ = true; 203 | } 204 | 205 | bool Viewer::isStopped() { 206 | unique_lock lock(mutex_stop_); 207 | return is_stopped_; 208 | } 209 | 210 | bool Viewer::Stop() { 211 | unique_lock lock(mutex_stop_); 212 | unique_lock lock2(mutex_finish_); 213 | 214 | if (is_finish_requested_) 215 | return false; 216 | else if (is_stop_requested_) { 217 | is_stopped_ = true; 218 | is_stop_requested_ = false; 219 | return true; 220 | } 221 | 222 | return false; 223 | } 224 | 225 | void Viewer::SetFollowCamera() { 226 | unique_lock lock(mutex_stop_); 227 | is_follow_ = true; 228 | } 229 | 230 | void Viewer::Release() { 231 | unique_lock lock(mutex_stop_); 232 | is_stopped_ = false; 233 | } 234 | } // namespace ORB_SLAM2 235 | -------------------------------------------------------------------------------- /src/main.cc: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: main.cc 6 | * 7 | * Created On: Tue 03 Sep 2019 06:19:56 PM CST 8 | * Licensed under The GPLv3 License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include "MonoORBSlam.h" 17 | 18 | DEFINE_string(voc, "vocabulary/ORBvoc.txt", "Path to ORB vocabulary"); 19 | DEFINE_string(config, "configs/TUM2.yaml", "Path to camera intrinsics"); 20 | DEFINE_string(images, "rgbd_dataset_freiburg2_desk", "Path to images folder"); 21 | 22 | void LoadImages(const std::string& strFile, std::vector& vstrImageFilenames, 23 | std::vector& vTimestamps) { 24 | std::ifstream f(strFile.c_str()); 25 | if (!f.good()) LOG(FATAL) << strFile << " not exists"; 26 | 27 | // skip first three lines 28 | std::string s0; 29 | getline(f, s0); 30 | getline(f, s0); 31 | getline(f, s0); 32 | 33 | while (!f.eof()) { 34 | std::string s; 35 | getline(f, s); 36 | if (!s.empty()) { 37 | std::stringstream ss; 38 | ss << s; 39 | double t; 40 | std::string sRGB; 41 | ss >> t; 42 | vTimestamps.push_back(t); 43 | ss >> sRGB; 44 | vstrImageFilenames.push_back(sRGB); 45 | } 46 | } 47 | } 48 | 49 | int main(int argc, char* argv[]) { 50 | google::InitGoogleLogging(argv[0]); 51 | google::ParseCommandLineFlags(&argc, &argv, true); 52 | 53 | FLAGS_logtostderr = true; 54 | FLAGS_colorlogtostderr = true; 55 | 56 | LOG(WARNING) << "Usage: " << argv[0] 57 | << " --voc ORBvoc.txt --config config.yaml --images imgs_dir"; 58 | std::vector string_image_filenames; 59 | std::vector timestamps; 60 | std::string string_file = std::string(FLAGS_images) + "/rgb.txt"; 61 | LoadImages(string_file, string_image_filenames, timestamps); 62 | 63 | int n_images = string_image_filenames.size(); 64 | 65 | ORB_SLAM2::MonoORBSlam slam(FLAGS_voc, FLAGS_config, true); 66 | 67 | std::vector times_track; 68 | times_track.resize(n_images); 69 | 70 | LOG(INFO) << "----------"; 71 | LOG(INFO) << "Start processing sequence ..."; 72 | LOG(INFO) << "Images in the sequence: " << n_images; 73 | 74 | cv::Mat img; 75 | for (int i = 0; i < n_images; i++) { 76 | img = 77 | cv::imread(std::string(FLAGS_images) + "/" + string_image_filenames[i], 78 | CV_LOAD_IMAGE_UNCHANGED); 79 | double tframe = timestamps[i]; 80 | 81 | if (img.empty()) { 82 | LOG(FATAL) << "Failed to load image at: " << std::string(FLAGS_images) 83 | << "/" << string_image_filenames[i]; 84 | } 85 | std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); 86 | 87 | slam.TrackMonocular(img, tframe); 88 | 89 | std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now(); 90 | 91 | double ttrack = 92 | std::chrono::duration_cast>(t2 - t1) 93 | .count(); 94 | 95 | times_track[i] = ttrack; 96 | 97 | double T = 0; 98 | if (i < n_images - 1) { 99 | T = timestamps[i + 1] - tframe; 100 | } else if (i > 0) { 101 | T = tframe - timestamps[i - 1]; 102 | } 103 | 104 | if (ttrack < T) { 105 | usleep((T - ttrack) * 1e6); 106 | } 107 | // if (i == n_images - 1) cv::waitKey(0); 108 | } 109 | 110 | slam.Shutdown(); 111 | sort(times_track.begin(), times_track.end()); 112 | float total_time = 0; 113 | for (int i = 0; i < n_images; i++) { 114 | total_time += times_track[i]; 115 | } 116 | LOG(INFO) << "-------------"; 117 | LOG(INFO) << "median tracking time: " << times_track[n_images / 2]; 118 | LOG(INFO) << "mean tracking time: " << total_time / n_images; 119 | 120 | slam.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); 121 | slam.SaveMap("map.yaml"); 122 | 123 | cv::waitKey(0); 124 | return 0; 125 | } 126 | -------------------------------------------------------------------------------- /vocabulary/ORBvoc.txt.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/b51/ceres_mono_orb_slam2/831abd513c5620bf802c11069b5c450a5d6b30e4/vocabulary/ORBvoc.txt.tar.gz --------------------------------------------------------------------------------