├── .gitignore ├── CMakeLists.txt ├── Dependencies.md ├── LICENSE.txt ├── README.md ├── code ├── .gitignore ├── Dependencies.md ├── Examples │ ├── Monocular │ │ ├── EuRoC.yaml │ │ ├── EuRoC_TimeStamps │ │ │ ├── MH01.txt │ │ │ ├── MH02.txt │ │ │ ├── MH03.txt │ │ │ ├── MH04.txt │ │ │ ├── MH05.txt │ │ │ ├── V101.txt │ │ │ ├── V102.txt │ │ │ ├── V103.txt │ │ │ ├── V201.txt │ │ │ ├── V202.txt │ │ │ └── V203.txt │ │ ├── KITTI00-02.yaml │ │ ├── KITTI03.yaml │ │ ├── KITTI04-12.yaml │ │ ├── TUM1.yaml │ │ ├── TUM2.yaml │ │ ├── TUM3.yaml │ │ ├── client.cc │ │ ├── mono_euroc.cc │ │ ├── mono_kitti.cc │ │ ├── mono_tum.cc │ │ ├── relocalizer.cc │ │ ├── server.cc │ │ └── swarm_map.cc │ ├── RGB-D │ │ ├── TUM1.yaml │ │ ├── TUM2.yaml │ │ ├── TUM3.yaml │ │ ├── associations │ │ │ ├── fr1_desk.txt │ │ │ ├── fr1_desk2.txt │ │ │ ├── fr1_room.txt │ │ │ ├── fr1_xyz.txt │ │ │ ├── fr2_desk.txt │ │ │ ├── fr2_xyz.txt │ │ │ ├── fr3_nstr_tex_near.txt │ │ │ ├── fr3_office.txt │ │ │ ├── fr3_office_val.txt │ │ │ ├── fr3_str_tex_far.txt │ │ │ └── fr3_str_tex_near.txt │ │ └── rgbd_tum.cc │ ├── ROS │ │ └── ORB_SLAM2 │ │ │ ├── Asus.yaml │ │ │ ├── CMakeLists.txt │ │ │ ├── manifest.xml │ │ │ └── src │ │ │ ├── AR │ │ │ ├── ViewerAR.cc │ │ │ ├── ViewerAR.h │ │ │ └── ros_mono_ar.cc │ │ │ ├── ros_mono.cc │ │ │ ├── ros_rgbd.cc │ │ │ └── ros_stereo.cc │ └── Stereo │ │ ├── EuRoC.yaml │ │ ├── EuRoC_TimeStamps │ │ ├── MH01.txt │ │ ├── MH02.txt │ │ ├── MH03.txt │ │ ├── MH04.txt │ │ ├── MH05.txt │ │ ├── V101.txt │ │ ├── V102.txt │ │ ├── V103.txt │ │ ├── V201.txt │ │ ├── V202.txt │ │ └── V203.txt │ │ ├── KITTI00-02.yaml │ │ ├── KITTI03.yaml │ │ ├── KITTI04-12.yaml │ │ ├── stereo_euroc.cc │ │ └── stereo_kitti.cc ├── LICENSE.txt ├── License-gpl.txt ├── README.md ├── Thirdparty │ ├── 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 │ │ │ └── config.h │ │ ├── LICENSE.txt │ │ └── README.txt │ ├── g2o │ │ ├── CMakeLists.txt │ │ ├── README.txt │ │ ├── cmake_modules │ │ │ ├── FindBLAS.cmake │ │ │ ├── FindEigen3.cmake │ │ │ └── FindLAPACK.cmake │ │ ├── config.h.in │ │ ├── g2o │ │ │ ├── core │ │ │ │ ├── base_binary_edge.h │ │ │ │ ├── base_binary_edge.hpp │ │ │ │ ├── base_edge.h │ │ │ │ ├── base_multi_edge.h │ │ │ │ ├── base_multi_edge.hpp │ │ │ │ ├── base_unary_edge.h │ │ │ │ ├── base_unary_edge.hpp │ │ │ │ ├── base_vertex.h │ │ │ │ ├── base_vertex.hpp │ │ │ │ ├── batch_stats.cpp │ │ │ │ ├── batch_stats.h │ │ │ │ ├── block_solver.h │ │ │ │ ├── block_solver.hpp │ │ │ │ ├── cache.cpp │ │ │ │ ├── cache.h │ │ │ │ ├── creators.h │ │ │ │ ├── eigen_types.h │ │ │ │ ├── estimate_propagator.cpp │ │ │ │ ├── estimate_propagator.h │ │ │ │ ├── factory.cpp │ │ │ │ ├── factory.h │ │ │ │ ├── hyper_dijkstra.cpp │ │ │ │ ├── hyper_dijkstra.h │ │ │ │ ├── hyper_graph.cpp │ │ │ │ ├── hyper_graph.h │ │ │ │ ├── hyper_graph_action.cpp │ │ │ │ ├── hyper_graph_action.h │ │ │ │ ├── jacobian_workspace.cpp │ │ │ │ ├── jacobian_workspace.h │ │ │ │ ├── linear_solver.h │ │ │ │ ├── marginal_covariance_cholesky.cpp │ │ │ │ ├── marginal_covariance_cholesky.h │ │ │ │ ├── matrix_operations.h │ │ │ │ ├── matrix_structure.cpp │ │ │ │ ├── matrix_structure.h │ │ │ │ ├── openmp_mutex.h │ │ │ │ ├── optimizable_graph.cpp │ │ │ │ ├── optimizable_graph.h │ │ │ │ ├── optimization_algorithm.cpp │ │ │ │ ├── optimization_algorithm.h │ │ │ │ ├── optimization_algorithm_dogleg.cpp │ │ │ │ ├── optimization_algorithm_dogleg.h │ │ │ │ ├── optimization_algorithm_factory.cpp │ │ │ │ ├── optimization_algorithm_factory.h │ │ │ │ ├── optimization_algorithm_gauss_newton.cpp │ │ │ │ ├── optimization_algorithm_gauss_newton.h │ │ │ │ ├── optimization_algorithm_levenberg.cpp │ │ │ │ ├── optimization_algorithm_levenberg.h │ │ │ │ ├── optimization_algorithm_property.h │ │ │ │ ├── optimization_algorithm_with_hessian.cpp │ │ │ │ ├── optimization_algorithm_with_hessian.h │ │ │ │ ├── parameter.cpp │ │ │ │ ├── parameter.h │ │ │ │ ├── parameter_container.cpp │ │ │ │ ├── parameter_container.h │ │ │ │ ├── robust_kernel.cpp │ │ │ │ ├── robust_kernel.h │ │ │ │ ├── robust_kernel_factory.cpp │ │ │ │ ├── robust_kernel_factory.h │ │ │ │ ├── robust_kernel_impl.cpp │ │ │ │ ├── robust_kernel_impl.h │ │ │ │ ├── solver.cpp │ │ │ │ ├── solver.h │ │ │ │ ├── sparse_block_matrix.h │ │ │ │ ├── sparse_block_matrix.hpp │ │ │ │ ├── sparse_block_matrix_ccs.h │ │ │ │ ├── sparse_block_matrix_diagonal.h │ │ │ │ ├── sparse_block_matrix_test.cpp │ │ │ │ ├── sparse_optimizer.cpp │ │ │ │ └── sparse_optimizer.h │ │ │ ├── solvers │ │ │ │ ├── linear_solver_dense.h │ │ │ │ └── linear_solver_eigen.h │ │ │ ├── stuff │ │ │ │ ├── color_macros.h │ │ │ │ ├── macros.h │ │ │ │ ├── misc.h │ │ │ │ ├── os_specific.c │ │ │ │ ├── os_specific.h │ │ │ │ ├── property.cpp │ │ │ │ ├── property.h │ │ │ │ ├── string_tools.cpp │ │ │ │ ├── string_tools.h │ │ │ │ ├── timeutil.cpp │ │ │ │ └── timeutil.h │ │ │ └── types │ │ │ │ ├── se3_ops.h │ │ │ │ ├── se3_ops.hpp │ │ │ │ ├── se3quat.h │ │ │ │ ├── sim3.h │ │ │ │ ├── types_sba.cpp │ │ │ │ ├── types_sba.h │ │ │ │ ├── types_seven_dof_expmap.cpp │ │ │ │ ├── types_seven_dof_expmap.h │ │ │ │ ├── types_six_dof_expmap.cpp │ │ │ │ └── types_six_dof_expmap.h │ │ └── license-bsd.txt │ └── popl │ │ └── popl.hpp ├── Vocabulary │ └── ORBvoc.bin ├── build.sh ├── build_ros.sh ├── cmake_modules │ └── FindEigen3.cmake ├── include │ ├── AgentMediator.h │ ├── BoostArchiver.h │ ├── CLogger.h │ ├── ClientService.h │ ├── ConnectionService.h │ ├── Converter.h │ ├── DataSetUtil.h │ ├── DynamicExtractor.h │ ├── DynamicRunner.h │ ├── Frame.h │ ├── FrameDrawer.h │ ├── Initializer.h │ ├── KeyFrame.h │ ├── KeyFrameDatabase.h │ ├── LandmarkScoring.h │ ├── LocalMapping.h │ ├── LoopClosing.h │ ├── Map.h │ ├── MapDrawer.h │ ├── MapElementUpdate.h │ ├── MapEnhancer.h │ ├── MapManager.h │ ├── MapPoint.h │ ├── MapSlice.h │ ├── MapUpdater.h │ ├── Mapit.h │ ├── MediatorScheduler.h │ ├── ORBVocabulary.h │ ├── ORBextractor.h │ ├── ORBmatcher.h │ ├── Optimizer.h │ ├── PnPsolver.h │ ├── STS.h │ ├── ServerService.h │ ├── Sim3Solver.h │ ├── System.h │ ├── SystemState.h │ ├── Timer.h │ ├── Tracking.h │ ├── Utils.hpp │ ├── Viewer.h │ ├── WebSocket.h │ ├── cuda │ │ ├── Allocator.hpp │ │ ├── Cuda.hpp │ │ ├── Fast.hpp │ │ └── Orb.hpp │ ├── g2o │ │ └── EdgeSim3RelativeXYZ.h │ └── nanoflann.hpp └── src │ ├── AgentMediator.cc │ ├── ClientService.cc │ ├── Converter.cc │ ├── DataSetUtil.cc │ ├── DynamicExtractor.cc │ ├── DynamicRunner.cc │ ├── Frame.cc │ ├── FrameDrawer.cc │ ├── Initializer.cc │ ├── KeyFrame.cc │ ├── KeyFrameDatabase.cc │ ├── LandmarkScoring.cc │ ├── LocalMapping.cc │ ├── LoopClosing.cc │ ├── Map.cc │ ├── MapDrawer.cc │ ├── MapEnhancer.cc │ ├── MapManager.cc │ ├── MapPoint.cc │ ├── MapSlice.cc │ ├── MapUpdater.cc │ ├── Mapit.cc │ ├── MediatorScheduler.cc │ ├── Network │ ├── ConnectionService.cc │ ├── MapElementUpdate.cc │ └── WebSocket.cc │ ├── ORBextractor.cc │ ├── ORBmatcher.cc │ ├── Optimizer.cc │ ├── PnPsolver.cc │ ├── STS.cc │ ├── ServerService.cc │ ├── Sim3Solver.cc │ ├── System.cc │ ├── Timer.cc │ ├── Tracking.cc │ ├── Viewer.cc │ └── cuda │ ├── Allocator_gpu.cu │ ├── Cuda.cu │ ├── Fast_gpu.cu │ └── Orb_gpu.cu └── config ├── fr2-desk.yaml ├── fr2-large12.yaml ├── kitti00-02.yaml ├── mh1.yaml ├── mh123.yaml ├── mh12345.yaml ├── mh2.yaml ├── v1-123.yaml └── v2-123.yaml /Dependencies.md: -------------------------------------------------------------------------------- 1 | ##List of Known Dependencies 2 | ###SwarmMap version 1.0 3 | 4 | In this document we list all the pieces of code included by SwarmMap and linked libraries which are not property of the authors of SwarmMap. 5 | 6 | #####Code in **src** and **include** folders 7 | 8 | * *ORBextractor.cc*. 9 | This is a modified version of orb.cpp of OpenCV library. The original code is BSD licensed. 10 | 11 | * *PnPsolver.h, PnPsolver.cc*. 12 | This is a modified version of the epnp.h and epnp.cc of Vincent Lepetit. 13 | This code can be found in popular BSD licensed computer vision libraries as [OpenCV](https://github.com/Itseez/opencv/blob/master/modules/calib3d/src/epnp.cpp) and [OpenGV](https://github.com/laurentkneip/opengv/blob/master/src/absolute_pose/modules/Epnp.cpp). The original code is FreeBSD. 14 | 15 | * Function *ORBmatcher::DescriptorDistance* in *ORBmatcher.cc*. 16 | The code is from: http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel. 17 | The code is in the public domain. 18 | 19 | #####Code in Thirdparty folder 20 | 21 | * All code in **DBoW2** folder. 22 | This is a modified version of [DBoW2](https://github.com/dorian3d/DBoW2) and [DLib](https://github.com/dorian3d/DLib) library. All files included are BSD licensed. 23 | 24 | * All code in **g2o** folder. 25 | This is a modified version of [g2o](https://github.com/RainerKuemmerle/g2o). All files included are BSD licensed. 26 | 27 | * All code in **popl** foler. 28 | The code is from [popl](https://github.com/badaix/popl). [MIT license](https://github.com/badaix/popl/blob/master/LICENSE) 29 | 30 | #####Library dependencies 31 | 32 | * **Pangolin (visualization and user interface)**. 33 | [MIT license](https://en.wikipedia.org/wiki/MIT_License). 34 | 35 | * **OpenCV**. 36 | BSD license. 37 | 38 | * **Eigen3**. 39 | For versions greater than 3.1.1 is MPL2, earlier versions are LGPLv3. 40 | 41 | * **ROS (Optional, only if you build Examples/ROS)**. 42 | BSD license. In the manifest.xml the only declared package dependencies are roscpp, tf, sensor_msgs, image_transport, cv_bridge, which are all BSD licensed. 43 | 44 | * **spdlog**. 45 | MIT License 46 | 47 | * **boost**. 48 | Boost Software License 1.0 49 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | SwarmMap is released under a GPLv3 license (see License-gpl.txt). 2 | Please see Dependencies.md for a list of all included code and library dependencies which are not property of the authors of SwarmMap. 3 | 4 | For a closed-source version of SwarmMap for commercial purposes, please contact the authors. 5 | 6 | If you use SwarmMap in academic work, please cite the most relevant publication associated by visiting: 7 | https://github.com/MobiSense/SwarmMap 8 | -------------------------------------------------------------------------------- /code/.gitignore: -------------------------------------------------------------------------------- 1 | CMakeLists.txt.user 2 | Examples/Monocular/mono_euroc 3 | Examples/Monocular/mono_kitti 4 | Examples/Monocular/mono_tum 5 | Examples/RGB-D/rgbd_tum 6 | Examples/ROS/ORB_SLAM2/CMakeLists.txt.user 7 | Examples/ROS/ORB_SLAM2/Mono 8 | Examples/ROS/ORB_SLAM2/MonoAR 9 | Examples/ROS/ORB_SLAM2/RGBD 10 | Examples/ROS/ORB_SLAM2/Stereo 11 | Examples/ROS/ORB_SLAM2/build/ 12 | Examples/Stereo/stereo_euroc 13 | Examples/Stereo/stereo_kitti 14 | KeyFrameTrajectory.txt 15 | Thirdparty/DBoW2/build/ 16 | Thirdparty/DBoW2/lib/ 17 | Thirdparty/g2o/build/ 18 | Thirdparty/g2o/config.h 19 | Thirdparty/g2o/lib/ 20 | Vocabulary/ORBvoc.txt 21 | build/ 22 | *~ 23 | lib/ 24 | 25 | -------------------------------------------------------------------------------- /code/Dependencies.md: -------------------------------------------------------------------------------- 1 | ##List of Known Dependencies 2 | ###ORB-SLAM2 version 1.0 3 | 4 | In this document we list all the pieces of code included by ORB-SLAM2 and linked libraries which are not property of the authors of ORB-SLAM2. 5 | 6 | 7 | #####Code in **src** and **include** folders 8 | 9 | * *ORBextractor.cc*. 10 | This is a modified version of orb.cpp of OpenCV library. The original code is BSD licensed. 11 | 12 | * *PnPsolver.h, PnPsolver.cc*. 13 | This is a modified version of the epnp.h and epnp.cc of Vincent Lepetit. 14 | This code can be found in popular BSD licensed computer vision libraries as [OpenCV](https://github.com/Itseez/opencv/blob/master/modules/calib3d/src/epnp.cpp) and [OpenGV](https://github.com/laurentkneip/opengv/blob/master/src/absolute_pose/modules/Epnp.cpp). The original code is FreeBSD. 15 | 16 | * Function *ORBmatcher::DescriptorDistance* in *ORBmatcher.cc*. 17 | The code is from: http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel. 18 | The code is in the public domain. 19 | 20 | #####Code in Thirdparty folder 21 | 22 | * All code in **DBoW2** folder. 23 | This is a modified version of [DBoW2](https://github.com/dorian3d/DBoW2) and [DLib](https://github.com/dorian3d/DLib) library. All files included are BSD licensed. 24 | 25 | * All code in **g2o** folder. 26 | This is a modified version of [g2o](https://github.com/RainerKuemmerle/g2o). All files included are BSD licensed. 27 | 28 | #####Library dependencies 29 | 30 | * **Pangolin (visualization and user interface)**. 31 | [MIT license](https://en.wikipedia.org/wiki/MIT_License). 32 | 33 | * **OpenCV**. 34 | BSD license. 35 | 36 | * **Eigen3**. 37 | For versions greater than 3.1.1 is MPL2, earlier versions are LGPLv3. 38 | 39 | * **ROS (Optional, only if you build Examples/ROS)**. 40 | BSD license. In the manifest.xml the only declared package dependencies are roscpp, tf, sensor_msgs, image_transport, cv_bridge, which are all BSD licensed. 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /code/Examples/Monocular/EuRoC.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 458.654 9 | Camera.fy: 457.296 10 | Camera.cx: 367.215 11 | Camera.cy: 248.375 12 | 13 | Camera.k1: -0.28340811 14 | Camera.k2: 0.07395907 15 | Camera.p1: 0.00019359 16 | Camera.p2: 1.76187114e-05 17 | 18 | # Camera frames per second 19 | Camera.fps: 20.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 1000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #--------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.05 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 0.9 50 | Viewer.PointSize:2 51 | Viewer.CameraSize: 0.08 52 | Viewer.CameraLineWidth: 3 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -0.7 55 | Viewer.ViewpointZ: -1.8 56 | Viewer.ViewpointF: 500 57 | 58 | -------------------------------------------------------------------------------- /code/Examples/Monocular/KITTI00-02.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 718.856 9 | Camera.fy: 718.856 10 | Camera.cx: 607.1928 11 | Camera.cy: 185.2157 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | # Camera frames per second 19 | Camera.fps: 10.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 2000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #-------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.1 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 1 50 | Viewer.PointSize:2 51 | Viewer.CameraSize: 0.15 52 | Viewer.CameraLineWidth: 2 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -10 55 | Viewer.ViewpointZ: -0.1 56 | Viewer.ViewpointF: 2000 57 | 58 | -------------------------------------------------------------------------------- /code/Examples/Monocular/KITTI03.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 721.5377 9 | Camera.fy: 721.5377 10 | Camera.cx: 609.5593 11 | Camera.cy: 172.854 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | # Camera frames per second 19 | Camera.fps: 10.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 2000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #-------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.1 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 1 50 | Viewer.PointSize:2 51 | Viewer.CameraSize: 0.15 52 | Viewer.CameraLineWidth: 2 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -10 55 | Viewer.ViewpointZ: -0.1 56 | Viewer.ViewpointF: 2000 57 | 58 | -------------------------------------------------------------------------------- /code/Examples/Monocular/KITTI04-12.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 707.0912 9 | Camera.fy: 707.0912 10 | Camera.cx: 601.8873 11 | Camera.cy: 183.1104 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | # Camera frames per second 19 | Camera.fps: 10.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 2000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #-------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.1 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 1 50 | Viewer.PointSize:2 51 | Viewer.CameraSize: 0.15 52 | Viewer.CameraLineWidth: 2 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -10 55 | Viewer.ViewpointZ: -0.1 56 | Viewer.ViewpointF: 2000 57 | 58 | -------------------------------------------------------------------------------- /code/Examples/Monocular/TUM1.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 517.306408 9 | Camera.fy: 516.469215 10 | Camera.cx: 318.643040 11 | Camera.cy: 255.313989 12 | 13 | Camera.k1: 0.262383 14 | Camera.k2: -0.953104 15 | Camera.p1: -0.005358 16 | Camera.p2: 0.002628 17 | Camera.k3: 1.163314 18 | 19 | # Camera frames per second 20 | Camera.fps: 30.0 21 | 22 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 23 | Camera.RGB: 1 24 | 25 | #-------------------------------------------------------------------------------------------- 26 | # ORB Parameters 27 | #-------------------------------------------------------------------------------------------- 28 | 29 | # ORB Extractor: Number of features per image 30 | ORBextractor.nFeatures: 1000 31 | 32 | # ORB Extractor: Scale factor between levels in the scale pyramid 33 | ORBextractor.scaleFactor: 1.2 34 | 35 | # ORB Extractor: Number of levels in the scale pyramid 36 | ORBextractor.nLevels: 8 37 | 38 | # ORB Extractor: Fast threshold 39 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 40 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 41 | # You can lower these values if your images have low contrast 42 | ORBextractor.iniThFAST: 20 43 | ORBextractor.minThFAST: 7 44 | 45 | #-------------------------------------------------------------------------------------------- 46 | # Viewer Parameters 47 | #-------------------------------------------------------------------------------------------- 48 | Viewer.KeyFrameSize: 0.05 49 | Viewer.KeyFrameLineWidth: 1 50 | Viewer.GraphLineWidth: 0.9 51 | Viewer.PointSize:2 52 | Viewer.CameraSize: 0.08 53 | Viewer.CameraLineWidth: 3 54 | Viewer.ViewpointX: 0 55 | Viewer.ViewpointY: -0.7 56 | Viewer.ViewpointZ: -1.8 57 | Viewer.ViewpointF: 500 58 | 59 | -------------------------------------------------------------------------------- /code/Examples/Monocular/TUM2.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 520.908620 9 | Camera.fy: 521.007327 10 | Camera.cx: 325.141442 11 | Camera.cy: 249.701764 12 | 13 | Camera.k1: 0.231222 14 | Camera.k2: -0.784899 15 | Camera.p1: -0.003257 16 | Camera.p2: -0.000105 17 | Camera.k3: 0.917205 18 | 19 | # Camera frames per second 20 | Camera.fps: 30.0 21 | 22 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 23 | Camera.RGB: 1 24 | 25 | #-------------------------------------------------------------------------------------------- 26 | # ORB Parameters 27 | #-------------------------------------------------------------------------------------------- 28 | 29 | # ORB Extractor: Number of features per image 30 | ORBextractor.nFeatures: 1000 31 | 32 | # ORB Extractor: Scale factor between levels in the scale pyramid 33 | ORBextractor.scaleFactor: 1.2 34 | 35 | # ORB Extractor: Number of levels in the scale pyramid 36 | ORBextractor.nLevels: 8 37 | 38 | # ORB Extractor: Fast threshold 39 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 40 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 41 | # You can lower these values if your images have low contrast 42 | ORBextractor.iniThFAST: 20 43 | ORBextractor.minThFAST: 7 44 | 45 | #-------------------------------------------------------------------------------------------- 46 | # Viewer Parameters 47 | #-------------------------------------------------------------------------------------------- 48 | Viewer.KeyFrameSize: 0.05 49 | Viewer.KeyFrameLineWidth: 1 50 | Viewer.GraphLineWidth: 0.9 51 | Viewer.PointSize:2 52 | Viewer.CameraSize: 0.08 53 | Viewer.CameraLineWidth: 3 54 | Viewer.ViewpointX: 0 55 | Viewer.ViewpointY: -0.7 56 | Viewer.ViewpointZ: -1.8 57 | Viewer.ViewpointF: 500 58 | 59 | -------------------------------------------------------------------------------- /code/Examples/Monocular/TUM3.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 535.4 9 | Camera.fy: 539.2 10 | Camera.cx: 320.1 11 | Camera.cy: 247.6 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | # Camera frames per second 19 | Camera.fps: 30.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 1000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #-------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.05 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 0.9 50 | Viewer.PointSize:2 51 | Viewer.CameraSize: 0.08 52 | Viewer.CameraLineWidth: 3 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -0.7 55 | Viewer.ViewpointZ: -1.8 56 | Viewer.ViewpointF: 500 57 | 58 | -------------------------------------------------------------------------------- /code/Examples/RGB-D/TUM1.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 517.306408 9 | Camera.fy: 516.469215 10 | Camera.cx: 318.643040 11 | Camera.cy: 255.313989 12 | 13 | Camera.k1: 0.262383 14 | Camera.k2: -0.953104 15 | Camera.p1: -0.005358 16 | Camera.p2: 0.002628 17 | Camera.k3: 1.163314 18 | 19 | Camera.width: 640 20 | Camera.height: 480 21 | 22 | # Camera frames per second 23 | Camera.fps: 30.0 24 | 25 | # IR projector baseline times fx (aprox.) 26 | Camera.bf: 40.0 27 | 28 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 29 | Camera.RGB: 1 30 | 31 | # Close/Far threshold. Baseline times. 32 | ThDepth: 40.0 33 | 34 | # Deptmap values factor 35 | DepthMapFactor: 5000.0 36 | 37 | #-------------------------------------------------------------------------------------------- 38 | # ORB Parameters 39 | #-------------------------------------------------------------------------------------------- 40 | 41 | # ORB Extractor: Number of features per image 42 | ORBextractor.nFeatures: 1000 43 | 44 | # ORB Extractor: Scale factor between levels in the scale pyramid 45 | ORBextractor.scaleFactor: 1.2 46 | 47 | # ORB Extractor: Number of levels in the scale pyramid 48 | ORBextractor.nLevels: 8 49 | 50 | # ORB Extractor: Fast threshold 51 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 52 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 53 | # You can lower these values if your images have low contrast 54 | ORBextractor.iniThFAST: 20 55 | ORBextractor.minThFAST: 7 56 | 57 | #-------------------------------------------------------------------------------------------- 58 | # Viewer Parameters 59 | #-------------------------------------------------------------------------------------------- 60 | Viewer.KeyFrameSize: 0.05 61 | Viewer.KeyFrameLineWidth: 1 62 | Viewer.GraphLineWidth: 0.9 63 | Viewer.PointSize:2 64 | Viewer.CameraSize: 0.08 65 | Viewer.CameraLineWidth: 3 66 | Viewer.ViewpointX: 0 67 | Viewer.ViewpointY: -0.7 68 | Viewer.ViewpointZ: -1.8 69 | Viewer.ViewpointF: 500 70 | 71 | -------------------------------------------------------------------------------- /code/Examples/RGB-D/TUM2.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 520.908620 9 | Camera.fy: 521.007327 10 | Camera.cx: 325.141442 11 | Camera.cy: 249.701764 12 | 13 | Camera.k1: 0.231222 14 | Camera.k2: -0.784899 15 | Camera.p1: -0.003257 16 | Camera.p2: -0.000105 17 | Camera.k3: 0.917205 18 | 19 | Camera.width: 640 20 | Camera.height: 480 21 | 22 | # Camera frames per second 23 | Camera.fps: 30.0 24 | 25 | # IR projector baseline times fx (aprox.) 26 | Camera.bf: 40.0 27 | 28 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 29 | Camera.RGB: 1 30 | 31 | # Close/Far threshold. Baseline times. 32 | ThDepth: 40.0 33 | 34 | # Deptmap values factor 35 | DepthMapFactor: 5208.0 36 | 37 | #-------------------------------------------------------------------------------------------- 38 | # ORB Parameters 39 | #-------------------------------------------------------------------------------------------- 40 | 41 | # ORB Extractor: Number of features per image 42 | ORBextractor.nFeatures: 1000 43 | 44 | # ORB Extractor: Scale factor between levels in the scale pyramid 45 | ORBextractor.scaleFactor: 1.2 46 | 47 | # ORB Extractor: Number of levels in the scale pyramid 48 | ORBextractor.nLevels: 8 49 | 50 | # ORB Extractor: Fast threshold 51 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 52 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 53 | # You can lower these values if your images have low contrast 54 | ORBextractor.iniThFAST: 20 55 | ORBextractor.minThFAST: 7 56 | 57 | #-------------------------------------------------------------------------------------------- 58 | # Viewer Parameters 59 | #-------------------------------------------------------------------------------------------- 60 | Viewer.KeyFrameSize: 0.05 61 | Viewer.KeyFrameLineWidth: 1 62 | Viewer.GraphLineWidth: 0.9 63 | Viewer.PointSize:2 64 | Viewer.CameraSize: 0.08 65 | Viewer.CameraLineWidth: 3 66 | Viewer.ViewpointX: 0 67 | Viewer.ViewpointY: -0.7 68 | Viewer.ViewpointZ: -1.8 69 | Viewer.ViewpointF: 500 70 | 71 | -------------------------------------------------------------------------------- /code/Examples/RGB-D/TUM3.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 535.4 9 | Camera.fy: 539.2 10 | Camera.cx: 320.1 11 | Camera.cy: 247.6 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 640 19 | Camera.height: 480 20 | 21 | # Camera frames per second 22 | Camera.fps: 30.0 23 | 24 | # IR projector baseline times fx (aprox.) 25 | Camera.bf: 40.0 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 40.0 32 | 33 | # Deptmap values factor 34 | DepthMapFactor: 5000.0 35 | 36 | #-------------------------------------------------------------------------------------------- 37 | # ORB Parameters 38 | #-------------------------------------------------------------------------------------------- 39 | 40 | # ORB Extractor: Number of features per image 41 | ORBextractor.nFeatures: 1000 42 | 43 | # ORB Extractor: Scale factor between levels in the scale pyramid 44 | ORBextractor.scaleFactor: 1.2 45 | 46 | # ORB Extractor: Number of levels in the scale pyramid 47 | ORBextractor.nLevels: 8 48 | 49 | # ORB Extractor: Fast threshold 50 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 51 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 52 | # You can lower these values if your images have low contrast 53 | ORBextractor.iniThFAST: 20 54 | ORBextractor.minThFAST: 7 55 | 56 | #-------------------------------------------------------------------------------------------- 57 | # Viewer Parameters 58 | #-------------------------------------------------------------------------------------------- 59 | Viewer.KeyFrameSize: 0.05 60 | Viewer.KeyFrameLineWidth: 1 61 | Viewer.GraphLineWidth: 0.9 62 | Viewer.PointSize:2 63 | Viewer.CameraSize: 0.08 64 | Viewer.CameraLineWidth: 3 65 | Viewer.ViewpointX: 0 66 | Viewer.ViewpointY: -0.7 67 | Viewer.ViewpointZ: -1.8 68 | Viewer.ViewpointF: 500 69 | 70 | -------------------------------------------------------------------------------- /code/Examples/ROS/ORB_SLAM2/Asus.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 535.4 9 | Camera.fy: 539.2 10 | Camera.cx: 320.1 11 | Camera.cy: 247.6 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 640 19 | Camera.height: 480 20 | 21 | # Camera frames per second 22 | Camera.fps: 30.0 23 | 24 | # IR projector baseline times fx (aprox.) 25 | Camera.bf: 40.0 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 40.0 32 | 33 | # Deptmap values factor 34 | DepthMapFactor: 1.0 35 | 36 | #-------------------------------------------------------------------------------------------- 37 | # ORB Parameters 38 | #-------------------------------------------------------------------------------------------- 39 | 40 | # ORB Extractor: Number of features per image 41 | ORBextractor.nFeatures: 1000 42 | 43 | # ORB Extractor: Scale factor between levels in the scale pyramid 44 | ORBextractor.scaleFactor: 1.2 45 | 46 | # ORB Extractor: Number of levels in the scale pyramid 47 | ORBextractor.nLevels: 8 48 | 49 | # ORB Extractor: Fast threshold 50 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 51 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 52 | # You can lower these values if your images have low contrast 53 | ORBextractor.iniThFAST: 20 54 | ORBextractor.minThFAST: 7 55 | 56 | #-------------------------------------------------------------------------------------------- 57 | # Viewer Parameters 58 | #-------------------------------------------------------------------------------------------- 59 | Viewer.KeyFrameSize: 0.05 60 | Viewer.KeyFrameLineWidth: 1 61 | Viewer.GraphLineWidth: 0.9 62 | Viewer.PointSize:2 63 | Viewer.CameraSize: 0.08 64 | Viewer.CameraLineWidth: 3 65 | Viewer.ViewpointX: 0 66 | Viewer.ViewpointY: -0.7 67 | Viewer.ViewpointZ: -1.8 68 | Viewer.ViewpointF: 500 69 | 70 | -------------------------------------------------------------------------------- /code/Examples/ROS/ORB_SLAM2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | rosbuild_init() 5 | 6 | IF(NOT ROS_BUILD_TYPE) 7 | SET(ROS_BUILD_TYPE Release) 8 | ENDIF() 9 | 10 | MESSAGE("Build type: " ${ROS_BUILD_TYPE}) 11 | 12 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 13 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 14 | 15 | # Check C++11 or C++0x support 16 | include(CheckCXXCompilerFlag) 17 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 18 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 19 | if(COMPILER_SUPPORTS_CXX11) 20 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 21 | add_definitions(-DCOMPILEDWITHC11) 22 | message(STATUS "Using flag -std=c++11.") 23 | elseif(COMPILER_SUPPORTS_CXX0X) 24 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 25 | add_definitions(-DCOMPILEDWITHC0X) 26 | message(STATUS "Using flag -std=c++0x.") 27 | else() 28 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 29 | endif() 30 | 31 | LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../../../cmake_modules) 32 | 33 | find_package(OpenCV 3.0 QUIET) 34 | if(NOT OpenCV_FOUND) 35 | find_package(OpenCV 2.4.3 QUIET) 36 | if(NOT OpenCV_FOUND) 37 | message(FATAL_ERROR "OpenCV > 2.4.3 not found.") 38 | endif() 39 | endif() 40 | 41 | find_package(Eigen3 3.1.0 REQUIRED) 42 | find_package(Pangolin REQUIRED) 43 | 44 | include_directories( 45 | ${PROJECT_SOURCE_DIR} 46 | ${PROJECT_SOURCE_DIR}/../../../ 47 | ${PROJECT_SOURCE_DIR}/../../../include 48 | ${Pangolin_INCLUDE_DIRS} 49 | ) 50 | 51 | set(LIBS 52 | ${OpenCV_LIBS} 53 | ${EIGEN3_LIBS} 54 | ${Pangolin_LIBRARIES} 55 | ${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so 56 | ${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so 57 | ${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM2.so 58 | ) 59 | 60 | # Node for monocular camera 61 | rosbuild_add_executable(Mono 62 | src/ros_mono.cc 63 | ) 64 | 65 | target_link_libraries(Mono 66 | ${LIBS} 67 | ) 68 | 69 | # Node for monocular camera (Augmented Reality Demo) 70 | rosbuild_add_executable(MonoAR 71 | src/AR/ros_mono_ar.cc 72 | src/AR/ViewerAR.h 73 | src/AR/ViewerAR.cc 74 | ) 75 | 76 | target_link_libraries(MonoAR 77 | ${LIBS} 78 | ) 79 | 80 | # Node for stereo camera 81 | rosbuild_add_executable(Stereo 82 | src/ros_stereo.cc 83 | ) 84 | 85 | target_link_libraries(Stereo 86 | ${LIBS} 87 | ) 88 | 89 | # Node for RGB-D camera 90 | rosbuild_add_executable(RGBD 91 | src/ros_rgbd.cc 92 | ) 93 | 94 | target_link_libraries(RGBD 95 | ${LIBS} 96 | ) 97 | 98 | -------------------------------------------------------------------------------- /code/Examples/ROS/ORB_SLAM2/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ORB_SLAM2 5 | 6 | 7 | Raul Mur-Artal 8 | GPLv3 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /code/Examples/ROS/ORB_SLAM2/src/ros_mono.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | #include 31 | 32 | #include"../../../include/System.h" 33 | 34 | using namespace std; 35 | 36 | class ImageGrabber 37 | { 38 | public: 39 | ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){} 40 | 41 | void GrabImage(const sensor_msgs::ImageConstPtr& msg); 42 | 43 | ORB_SLAM2::System* mpSLAM; 44 | }; 45 | 46 | int main(int argc, char **argv) 47 | { 48 | ros::init(argc, argv, "Mono"); 49 | ros::start(); 50 | 51 | if(argc != 3) 52 | { 53 | cerr << endl << "Usage: rosrun ORB_SLAM2 Mono path_to_vocabulary path_to_settings" << endl; 54 | ros::shutdown(); 55 | return 1; 56 | } 57 | 58 | // Create SLAM system. It initializes all system threads and gets ready to process frames. 59 | ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true); 60 | 61 | ImageGrabber igb(&SLAM); 62 | 63 | ros::NodeHandle nodeHandler; 64 | ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb); 65 | 66 | ros::spin(); 67 | 68 | // Stop all threads 69 | SLAM.Shutdown(); 70 | 71 | // Save camera trajectory 72 | SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); 73 | 74 | ros::shutdown(); 75 | 76 | return 0; 77 | } 78 | 79 | void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg) 80 | { 81 | // Copy the ros image message to cv::Mat. 82 | cv_bridge::CvImageConstPtr cv_ptr; 83 | try 84 | { 85 | cv_ptr = cv_bridge::toCvShare(msg); 86 | } 87 | catch (cv_bridge::Exception& e) 88 | { 89 | ROS_ERROR("cv_bridge exception: %s", e.what()); 90 | return; 91 | } 92 | 93 | mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec()); 94 | } 95 | 96 | 97 | -------------------------------------------------------------------------------- /code/Examples/Stereo/KITTI00-02.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 718.856 9 | Camera.fy: 718.856 10 | Camera.cx: 607.1928 11 | Camera.cy: 185.2157 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 1241 19 | Camera.height: 376 20 | 21 | # Camera frames per second 22 | Camera.fps: 10.0 23 | 24 | # stereo baseline times fx 25 | Camera.bf: 386.1448 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 35 32 | 33 | #-------------------------------------------------------------------------------------------- 34 | # ORB Parameters 35 | #-------------------------------------------------------------------------------------------- 36 | 37 | # ORB Extractor: Number of features per image 38 | ORBextractor.nFeatures: 2000 39 | 40 | # ORB Extractor: Scale factor between levels in the scale pyramid 41 | ORBextractor.scaleFactor: 1.2 42 | 43 | # ORB Extractor: Number of levels in the scale pyramid 44 | ORBextractor.nLevels: 8 45 | 46 | # ORB Extractor: Fast threshold 47 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 48 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 49 | # You can lower these values if your images have low contrast 50 | ORBextractor.iniThFAST: 20 51 | ORBextractor.minThFAST: 7 52 | 53 | #-------------------------------------------------------------------------------------------- 54 | # Viewer Parameters 55 | #-------------------------------------------------------------------------------------------- 56 | Viewer.KeyFrameSize: 0.6 57 | Viewer.KeyFrameLineWidth: 2 58 | Viewer.GraphLineWidth: 1 59 | Viewer.PointSize:2 60 | Viewer.CameraSize: 0.7 61 | Viewer.CameraLineWidth: 3 62 | Viewer.ViewpointX: 0 63 | Viewer.ViewpointY: -100 64 | Viewer.ViewpointZ: -0.1 65 | Viewer.ViewpointF: 2000 66 | 67 | -------------------------------------------------------------------------------- /code/Examples/Stereo/KITTI03.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 721.5377 9 | Camera.fy: 721.5377 10 | Camera.cx: 609.5593 11 | Camera.cy: 172.854 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 1241 19 | Camera.height: 376 20 | 21 | # Camera frames per second 22 | Camera.fps: 10.0 23 | 24 | # stereo baseline times fx 25 | Camera.bf: 387.5744 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 40 32 | 33 | #-------------------------------------------------------------------------------------------- 34 | # ORB Parameters 35 | #-------------------------------------------------------------------------------------------- 36 | 37 | # ORB Extractor: Number of features per image 38 | ORBextractor.nFeatures: 2000 39 | 40 | # ORB Extractor: Scale factor between levels in the scale pyramid 41 | ORBextractor.scaleFactor: 1.2 42 | 43 | # ORB Extractor: Number of levels in the scale pyramid 44 | ORBextractor.nLevels: 8 45 | 46 | # ORB Extractor: Fast threshold 47 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 48 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 49 | # You can lower these values if your images have low contrast 50 | ORBextractor.iniThFAST: 20 51 | ORBextractor.minThFAST: 7 52 | 53 | #-------------------------------------------------------------------------------------------- 54 | # Viewer Parameters 55 | #-------------------------------------------------------------------------------------------- 56 | Viewer.KeyFrameSize: 0.6 57 | Viewer.KeyFrameLineWidth: 2 58 | Viewer.GraphLineWidth: 1 59 | Viewer.PointSize:2 60 | Viewer.CameraSize: 0.7 61 | Viewer.CameraLineWidth: 3 62 | Viewer.ViewpointX: 0 63 | Viewer.ViewpointY: -100 64 | Viewer.ViewpointZ: -0.1 65 | Viewer.ViewpointF: 2000 66 | 67 | -------------------------------------------------------------------------------- /code/Examples/Stereo/KITTI04-12.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 707.0912 9 | Camera.fy: 707.0912 10 | Camera.cx: 601.8873 11 | Camera.cy: 183.1104 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 1241 19 | Camera.height: 376 20 | 21 | # Camera frames per second 22 | Camera.fps: 10.0 23 | 24 | # stereo baseline times fx 25 | Camera.bf: 379.8145 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 40 32 | 33 | #-------------------------------------------------------------------------------------------- 34 | # ORB Parameters 35 | #-------------------------------------------------------------------------------------------- 36 | 37 | # ORB Extractor: Number of features per image 38 | ORBextractor.nFeatures: 2000 39 | 40 | # ORB Extractor: Scale factor between levels in the scale pyramid 41 | ORBextractor.scaleFactor: 1.2 42 | 43 | # ORB Extractor: Number of levels in the scale pyramid 44 | ORBextractor.nLevels: 8 45 | 46 | # ORB Extractor: Fast threshold 47 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 48 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 49 | # You can lower these values if your images have low contrast 50 | ORBextractor.iniThFAST: 12 51 | ORBextractor.minThFAST: 7 52 | 53 | #-------------------------------------------------------------------------------------------- 54 | # Viewer Parameters 55 | #-------------------------------------------------------------------------------------------- 56 | Viewer.KeyFrameSize: 0.6 57 | Viewer.KeyFrameLineWidth: 2 58 | Viewer.GraphLineWidth: 1 59 | Viewer.PointSize:2 60 | Viewer.CameraSize: 0.7 61 | Viewer.CameraLineWidth: 3 62 | Viewer.ViewpointX: 0 63 | Viewer.ViewpointY: -100 64 | Viewer.ViewpointZ: -0.1 65 | Viewer.ViewpointF: 2000 66 | 67 | -------------------------------------------------------------------------------- /code/LICENSE.txt: -------------------------------------------------------------------------------- 1 | ORB-SLAM2 is released under a GPLv3 license (see License-gpl.txt). 2 | Please see Dependencies.md for a list of all included code and library dependencies which are not property of the authors of ORB-SLAM2. 3 | 4 | For a closed-source version of ORB-SLAM2 for commercial purposes, please contact the authors. 5 | 6 | If you use ORB-SLAM in an academic work, please cite the most relevant publication associated by visiting: 7 | https://github.com/raulmur/ORB_SLAM2 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /code/Thirdparty/DBoW2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(DBoW2) 3 | 4 | set (CMAKE_CXX_STANDARD 11) 5 | 6 | # Set optimized building: 7 | if(MSVC_IDE) 8 | add_definitions(-D_CRT_SECURE_NO_WARNINGS) 9 | else(MSVC_IDE) 10 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 11 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 12 | endif(MSVC_IDE) 13 | 14 | set(HDRS_DBOW2 15 | DBoW2/BowVector.h 16 | DBoW2/FORB.h 17 | DBoW2/FClass.h 18 | DBoW2/FeatureVector.h 19 | DBoW2/ScoringObject.h 20 | DBoW2/TemplatedVocabulary.h) 21 | set(SRCS_DBOW2 22 | DBoW2/BowVector.cpp 23 | DBoW2/FORB.cpp 24 | DBoW2/FeatureVector.cpp 25 | DBoW2/ScoringObject.cpp) 26 | 27 | set(HDRS_DUTILS 28 | DUtils/Random.h 29 | DUtils/Timestamp.h) 30 | set(SRCS_DUTILS 31 | DUtils/Random.cpp 32 | DUtils/Timestamp.cpp) 33 | 34 | #find_package(OpenCV REQUIRED) 35 | find_package(OpenCV 3.1.0 QUIET) 36 | 37 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 38 | 39 | include_directories(${OpenCV_INCLUDE_DIRS}) 40 | add_library(DBoW2 SHARED ${SRCS_DBOW2} ${SRCS_DUTILS}) 41 | target_link_libraries(DBoW2 ${OpenCV_LIBS}) 42 | 43 | -------------------------------------------------------------------------------- /code/Thirdparty/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 | -------------------------------------------------------------------------------- /code/Thirdparty/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 | #include "../DUtils/config.h" 18 | 19 | namespace DBoW2 { 20 | 21 | /// Id of words 22 | typedef unsigned int WordId; 23 | 24 | /// Value of a word 25 | typedef double WordValue; 26 | 27 | /// Id of nodes in the vocabulary treee 28 | typedef unsigned int NodeId; 29 | 30 | /// L-norms for normalization 31 | EXPORT typedef enum LNorm 32 | { 33 | L1, 34 | L2 35 | } LNorm; 36 | 37 | /// Weighting type 38 | EXPORT typedef enum WeightingType 39 | { 40 | TF_IDF, 41 | TF, 42 | IDF, 43 | BINARY 44 | } WeightingType; 45 | 46 | /// Scoring type 47 | EXPORT typedef enum ScoringType 48 | { 49 | L1_NORM, 50 | L2_NORM, 51 | CHI_SQUARE, 52 | KL, 53 | BHATTACHARYYA, 54 | DOT_PRODUCT, 55 | } ScoringType; 56 | 57 | /// Vector of words to represent images 58 | /// stl的map结构，key为wordId，value为tfidf中的tf 59 | class EXPORT BowVector: 60 | public std::map 61 | { 62 | public: 63 | 64 | /** 65 | * Constructor 66 | */ 67 | BowVector(void); 68 | 69 | /** 70 | * Destructor 71 | */ 72 | ~BowVector(void); 73 | 74 | /** 75 | * Adds a value to a word value existing in the vector, or creates a new 76 | * word with the given value 77 | * @param id word id to look for 78 | * @param v value to create the word with, or to add to existing word 79 | */ 80 | void addWeight(WordId id, WordValue v); 81 | 82 | /** 83 | * Adds a word with a value to the vector only if this does not exist yet 84 | * @param id word id to look for 85 | * @param v value to give to the word if this does not exist 86 | */ 87 | void addIfNotExist(WordId id, WordValue v); 88 | 89 | /** 90 | * L1-Normalizes the values in the vector 91 | * @param norm_type norm used 92 | */ 93 | void normalize(LNorm norm_type); 94 | 95 | /** 96 | * Prints the content of the bow vector 97 | * @param out stream 98 | * @param v 99 | */ 100 | friend std::ostream& operator<<(std::ostream &out, const BowVector &v); 101 | 102 | /** 103 | * Saves the bow vector as a vector in a matlab file 104 | * @param filename 105 | * @param W number of words in the vocabulary 106 | */ 107 | void saveM(const std::string &filename, size_t W) const; 108 | }; 109 | 110 | } // namespace DBoW2 111 | 112 | #endif 113 | -------------------------------------------------------------------------------- /code/Thirdparty/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 | #include "../DUtils/config.h" 18 | 19 | namespace DBoW2 { 20 | 21 | /// Generic class to encapsulate functions to manage descriptors. 22 | /** 23 | * This class must be inherited. Derived classes can be used as the 24 | * parameter F when creating Templated structures 25 | * (TemplatedVocabulary, TemplatedDatabase, ...) 26 | */ 27 | class EXPORT FClass 28 | { 29 | class TDescriptor; 30 | typedef const TDescriptor *pDescriptor; 31 | 32 | /** 33 | * Calculates the mean value of a set of descriptors 34 | * @param descriptors 35 | * @param mean mean descriptor 36 | */ 37 | virtual void meanValue(const std::vector &descriptors, 38 | TDescriptor &mean) = 0; 39 | 40 | /** 41 | * Calculates the distance between two descriptors 42 | * @param a 43 | * @param b 44 | * @return distance 45 | */ 46 | static double distance(const TDescriptor &a, const TDescriptor &b); 47 | 48 | /** 49 | * Returns a string version of the descriptor 50 | * @param a descriptor 51 | * @return string version 52 | */ 53 | static std::string toString(const TDescriptor &a); 54 | 55 | /** 56 | * Returns a descriptor from a string 57 | * @param a descriptor 58 | * @param s string version 59 | */ 60 | static void fromString(TDescriptor &a, const std::string &s); 61 | 62 | /** 63 | * Returns a mat with the descriptors in float format 64 | * @param descriptors 65 | * @param mat (out) NxL 32F matrix 66 | */ 67 | static void toMat32F(const std::vector &descriptors, 68 | cv::Mat &mat); 69 | }; 70 | 71 | } // namespace DBoW2 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /code/Thirdparty/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 | #include "../DUtils/config.h" 20 | 21 | namespace DBoW2 { 22 | 23 | /// Functions to manipulate ORB descriptors 24 | class EXPORT FORB : protected FClass 25 | { 26 | public: 27 | 28 | /// Descriptor type 29 | typedef cv::Mat TDescriptor; // CV_8U 30 | /// Pointer to a single descriptor 31 | typedef const TDescriptor *pDescriptor; 32 | /// Descriptor length (in bytes) 33 | static const int L = 32; 34 | 35 | /** 36 | * Calculates the mean value of a set of descriptors 37 | * @param descriptors 38 | * @param mean mean descriptor 39 | */ 40 | static void meanValue(const std::vector &descriptors, 41 | TDescriptor &mean); 42 | 43 | /** 44 | * Calculates the distance between two descriptors 45 | * @param a 46 | * @param b 47 | * @return distance 48 | */ 49 | static int distance(const TDescriptor &a, const TDescriptor &b); 50 | 51 | /** 52 | * Returns a string version of the descriptor 53 | * @param a descriptor 54 | * @return string version 55 | */ 56 | static std::string toString(const TDescriptor &a); 57 | 58 | /** 59 | * Returns a descriptor from a string 60 | * @param a descriptor 61 | * @param s string version 62 | */ 63 | static void fromString(TDescriptor &a, const std::string &s); 64 | 65 | /** 66 | * Returns a mat with the descriptors in float format 67 | * @param descriptors 68 | * @param mat (out) NxL 32F matrix 69 | */ 70 | static void toMat32F(const std::vector &descriptors, 71 | cv::Mat &mat); 72 | 73 | static void toMat8U(const std::vector &descriptors, 74 | cv::Mat &mat); 75 | 76 | }; 77 | 78 | } // namespace DBoW2 79 | 80 | #endif 81 | 82 | -------------------------------------------------------------------------------- /code/Thirdparty/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 | -------------------------------------------------------------------------------- /code/Thirdparty/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 | #include "../DUtils/config.h" 19 | 20 | namespace DBoW2 { 21 | 22 | /// Vector of nodes with indexes of local features 23 | class EXPORT FeatureVector: 24 | public std::map > 25 | { 26 | public: 27 | 28 | /** 29 | * Constructor 30 | */ 31 | FeatureVector(void); 32 | 33 | /** 34 | * Destructor 35 | */ 36 | ~FeatureVector(void); 37 | 38 | /** 39 | * Adds a feature to an existing node, or adds a new node with an initial 40 | * feature 41 | * @param id node id to add or to modify 42 | * @param i_feature index of feature to add to the given node 43 | */ 44 | void addFeature(NodeId id, unsigned int i_feature); 45 | 46 | /** 47 | * Sends a string versions of the feature vector through the stream 48 | * @param out stream 49 | * @param v feature vector 50 | */ 51 | friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v); 52 | 53 | }; 54 | 55 | } // namespace DBoW2 56 | 57 | #endif 58 | 59 | -------------------------------------------------------------------------------- /code/Thirdparty/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 | #include "../DUtils/config.h" 16 | 17 | namespace DBoW2 { 18 | 19 | /// Base class of scoring functions 20 | class EXPORT GeneralScoring 21 | { 22 | public: 23 | /** 24 | * Computes the score between two vectors. Vectors must be sorted and 25 | * normalized if necessary 26 | * @param v (in/out) 27 | * @param w (in/out) 28 | * @return score 29 | */ 30 | virtual double score(const BowVector &v, const BowVector &w) const = 0; 31 | 32 | /** 33 | * Returns whether a vector must be normalized before scoring according 34 | * to the scoring scheme 35 | * @param norm norm to use 36 | * @return true iff must normalize 37 | */ 38 | virtual bool mustNormalize(LNorm &norm) const = 0; 39 | 40 | /// Log of epsilon 41 | static const double LOG_EPS; 42 | // If you change the type of WordValue, make sure you change also the 43 | // epsilon value (this is needed by the KL method) 44 | 45 | virtual ~GeneralScoring() {} //!< Required for virtual base classes 46 | 47 | }; 48 | 49 | /** 50 | * Macro for defining Scoring classes 51 | * @param NAME name of class 52 | * @param MUSTNORMALIZE if vectors must be normalized to compute the score 53 | * @param NORM type of norm to use when MUSTNORMALIZE 54 | */ 55 | #define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ 56 | NAME: public GeneralScoring \ 57 | { public: \ 58 | /** \ 59 | * Computes score between two vectors \ 60 | * @param v \ 61 | * @param w \ 62 | * @return score between v and w \ 63 | */ \ 64 | virtual double score(const BowVector &v, const BowVector &w) const; \ 65 | \ 66 | /** \ 67 | * Says if a vector must be normalized according to the scoring function \ 68 | * @param norm (out) if true, norm to use 69 | * @return true iff vectors must be normalized \ 70 | */ \ 71 | virtual inline bool mustNormalize(LNorm &norm) const \ 72 | { norm = NORM; return MUSTNORMALIZE; } \ 73 | } 74 | 75 | /// L1 Scoring object 76 | class EXPORT __SCORING_CLASS(L1Scoring, true, L1); 77 | 78 | /// L2 Scoring object 79 | class EXPORT __SCORING_CLASS(L2Scoring, true, L2); 80 | 81 | /// Chi square Scoring object 82 | class EXPORT __SCORING_CLASS(ChiSquareScoring, true, L1); 83 | 84 | /// KL divergence Scoring object 85 | class EXPORT __SCORING_CLASS(KLScoring, true, L1); 86 | 87 | /// Bhattacharyya Scoring object 88 | class EXPORT __SCORING_CLASS(BhattacharyyaScoring, true, L1); 89 | 90 | /// Dot product Scoring object 91 | class EXPORT __SCORING_CLASS(DotProductScoring, false, L1); 92 | 93 | #undef __SCORING_CLASS 94 | 95 | } // namespace DBoW2 96 | 97 | #endif 98 | 99 | -------------------------------------------------------------------------------- /code/Thirdparty/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 | -------------------------------------------------------------------------------- /code/Thirdparty/DBoW2/DUtils/config.h: -------------------------------------------------------------------------------- 1 | #ifndef CONFIG_H 2 | #define CONFIG_H 3 | 4 | #ifdef WIN32 5 | #ifndef DBoW2_EXPORTS 6 | #define DBoW2_EXPORTS 7 | #endif 8 | #ifdef DBoW2_EXPORTS 9 | #define EXPORT __declspec(dllexport) 10 | #else 11 | #define EXPORT __declspec(dllimport) 12 | #endif 13 | #else 14 | #define EXPORT 15 | #endif 16 | 17 | #endif // CONFIG_H 18 | -------------------------------------------------------------------------------- /code/Thirdparty/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 | -------------------------------------------------------------------------------- /code/Thirdparty/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 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/README.txt: -------------------------------------------------------------------------------- 1 | You should have received this g2o version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2). 2 | See the original g2o library at: https://github.com/RainerKuemmerle/g2o 3 | All files included in this g2o version are BSD, see license-bsd.txt 4 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/cmake_modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | # specific additional paths for some OS 65 | if (WIN32) 66 | set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include") 67 | endif(WIN32) 68 | 69 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 70 | PATHS 71 | ${CMAKE_INSTALL_PREFIX}/include 72 | ${EIGEN_ADDITIONAL_SEARCH_PATHS} 73 | ${KDE4_INCLUDE_DIR} 74 | PATH_SUFFIXES eigen3 eigen 75 | ) 76 | 77 | if(EIGEN3_INCLUDE_DIR) 78 | _eigen3_check_version() 79 | endif(EIGEN3_INCLUDE_DIR) 80 | 81 | include(FindPackageHandleStandardArgs) 82 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 83 | 84 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 85 | 86 | endif(EIGEN3_INCLUDE_DIR) 87 | 88 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/config.h.in: -------------------------------------------------------------------------------- 1 | #ifndef G2O_CONFIG_H 2 | #define G2O_CONFIG_H 3 | 4 | #cmakedefine G2O_OPENMP 1 5 | #cmakedefine G2O_SHARED_LIBS 1 6 | 7 | // give a warning if Eigen defaults to row-major matrices. 8 | // We internally assume column-major matrices throughout the code. 9 | #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR 10 | # error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)" 11 | #endif 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/core/base_vertex.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | template 28 | BaseVertex::BaseVertex() : 29 | OptimizableGraph::Vertex(), 30 | _hessian(0, D, D) 31 | { 32 | _dimension = D; 33 | } 34 | 35 | template 36 | double BaseVertex::solveDirect(double lambda) { 37 | Matrix tempA=_hessian + Matrix ::Identity()*lambda; 38 | double det=tempA.determinant(); 39 | if (g2o_isnan(det) || det < std::numeric_limits::epsilon()) 40 | return det; 41 | Matrix dx=tempA.llt().solve(_b); 42 | oplus(&dx[0]); 43 | return det; 44 | } 45 | 46 | template 47 | void BaseVertex::clearQuadraticForm() { 48 | _b.setZero(); 49 | } 50 | 51 | template 52 | void BaseVertex::mapHessianMemory(double* d) 53 | { 54 | new (&_hessian) HessianBlockType(d, D, D); 55 | } 56 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/core/batch_stats.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "batch_stats.h" 28 | #include 29 | 30 | namespace g2o { 31 | using namespace std; 32 | 33 | G2OBatchStatistics* G2OBatchStatistics::_globalStats=0; 34 | 35 | #ifndef PTHING 36 | #define PTHING(s) \ 37 | #s << "= " << (st.s) << "\t " 38 | #endif 39 | 40 | G2OBatchStatistics::G2OBatchStatistics(){ 41 | // zero all. 42 | memset (this, 0, sizeof(G2OBatchStatistics)); 43 | 44 | // set the iteration to -1 to show that it isn't valid 45 | iteration = -1; 46 | } 47 | 48 | std::ostream& operator << (std::ostream& os , const G2OBatchStatistics& st) 49 | { 50 | os << PTHING(iteration); 51 | 52 | os << PTHING( numVertices ); // how many vertices are involved 53 | os << PTHING( numEdges ); // hoe many edges 54 | os << PTHING( chi2 ); // total chi2 55 | 56 | /** timings **/ 57 | // nonlinear part 58 | os << PTHING( timeResiduals ); 59 | os << PTHING( timeLinearize ); // jacobians 60 | os << PTHING( timeQuadraticForm ); // construct the quadratic form in the graph 61 | 62 | // block_solver (constructs Ax=b, plus maybe schur); 63 | os << PTHING( timeSchurComplement ); // compute schur complement (0 if not done); 64 | 65 | // linear solver (computes Ax=b); ); 66 | os << PTHING( timeSymbolicDecomposition ); // symbolic decomposition (0 if not done); 67 | os << PTHING( timeNumericDecomposition ); // numeric decomposition (0 if not done); 68 | os << PTHING( timeLinearSolution ); // total time for solving Ax=b 69 | os << PTHING( iterationsLinearSolver ); // iterations of PCG 70 | os << PTHING( timeUpdate ); // oplus 71 | os << PTHING( timeIteration ); // total time ); 72 | 73 | os << PTHING( levenbergIterations ); 74 | os << PTHING( timeLinearSolver); 75 | 76 | os << PTHING(hessianDimension); 77 | os << PTHING(hessianPoseDimension); 78 | os << PTHING(hessianLandmarkDimension); 79 | os << PTHING(choleskyNNZ); 80 | os << PTHING(timeMarginals); 81 | 82 | return os; 83 | }; 84 | 85 | void G2OBatchStatistics::setGlobalStats(G2OBatchStatistics* b) 86 | { 87 | _globalStats = b; 88 | } 89 | 90 | } // end namespace 91 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/core/creators.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_CREATORS_H 28 | #define G2O_CREATORS_H 29 | 30 | #include "hyper_graph.h" 31 | 32 | #include 33 | #include 34 | 35 | namespace g2o 36 | { 37 | 38 | /** 39 | * \brief Abstract interface for allocating HyperGraphElement 40 | */ 41 | class AbstractHyperGraphElementCreator 42 | { 43 | public: 44 | /** 45 | * create a hyper graph element. Has to implemented in derived class. 46 | */ 47 | virtual HyperGraph::HyperGraphElement* construct() = 0; 48 | /** 49 | * name of the class to be created. Has to implemented in derived class. 50 | */ 51 | virtual const std::string& name() const = 0; 52 | 53 | virtual ~AbstractHyperGraphElementCreator() { } 54 | }; 55 | 56 | /** 57 | * \brief templatized creator class which creates graph elements 58 | */ 59 | template 60 | class HyperGraphElementCreator : public AbstractHyperGraphElementCreator 61 | { 62 | public: 63 | HyperGraphElementCreator() : _name(typeid(T).name()) {} 64 | #if defined (WINDOWS) && defined(__GNUC__) // force stack alignment on Windows with GCC 65 | __attribute__((force_align_arg_pointer)) 66 | #endif 67 | HyperGraph::HyperGraphElement* construct() { return new T;} 68 | virtual const std::string& name() const { return _name;} 69 | protected: 70 | std::string _name; 71 | }; 72 | 73 | } // end namespace 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/core/jacobian_workspace.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "jacobian_workspace.h" 28 | 29 | #include 30 | 31 | #include "optimizable_graph.h" 32 | 33 | using namespace std; 34 | 35 | namespace g2o { 36 | 37 | JacobianWorkspace::JacobianWorkspace() : 38 | _maxNumVertices(-1), _maxDimension(-1) 39 | { 40 | } 41 | 42 | JacobianWorkspace::~JacobianWorkspace() 43 | { 44 | } 45 | 46 | bool JacobianWorkspace::allocate() 47 | { 48 | //cerr << __PRETTY_FUNCTION__ << " " << PVAR(this) << " " << PVAR(_maxNumVertices) << " " << PVAR(_maxDimension) << endl; 49 | if (_maxNumVertices <=0 || _maxDimension <= 0) 50 | return false; 51 | _workspace.resize(_maxNumVertices); 52 | for (WorkspaceVector::iterator it = _workspace.begin(); it != _workspace.end(); ++it) { 53 | it->resize(_maxDimension); 54 | it->setZero(); 55 | } 56 | return true; 57 | } 58 | 59 | void JacobianWorkspace::updateSize(const HyperGraph::Edge* e_) 60 | { 61 | const OptimizableGraph::Edge* e = static_cast(e_); 62 | int errorDimension = e->dimension(); 63 | int numVertices = e->vertices().size(); 64 | int maxDimensionForEdge = -1; 65 | for (int i = 0; i < numVertices; ++i) { 66 | const OptimizableGraph::Vertex* v = static_cast(e->vertex(i)); 67 | assert(v && "Edge has no vertex assigned"); 68 | maxDimensionForEdge = max(v->dimension() * errorDimension, maxDimensionForEdge); 69 | } 70 | _maxNumVertices = max(numVertices, _maxNumVertices); 71 | _maxDimension = max(maxDimensionForEdge, _maxDimension); 72 | //cerr << __PRETTY_FUNCTION__ << " " << PVAR(this) << " " << PVAR(_maxNumVertices) << " " << PVAR(_maxDimension) << endl; 73 | } 74 | 75 | void JacobianWorkspace::updateSize(const OptimizableGraph& graph) 76 | { 77 | for (OptimizableGraph::EdgeSet::const_iterator it = graph.edges().begin(); it != graph.edges().end(); ++it) { 78 | const OptimizableGraph::Edge* e = static_cast(*it); 79 | updateSize(e); 80 | } 81 | } 82 | 83 | void JacobianWorkspace::updateSize(int numVertices, int dimension) 84 | { 85 | _maxNumVertices = max(numVertices, _maxNumVertices); 86 | _maxDimension = max(dimension, _maxDimension); 87 | } 88 | 89 | } // end namespace 90 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/core/jacobian_workspace.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef JACOBIAN_WORKSPACE_H 28 | #define JACOBIAN_WORKSPACE_H 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | #include "hyper_graph.h" 37 | 38 | namespace g2o { 39 | 40 | struct OptimizableGraph; 41 | 42 | /** 43 | * \brief provide memory workspace for computing the Jacobians 44 | * 45 | * The workspace is used by an OptimizableGraph to provide temporary memory 46 | * for computing the Jacobian of the error functions. 47 | * Before calling linearizeOplus on an edge, the workspace needs to be allocated 48 | * by calling allocate(). 49 | */ 50 | class JacobianWorkspace 51 | { 52 | public: 53 | typedef std::vector > WorkspaceVector; 54 | 55 | public: 56 | JacobianWorkspace(); 57 | ~JacobianWorkspace(); 58 | 59 | /** 60 | * allocate the workspace 61 | */ 62 | bool allocate(); 63 | 64 | /** 65 | * update the maximum required workspace needed by taking into account this edge 66 | */ 67 | void updateSize(const HyperGraph::Edge* e); 68 | 69 | /** 70 | * update the required workspace by looking at a full graph 71 | */ 72 | void updateSize(const OptimizableGraph& graph); 73 | 74 | /** 75 | * manually update with the given parameters 76 | */ 77 | void updateSize(int numVertices, int dimension); 78 | 79 | /** 80 | * return the workspace for a vertex in an edge 81 | */ 82 | double* workspaceForVertex(int vertexIndex) 83 | { 84 | assert(vertexIndex >= 0 && (size_t)vertexIndex < _workspace.size() && "Index out of bounds"); 85 | return _workspace[vertexIndex].data(); 86 | } 87 | 88 | protected: 89 | WorkspaceVector _workspace; ///< the memory pre-allocated for computing the Jacobians 90 | int _maxNumVertices; ///< the maximum number of vertices connected by a hyper-edge 91 | int _maxDimension; ///< the maximum dimension (number of elements) for a Jacobian 92 | }; 93 | 94 | } // end namespace 95 | 96 | #endif 97 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/core/matrix_operations.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_CORE_MATRIX_OPERATIONS_H 28 | #define G2O_CORE_MATRIX_OPERATIONS_H 29 | 30 | #include 31 | 32 | namespace g2o { 33 | namespace internal { 34 | 35 | template 36 | inline void axpy(const MatrixType& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 37 | { 38 | y.segment(yoff) += A * x.segment(xoff); 39 | } 40 | 41 | template 42 | inline void axpy(const Eigen::Matrix& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 43 | { 44 | y.segment(yoff, A.rows()) += A * x.segment::ColsAtCompileTime>(xoff); 45 | } 46 | 47 | template<> 48 | inline void axpy(const Eigen::MatrixXd& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 49 | { 50 | y.segment(yoff, A.rows()) += A * x.segment(xoff, A.cols()); 51 | } 52 | 53 | template 54 | inline void atxpy(const MatrixType& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 55 | { 56 | y.segment(yoff) += A.transpose() * x.segment(xoff); 57 | } 58 | 59 | template 60 | inline void atxpy(const Eigen::Matrix& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 61 | { 62 | y.segment::ColsAtCompileTime>(yoff) += A.transpose() * x.segment(xoff, A.rows()); 63 | } 64 | 65 | template<> 66 | inline void atxpy(const Eigen::MatrixXd& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 67 | { 68 | y.segment(yoff, A.cols()) += A.transpose() * x.segment(xoff, A.rows()); 69 | } 70 | 71 | } // end namespace internal 72 | } // end namespace g2o 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/core/matrix_structure.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_MATRIX_STRUCTURE_H 28 | #define G2O_MATRIX_STRUCTURE_H 29 | 30 | 31 | namespace g2o { 32 | 33 | /** 34 | * \brief representing the structure of a matrix in column compressed structure (only the upper triangular part of the matrix) 35 | */ 36 | class MatrixStructure 37 | { 38 | public: 39 | MatrixStructure(); 40 | ~MatrixStructure(); 41 | /** 42 | * allocate space for the Matrix Structure. You may call this on an already allocated struct, it will 43 | * then reallocate the memory + additional space (double the required space). 44 | */ 45 | void alloc(int n_, int nz); 46 | 47 | void free(); 48 | 49 | /** 50 | * Write the matrix pattern to a file. File is also loadable by octave, e.g., then use spy(matrix) 51 | */ 52 | bool write(const char* filename) const; 53 | 54 | int n; ///< A is m-by-n. n must be >= 0. 55 | int m; ///< A is m-by-n. m must be >= 0. 56 | int* Ap; ///< column pointers for A, of size n+1 57 | int* Aii; ///< row indices of A, of size nz = Ap [n] 58 | 59 | //! max number of non-zeros blocks 60 | int nzMax() const { return maxNz;} 61 | 62 | protected: 63 | int maxN; ///< size of the allocated memory 64 | int maxNz; ///< size of the allocated memory 65 | }; 66 | 67 | } // end namespace 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/core/openmp_mutex.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPENMP_MUTEX 28 | #define G2O_OPENMP_MUTEX 29 | 30 | #include "../../config.h" 31 | 32 | #ifdef G2O_OPENMP 33 | #include 34 | #else 35 | #include 36 | #endif 37 | 38 | namespace g2o { 39 | 40 | #ifdef G2O_OPENMP 41 | 42 | /** 43 | * \brief Mutex realized via OpenMP 44 | */ 45 | class OpenMPMutex 46 | { 47 | public: 48 | OpenMPMutex() { omp_init_lock(&_lock); } 49 | ~OpenMPMutex() { omp_destroy_lock(&_lock); } 50 | void lock() { omp_set_lock(&_lock); } 51 | void unlock() { omp_unset_lock(&_lock); } 52 | protected: 53 | omp_lock_t _lock; 54 | }; 55 | 56 | #else 57 | 58 | /* 59 | * dummy which does nothing in case we don't have OpenMP support. 60 | * In debug mode, the mutex allows to verify the correct lock and unlock behavior 61 | */ 62 | class OpenMPMutex 63 | { 64 | public: 65 | #ifdef NDEBUG 66 | OpenMPMutex() {} 67 | #else 68 | OpenMPMutex() : _cnt(0) {} 69 | #endif 70 | ~OpenMPMutex() { assert(_cnt == 0 && "Freeing locked mutex");} 71 | void lock() { assert(++_cnt == 1 && "Locking already locked mutex");} 72 | void unlock() { assert(--_cnt == 0 && "Trying to unlock a mutex which is not locked");} 73 | protected: 74 | #ifndef NDEBUG 75 | char _cnt; 76 | #endif 77 | }; 78 | 79 | #endif 80 | 81 | /** 82 | * \brief lock a mutex within a scope 83 | */ 84 | class ScopedOpenMPMutex 85 | { 86 | public: 87 | explicit ScopedOpenMPMutex(OpenMPMutex* mutex) : _mutex(mutex) { _mutex->lock(); } 88 | ~ScopedOpenMPMutex() { _mutex->unlock(); } 89 | private: 90 | OpenMPMutex* const _mutex; 91 | ScopedOpenMPMutex(const ScopedOpenMPMutex&); 92 | void operator=(const ScopedOpenMPMutex&); 93 | }; 94 | 95 | } 96 | 97 | #endif 98 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/core/optimization_algorithm.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "optimization_algorithm.h" 28 | 29 | using namespace std; 30 | 31 | namespace g2o { 32 | 33 | OptimizationAlgorithm::OptimizationAlgorithm() : 34 | _optimizer(0) 35 | { 36 | } 37 | 38 | OptimizationAlgorithm::~OptimizationAlgorithm() 39 | { 40 | } 41 | 42 | void OptimizationAlgorithm::printProperties(std::ostream& os) const 43 | { 44 | os << "------------- Algorithm Properties -------------" << endl; 45 | for (PropertyMap::const_iterator it = _properties.begin(); it != _properties.end(); ++it) { 46 | BaseProperty* p = it->second; 47 | os << it->first << "\t" << p->toString() << endl; 48 | } 49 | os << "------------------------------------------------" << endl; 50 | } 51 | 52 | bool OptimizationAlgorithm::updatePropertiesFromString(const std::string& propString) 53 | { 54 | return _properties.updateMapFromString(propString); 55 | } 56 | 57 | void OptimizationAlgorithm::setOptimizer(SparseOptimizer* optimizer) 58 | { 59 | _optimizer = optimizer; 60 | } 61 | 62 | } // end namespace 63 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H 29 | 30 | #include "optimization_algorithm_with_hessian.h" 31 | 32 | namespace g2o { 33 | 34 | class BlockSolverBase; 35 | 36 | /** 37 | * \brief Implementation of Powell's Dogleg Algorithm 38 | */ 39 | class OptimizationAlgorithmDogleg : public OptimizationAlgorithmWithHessian 40 | { 41 | public: 42 | /** \brief type of the step to take */ 43 | enum { 44 | STEP_UNDEFINED, 45 | STEP_SD, STEP_GN, STEP_DL 46 | }; 47 | 48 | public: 49 | /** 50 | * construct the Dogleg algorithm, which will use the given Solver for solving the 51 | * linearized system. 52 | */ 53 | explicit OptimizationAlgorithmDogleg(BlockSolverBase* solver); 54 | virtual ~OptimizationAlgorithmDogleg(); 55 | 56 | virtual SolverResult solve(int iteration, bool online = false); 57 | 58 | virtual void printVerbose(std::ostream& os) const; 59 | 60 | //! return the type of the last step taken by the algorithm 61 | int lastStep() const { return _lastStep;} 62 | //! return the diameter of the trust region 63 | double trustRegion() const { return _delta;} 64 | 65 | //! convert the type into an integer 66 | static const char* stepType2Str(int stepType); 67 | 68 | protected: 69 | // parameters 70 | Property* _maxTrialsAfterFailure; 71 | Property* _userDeltaInit; 72 | // damping to enforce positive definite matrix 73 | Property* _initialLambda; 74 | Property* _lamdbaFactor; 75 | 76 | Eigen::VectorXd _hsd; ///< steepest decent step 77 | Eigen::VectorXd _hdl; ///< final dogleg step 78 | Eigen::VectorXd _auxVector; ///< auxilary vector used to perform multiplications or other stuff 79 | 80 | double _currentLambda; ///< the damping factor to force positive definite matrix 81 | double _delta; ///< trust region 82 | int _lastStep; ///< type of the step taken by the algorithm 83 | bool _wasPDInAllIterations; ///< the matrix we solve was positive definite in all iterations -> if not apply damping 84 | int _lastNumTries; 85 | }; 86 | 87 | } // end namespace 88 | 89 | #endif 90 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H 29 | 30 | #include "optimization_algorithm_with_hessian.h" 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief Implementation of the Gauss Newton Algorithm 36 | */ 37 | class OptimizationAlgorithmGaussNewton : public OptimizationAlgorithmWithHessian 38 | { 39 | public: 40 | /** 41 | * construct the Gauss Newton algorithm, which use the given Solver for solving the 42 | * linearized system. 43 | */ 44 | explicit OptimizationAlgorithmGaussNewton(Solver* solver); 45 | virtual ~OptimizationAlgorithmGaussNewton(); 46 | 47 | virtual SolverResult solve(int iteration, bool online = false); 48 | 49 | virtual void printVerbose(std::ostream& os) const; 50 | }; 51 | 52 | } // end namespace 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/core/optimization_algorithm_property.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H 29 | 30 | #include 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief describe the properties of a solver 36 | */ 37 | struct OptimizationAlgorithmProperty 38 | { 39 | std::string name; ///< name of the solver, e.g., var 40 | std::string desc; ///< short description of the solver 41 | std::string type; ///< type of solver, e.g., "CSparse Cholesky", "PCG" 42 | bool requiresMarginalize; ///< whether the solver requires marginalization of landmarks 43 | int poseDim; ///< dimension of the pose vertices (-1 if variable) 44 | int landmarkDim; ///< dimension of the landmar vertices (-1 if variable) 45 | OptimizationAlgorithmProperty() : 46 | name(), desc(), type(), requiresMarginalize(false), poseDim(-1), landmarkDim(-1) 47 | { 48 | } 49 | OptimizationAlgorithmProperty(const std::string& name_, const std::string& desc_, const std::string& type_, bool requiresMarginalize_, int poseDim_, int landmarkDim_) : 50 | name(name_), desc(desc_), type(type_), requiresMarginalize(requiresMarginalize_), poseDim(poseDim_), landmarkDim(landmarkDim_) 51 | { 52 | } 53 | }; 54 | 55 | } // end namespace 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H 29 | 30 | #include "optimization_algorithm.h" 31 | 32 | namespace g2o { 33 | 34 | class Solver; 35 | 36 | /** 37 | * \brief Base for solvers operating on the approximated Hessian, e.g., Gauss-Newton, Levenberg 38 | */ 39 | class OptimizationAlgorithmWithHessian : public OptimizationAlgorithm 40 | { 41 | public: 42 | explicit OptimizationAlgorithmWithHessian(Solver* solver); 43 | virtual ~OptimizationAlgorithmWithHessian(); 44 | 45 | virtual bool init(bool online = false); 46 | 47 | virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices); 48 | 49 | virtual bool buildLinearStructure(); 50 | 51 | virtual void updateLinearSystem(); 52 | 53 | virtual bool updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges); 54 | 55 | //! return the underlying solver used to solve the linear system 56 | Solver* solver() { return _solver;} 57 | 58 | /** 59 | * write debug output of the Hessian if system is not positive definite 60 | */ 61 | virtual void setWriteDebug(bool writeDebug); 62 | virtual bool writeDebug() const { return _writeDebug->value();} 63 | 64 | protected: 65 | Solver* _solver; 66 | Property* _writeDebug; 67 | 68 | }; 69 | 70 | }// end namespace 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/core/parameter.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "parameter.h" 28 | 29 | namespace g2o { 30 | 31 | Parameter::Parameter() : _id(-1) 32 | { 33 | } 34 | 35 | void Parameter::setId(int id_) 36 | { 37 | _id = id_; 38 | } 39 | 40 | } // end namespace 41 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/core/parameter.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_GRAPH_PARAMETER_HH_ 28 | #define G2O_GRAPH_PARAMETER_HH_ 29 | 30 | #include 31 | 32 | #include "hyper_graph.h" 33 | 34 | namespace g2o { 35 | 36 | class Parameter : public HyperGraph::HyperGraphElement 37 | { 38 | public: 39 | Parameter(); 40 | virtual ~Parameter() {}; 41 | //! read the data from a stream 42 | virtual bool read(std::istream& is) = 0; 43 | //! write the data to a stream 44 | virtual bool write(std::ostream& os) const = 0; 45 | int id() const {return _id;} 46 | void setId(int id_); 47 | virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_PARAMETER;} 48 | protected: 49 | int _id; 50 | }; 51 | 52 | typedef std::vector ParameterVector; 53 | 54 | } // end namespace 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/core/parameter_container.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_GRAPH_PARAMETER_CONTAINER_HH_ 28 | #define G2O_GRAPH_PARAMETER_CONTAINER_HH_ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace g2o { 35 | 36 | class Parameter; 37 | 38 | /** 39 | * \brief map id to parameters 40 | */ 41 | class ParameterContainer : protected std::map 42 | { 43 | public: 44 | typedef std::map BaseClass; 45 | 46 | /** 47 | * create a container for the parameters. 48 | * @param isMainStorage_ pointers to the parameters are owned by this container, i.e., freed in its constructor 49 | */ 50 | ParameterContainer(bool isMainStorage_=true); 51 | virtual ~ParameterContainer(); 52 | //! add parameter to the container 53 | bool addParameter(Parameter* p); 54 | //! return a parameter based on its ID 55 | Parameter* getParameter(int id); 56 | //! remove a parameter from the container, i.e., the user now owns the pointer 57 | Parameter* detachParameter(int id); 58 | //! read parameters from a stream 59 | virtual bool read(std::istream& is, const std::map* renamedMap =0); 60 | //! write the data to a stream 61 | virtual bool write(std::ostream& os) const; 62 | bool isMainStorage() const {return _isMainStorage;} 63 | void clear(); 64 | 65 | // stuff of the base class that should re-appear 66 | using BaseClass::size; 67 | 68 | protected: 69 | bool _isMainStorage; 70 | }; 71 | 72 | } // end namespace 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/core/robust_kernel.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "robust_kernel.h" 28 | 29 | namespace g2o { 30 | 31 | RobustKernel::RobustKernel() : 32 | _delta(1.) 33 | { 34 | } 35 | 36 | RobustKernel::RobustKernel(double delta) : 37 | _delta(delta) 38 | { 39 | } 40 | 41 | void RobustKernel::setDelta(double delta) 42 | { 43 | _delta = delta; 44 | } 45 | 46 | } // end namespace g2o 47 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/core/robust_kernel.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_ROBUST_KERNEL_H 28 | #define G2O_ROBUST_KERNEL_H 29 | 30 | #ifdef _MSC_VER 31 | #include 32 | #else 33 | #include 34 | #endif 35 | #include 36 | 37 | 38 | namespace g2o { 39 | 40 | /** 41 | * \brief base for all robust cost functions 42 | * 43 | * Note in all the implementations for the other cost functions the e in the 44 | * funtions corresponds to the sqaured errors, i.e., the robustification 45 | * functions gets passed the squared error. 46 | * 47 | * e.g. the robustified least squares function is 48 | * 49 | * chi^2 = sum_{e} rho( e^T Omega e ) 50 | */ 51 | class RobustKernel 52 | { 53 | public: 54 | RobustKernel(); 55 | explicit RobustKernel(double delta); 56 | virtual ~RobustKernel() {} 57 | /** 58 | * compute the scaling factor for a error: 59 | * The error is e^T Omega e 60 | * The output rho is 61 | * rho[0]: The actual scaled error value 62 | * rho[1]: First derivative of the scaling function 63 | * rho[2]: Second derivative of the scaling function 64 | */ 65 | virtual void robustify(double squaredError, Eigen::Vector3d& rho) const = 0; 66 | 67 | /** 68 | * set the window size of the error. A squared error above delta^2 is considered 69 | * as outlier in the data. 70 | */ 71 | virtual void setDelta(double delta); 72 | double delta() const { return _delta;} 73 | 74 | protected: 75 | double _delta; 76 | }; 77 | typedef std::tr1::shared_ptr RobustKernelPtr; 78 | 79 | } // end namespace g2o 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/core/solver.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "solver.h" 28 | 29 | #include 30 | #include 31 | 32 | namespace g2o { 33 | 34 | Solver::Solver() : 35 | _optimizer(0), _x(0), _b(0), _xSize(0), _maxXSize(0), 36 | _isLevenberg(false), _additionalVectorSpace(0) 37 | { 38 | } 39 | 40 | Solver::~Solver() 41 | { 42 | delete[] _x; 43 | delete[] _b; 44 | } 45 | 46 | void Solver::resizeVector(size_t sx) 47 | { 48 | size_t oldSize = _xSize; 49 | _xSize = sx; 50 | sx += _additionalVectorSpace; // allocate some additional space if requested 51 | if (_maxXSize < sx) { 52 | _maxXSize = 2*sx; 53 | delete[] _x; 54 | _x = new double[_maxXSize]; 55 | #ifndef NDEBUG 56 | memset(_x, 0, _maxXSize * sizeof(double)); 57 | #endif 58 | if (_b) { // backup the former b, might still be needed for online processing 59 | memcpy(_x, _b, oldSize * sizeof(double)); 60 | delete[] _b; 61 | _b = new double[_maxXSize]; 62 | std::swap(_b, _x); 63 | } else { 64 | _b = new double[_maxXSize]; 65 | #ifndef NDEBUG 66 | memset(_b, 0, _maxXSize * sizeof(double)); 67 | #endif 68 | } 69 | } 70 | } 71 | 72 | void Solver::setOptimizer(SparseOptimizer* optimizer) 73 | { 74 | _optimizer = optimizer; 75 | } 76 | 77 | void Solver::setLevenberg(bool levenberg) 78 | { 79 | _isLevenberg = levenberg; 80 | } 81 | 82 | void Solver::setAdditionalVectorSpace(size_t s) 83 | { 84 | _additionalVectorSpace = s; 85 | } 86 | 87 | } // end namespace 88 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/core/sparse_block_matrix_test.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "sparse_block_matrix.h" 28 | #include 29 | 30 | using namespace std; 31 | using namespace g2o; 32 | using namespace Eigen; 33 | 34 | typedef SparseBlockMatrix< MatrixXd > 35 | SparseBlockMatrixX; 36 | 37 | std::ostream& operator << (std::ostream& os, const SparseBlockMatrixX::SparseMatrixBlock& m) { 38 | for (int i=0; iblock(0,0, true); 55 | cerr << b->rows() << " " << b->cols() << endl; 56 | for (int i=0; irows(); ++i) 57 | for (int j=0; jcols(); ++j){ 58 | (*b)(i,j)=i*b->cols()+j; 59 | } 60 | 61 | 62 | cerr << "block access 2" << endl; 63 | b=M->block(0,2, true); 64 | cerr << b->rows() << " " << b->cols() << endl; 65 | for (int i=0; irows(); ++i) 66 | for (int j=0; jcols(); ++j){ 67 | (*b)(i,j)=i*b->cols()+j; 68 | } 69 | 70 | b=M->block(3,2, true); 71 | cerr << b->rows() << " " << b->cols() << endl; 72 | for (int i=0; irows(); ++i) 73 | for (int j=0; jcols(); ++j){ 74 | (*b)(i,j)=i*b->cols()+j; 75 | } 76 | 77 | cerr << *M << endl; 78 | 79 | cerr << "SUM" << endl; 80 | 81 | SparseBlockMatrixX* Ms=0; 82 | M->add(Ms); 83 | M->add(Ms); 84 | cerr << *Ms; 85 | 86 | SparseBlockMatrixX* Mt=0; 87 | M->transpose(Mt); 88 | cerr << *Mt << endl; 89 | 90 | SparseBlockMatrixX* Mp=0; 91 | M->multiply(Mp, Mt); 92 | cerr << *Mp << endl; 93 | 94 | int iperm[]={3,2,1,0}; 95 | SparseBlockMatrixX* PMp=0; 96 | 97 | Mp->symmPermutation(PMp,iperm, false); 98 | cerr << *PMp << endl; 99 | 100 | PMp->clear(true); 101 | Mp->block(3,0)->fill(0.); 102 | Mp->symmPermutation(PMp,iperm, true); 103 | cerr << *PMp << endl; 104 | 105 | 106 | 107 | } 108 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/stuff/color_macros.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_COLOR_MACROS_H 28 | #define G2O_COLOR_MACROS_H 29 | 30 | // font attributes 31 | #define FT_BOLD "\033[1m" 32 | #define FT_UNDERLINE "\033[4m" 33 | 34 | //background color 35 | #define BG_BLACK "\033[40m" 36 | #define BG_RED "\033[41m" 37 | #define BG_GREEN "\033[42m" 38 | #define BG_YELLOW "\033[43m" 39 | #define BG_LIGHTBLUE "\033[44m" 40 | #define BG_MAGENTA "\033[45m" 41 | #define BG_BLUE "\033[46m" 42 | #define BG_WHITE "\033[47m" 43 | 44 | // font color 45 | #define CL_BLACK(s) "\033[30m" << s << "\033[0m" 46 | #define CL_RED(s) "\033[31m" << s << "\033[0m" 47 | #define CL_GREEN(s) "\033[32m" << s << "\033[0m" 48 | #define CL_YELLOW(s) "\033[33m" << s << "\033[0m" 49 | #define CL_LIGHTBLUE(s) "\033[34m" << s << "\033[0m" 50 | #define CL_MAGENTA(s) "\033[35m" << s << "\033[0m" 51 | #define CL_BLUE(s) "\033[36m" << s << "\033[0m" 52 | #define CL_WHITE(s) "\033[37m" << s << "\033[0m" 53 | 54 | #define FG_BLACK "\033[30m" 55 | #define FG_RED "\033[31m" 56 | #define FG_GREEN "\033[32m" 57 | #define FG_YELLOW "\033[33m" 58 | #define FG_LIGHTBLUE "\033[34m" 59 | #define FG_MAGENTA "\033[35m" 60 | #define FG_BLUE "\033[36m" 61 | #define FG_WHITE "\033[37m" 62 | 63 | #define FG_NORM "\033[0m" 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/stuff/os_specific.c: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "os_specific.h" 28 | 29 | #ifdef WINDOWS 30 | 31 | int vasprintf(char** strp, const char* fmt, va_list ap) 32 | { 33 | int n; 34 | int size = 100; 35 | char* p; 36 | char* np; 37 | 38 | if ((p = (char*)malloc(size * sizeof(char))) == NULL) 39 | return -1; 40 | 41 | while (1) { 42 | #ifdef _MSC_VER 43 | n = vsnprintf_s(p, size, size - 1, fmt, ap); 44 | #else 45 | n = vsnprintf(p, size, fmt, ap); 46 | #endif 47 | if (n > -1 && n < size) { 48 | *strp = p; 49 | return n; 50 | } 51 | if (n > -1) 52 | size = n+1; 53 | else 54 | size *= 2; 55 | if ((np = (char*)realloc (p, size * sizeof(char))) == NULL) { 56 | free(p); 57 | return -1; 58 | } else 59 | p = np; 60 | } 61 | } 62 | 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/stuff/os_specific.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OS_SPECIFIC_HH_ 28 | #define G2O_OS_SPECIFIC_HH_ 29 | 30 | #ifdef WINDOWS 31 | #include 32 | #include 33 | #include 34 | #ifndef _WINDOWS 35 | #include 36 | #endif 37 | #define drand48() ((double) rand()/(double)RAND_MAX) 38 | 39 | #ifdef __cplusplus 40 | extern "C" { 41 | #endif 42 | 43 | int vasprintf(char** strp, const char* fmt, va_list ap); 44 | 45 | #ifdef __cplusplus 46 | } 47 | #endif 48 | 49 | #endif 50 | 51 | #ifdef UNIX 52 | #include 53 | // nothing to do on real operating systems 54 | #endif 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/types/se3_ops.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 H. Strasdat 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_MATH_STUFF 28 | #define G2O_MATH_STUFF 29 | 30 | #include 31 | #include 32 | 33 | namespace g2o { 34 | using namespace Eigen; 35 | 36 | inline Matrix3d skew(const Vector3d&v); 37 | inline Vector3d deltaR(const Matrix3d& R); 38 | inline Vector2d project(const Vector3d&); 39 | inline Vector3d project(const Vector4d&); 40 | inline Vector3d unproject(const Vector2d&); 41 | inline Vector4d unproject(const Vector3d&); 42 | 43 | #include "se3_ops.hpp" 44 | 45 | } 46 | 47 | #endif //MATH_STUFF 48 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/types/se3_ops.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 H. Strasdat 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | Matrix3d skew(const Vector3d&v) 28 | { 29 | Matrix3d m; 30 | m.fill(0.); 31 | m(0,1) = -v(2); 32 | m(0,2) = v(1); 33 | m(1,2) = -v(0); 34 | m(1,0) = v(2); 35 | m(2,0) = -v(1); 36 | m(2,1) = v(0); 37 | return m; 38 | } 39 | 40 | Vector3d deltaR(const Matrix3d& R) 41 | { 42 | Vector3d v; 43 | v(0)=R(2,1)-R(1,2); 44 | v(1)=R(0,2)-R(2,0); 45 | v(2)=R(1,0)-R(0,1); 46 | return v; 47 | } 48 | 49 | Vector2d project(const Vector3d& v) 50 | { 51 | Vector2d res; 52 | res(0) = v(0)/v(2); 53 | res(1) = v(1)/v(2); 54 | return res; 55 | } 56 | 57 | Vector3d project(const Vector4d& v) 58 | { 59 | Vector3d res; 60 | res(0) = v(0)/v(3); 61 | res(1) = v(1)/v(3); 62 | res(2) = v(2)/v(3); 63 | return res; 64 | } 65 | 66 | Vector3d unproject(const Vector2d& v) 67 | { 68 | Vector3d res; 69 | res(0) = v(0); 70 | res(1) = v(1); 71 | res(2) = 1; 72 | return res; 73 | } 74 | 75 | Vector4d unproject(const Vector3d& v) 76 | { 77 | Vector4d res; 78 | res(0) = v(0); 79 | res(1) = v(1); 80 | res(2) = v(2); 81 | res(3) = 1; 82 | return res; 83 | } 84 | 85 | 86 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/types/types_sba.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 Kurt Konolige 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "types_sba.h" 28 | #include 29 | 30 | namespace g2o { 31 | 32 | using namespace std; 33 | 34 | 35 | VertexSBAPointXYZ::VertexSBAPointXYZ() : BaseVertex<3, Vector3d>() 36 | { 37 | } 38 | 39 | bool VertexSBAPointXYZ::read(std::istream& is) 40 | { 41 | Vector3d lv; 42 | for (int i=0; i<3; i++) 43 | is >> _estimate[i]; 44 | return true; 45 | } 46 | 47 | bool VertexSBAPointXYZ::write(std::ostream& os) const 48 | { 49 | Vector3d lv=estimate(); 50 | for (int i=0; i<3; i++){ 51 | os << lv[i] << " "; 52 | } 53 | return os.good(); 54 | } 55 | 56 | } // end namespace 57 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/g2o/types/types_sba.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 Kurt Konolige 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_SBA_TYPES 28 | #define G2O_SBA_TYPES 29 | 30 | #include "../core/base_vertex.h" 31 | 32 | #include 33 | #include 34 | 35 | namespace g2o { 36 | 37 | /** 38 | * \brief Point vertex, XYZ 39 | */ 40 | class VertexSBAPointXYZ : public BaseVertex<3, Vector3d> 41 | { 42 | public: 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 44 | VertexSBAPointXYZ(); 45 | virtual bool read(std::istream& is); 46 | virtual bool write(std::ostream& os) const; 47 | 48 | virtual void setToOriginImpl() { 49 | _estimate.fill(0.); 50 | } 51 | 52 | virtual void oplusImpl(const double* update) 53 | { 54 | Eigen::Map v(update); 55 | _estimate += v; 56 | } 57 | }; 58 | 59 | } // end namespace 60 | 61 | #endif // SBA_TYPES 62 | -------------------------------------------------------------------------------- /code/Thirdparty/g2o/license-bsd.txt: -------------------------------------------------------------------------------- 1 | g2o - General Graph Optimization 2 | Copyright (C) 2011 Rainer Kuemmerle, Giorgio Grisetti, Hauke Strasdat, 3 | Kurt Konolige, and Wolfram Burgard 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 are 8 | met: 9 | 10 | * Redistributions of source code must retain the above copyright notice, 11 | this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 17 | IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 18 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 19 | PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 20 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 22 | TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 23 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 24 | LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 25 | NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | 28 | -------------------------------------------------------------------------------- /code/Vocabulary/ORBvoc.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MobiSense/SwarmMap/3ad3c5f76f67b635bfe533d80ca9d94a21a67721/code/Vocabulary/ORBvoc.bin -------------------------------------------------------------------------------- /code/build.sh: -------------------------------------------------------------------------------- 1 | echo "Configuring and building Thirdparty/DBoW2 ..." 2 | 3 | cd Thirdparty/DBoW2 4 | mkdir build 5 | cd build 6 | cmake .. -DCMAKE_BUILD_TYPE=Release 7 | make -j 8 | 9 | cd ../../g2o 10 | 11 | echo "Configuring and building Thirdparty/g2o ..." 12 | 13 | mkdir build 14 | cd build 15 | cmake .. -DCMAKE_BUILD_TYPE=Release 16 | make -j 17 | 18 | cd ../../../ 19 | 20 | echo "Configuring and building ORB_SLAM2 ..." 21 | 22 | mkdir build 23 | cd build 24 | cmake .. -DCMAKE_BUILD_TYPE=Release 25 | make -j 26 | -------------------------------------------------------------------------------- /code/build_ros.sh: -------------------------------------------------------------------------------- 1 | echo "Building ROS nodes" 2 | 3 | cd Examples/ROS/ORB_SLAM2 4 | mkdir build 5 | cd build 6 | cmake .. -DROS_BUILD_TYPE=Release 7 | make -j 8 | -------------------------------------------------------------------------------- /code/cmake_modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | # specific additional paths for some OS 65 | if (WIN32) 66 | set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include") 67 | endif(WIN32) 68 | 69 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 70 | PATHS 71 | ${CMAKE_INSTALL_PREFIX}/include 72 | ${EIGEN_ADDITIONAL_SEARCH_PATHS} 73 | ${KDE4_INCLUDE_DIR} 74 | PATH_SUFFIXES eigen3 eigen 75 | ) 76 | 77 | if(EIGEN3_INCLUDE_DIR) 78 | _eigen3_check_version() 79 | endif(EIGEN3_INCLUDE_DIR) 80 | 81 | include(FindPackageHandleStandardArgs) 82 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 83 | 84 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 85 | 86 | endif(EIGEN3_INCLUDE_DIR) 87 | 88 | -------------------------------------------------------------------------------- /code/include/ClientService.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Halcao on 2022/8/25. 3 | // 4 | 5 | #ifndef SWARM_MAP_CLIENTSERVICE_H 6 | #define SWARM_MAP_CLIENTSERVICE_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include "WebSocket.h" 12 | //#include "System.h" 13 | //#include "ConnectionService.h" 14 | //#include "System.h" 15 | 16 | namespace ORB_SLAM2 { 17 | class System; 18 | struct SystemState; 19 | using std::vector; 20 | 21 | class ClientService: public ConnectionService { 22 | public: 23 | explicit ClientService(System *); 24 | // ClientService(System *); 25 | void Connect(const std::string &host, unsigned int port); 26 | void ReportState(const SystemState &state); 27 | 28 | // client sends to server 29 | void PushMap(const std::string &content); 30 | 31 | std::tuple Register(const std::string &host, unsigned int port); 32 | protected: 33 | // content to be sent 34 | std::queue contentQueue; 35 | bool mbConnected = false; 36 | std::mutex mContentMutex; 37 | std::thread *mThread; 38 | System *mpSLAM{}; 39 | 40 | // Client connection service 41 | std::shared_ptr service; 42 | void SendRequest(const Request &req); 43 | void OnRequest(const std::string &msg); 44 | }; 45 | } 46 | 47 | #endif //SWARM_MAP_CLIENTSERVICE_H 48 | -------------------------------------------------------------------------------- /code/include/ConnectionService.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Halcao on 2022/2/28. 3 | // 4 | 5 | #ifndef EDGE_SLAM_CONNECTIONSERVICE_H 6 | #define EDGE_SLAM_CONNECTIONSERVICE_H 7 | 8 | //#include 9 | //#include 10 | //#include 11 | //#include 12 | //#include "WebSocket.h" 13 | //#include "SystemState.h" 14 | //#include "ClientService.h" 15 | //#include "ServerService.h" 16 | 17 | namespace ORB_SLAM2 { 18 | 19 | //class AgentMediator; 20 | //class System; 21 | //using std::vector; 22 | //using tcp = boost::asio::ip::tcp; // from 23 | //namespace websocket = boost::beast::websocket; // from 24 | //using work_guard_type = boost::asio::executor_work_guard; 25 | // 26 | //template 27 | //class ConnectionService { 28 | //public: 29 | ////ConnectionService(); 30 | // explicit ConnectionService(AgentMediator *pMediator); 31 | // 32 | // explicit ConnectionService(System *pSLAM); 33 | // 34 | // ConnectionService(); 35 | // 36 | // ~ConnectionService(); 37 | // void Connect(const std::string &host, unsigned int port); 38 | // void Disconnect(); 39 | // 40 | // 41 | //protected: 42 | // // content to be sent 43 | // std::queue contentQueue; 44 | // System *mpSLAM{}; 45 | // AgentMediator *mpMediator{}; 46 | // bool mbConnected = false; 47 | // std::mutex mContentMutex; 48 | // std::thread *mThread; 49 | // 50 | // // Server dispatch id and corresponding service port 51 | // std::shared_ptr dispatchService; 52 | // 53 | // // Server connection service 54 | // std::shared_ptr serverService; 55 | // // Client connection service 56 | // std::shared_ptr clientService; 57 | // 58 | // std::shared_ptr service; 59 | // std::string GetNewContent(); 60 | // 61 | // void AddNewContent(std::string &content); 62 | // 63 | // void Run(); 64 | // 65 | // void Bind(unsigned int port); 66 | // 67 | // void SendRequest(const Request &req); 68 | // 69 | // void OnRequest(const std::string &msg); 70 | //}; 71 | } 72 | #endif //EDGE_SLAM_CONNECTIONSERVICE_H 73 | -------------------------------------------------------------------------------- /code/include/Converter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef CONVERTER_H 22 | #define CONVERTER_H 23 | 24 | #include 25 | 26 | #include 27 | #include"Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" 28 | #include"Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" 29 | 30 | namespace ORB_SLAM2 31 | { 32 | 33 | class Converter 34 | { 35 | public: 36 | static std::vector toDescriptorVector(const cv::Mat &Descriptors); 37 | 38 | static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT); 39 | static g2o::SE3Quat toSE3Quat(const g2o::Sim3 &gSim3); 40 | 41 | static cv::Mat toCvMat(const g2o::SE3Quat &SE3); 42 | static cv::Mat toCvMat(const g2o::Sim3 &Sim3); 43 | static cv::Mat toCvMat(const Eigen::Matrix &m); 44 | static cv::Mat toCvMat(const Eigen::Matrix3d &m); 45 | static cv::Mat toCvMat(const Eigen::Matrix &m); 46 | static cv::Mat toCvSE3(const Eigen::Matrix &R, const Eigen::Matrix &t); 47 | 48 | static Eigen::Matrix toVector3d(const cv::Mat &cvVector); 49 | static Eigen::Matrix toVector3d(const cv::Point3f &cvPoint); 50 | static Eigen::Matrix toMatrix3d(const cv::Mat &cvMat3); 51 | 52 | static std::vector toQuaternion(const cv::Mat &M); 53 | 54 | static cv::Mat toCvSE3Inverse(const cv::Mat Tcw); 55 | }; 56 | 57 | }// namespace ORB_SLAM 58 | 59 | #endif // CONVERTER_H 60 | -------------------------------------------------------------------------------- /code/include/DataSetUtil.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Halcao on 2020/5/17. 3 | // 4 | 5 | #ifndef EDGE_SLAM_DATASETUTIL_H 6 | #define EDGE_SLAM_DATASETUTIL_H 7 | 8 | 9 | #include 10 | #include 11 | 12 | namespace ORB_SLAM2 { 13 | class DataSetUtil { 14 | public: 15 | static void LoadEuRoC(const std::string &strImagePath, const std::string &strPathTimes, 16 | std::vector &vstrImages, std::vector &vTimeStamps); 17 | 18 | static void LoadTUM(const std::string &path, std::vector &vstrImageFilenames, 19 | std::vector &vTimestamps); 20 | 21 | static void LoadKITTI(const std::string &path, std::vector &vstrImageFilenames, 22 | std::vector &vTimestamps); 23 | }; 24 | } 25 | 26 | 27 | 28 | #endif //EDGE_SLAM_DATASETUTIL_H 29 | -------------------------------------------------------------------------------- /code/include/DynamicExtractor.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zjl on 11/4/2020. 3 | // 4 | 5 | #ifndef EDGE_SLAM_DYNAMICEXTRACTOR_H 6 | #define EDGE_SLAM_DYNAMICEXTRACTOR_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | 25 | namespace ORB_SLAM2 { 26 | class DynamicExtractor { 27 | 28 | public: 29 | // need model location to load the model 30 | // only supports mask-rcnn for now 31 | DynamicExtractor(const std::string &strModelPath, int maxUsage=1, bool useOpticalFlow=false, 32 | float confThreshold = 0.5, float maskThreshold = 0.3); 33 | 34 | // compute dynamic mask for given frame 35 | // void extractMask(const cv::Mat &frame, cv::Mat &dynamic_mask); 36 | void extractMaskDirect(const cv::Mat &frame, cv::Mat &dynamic_mask); 37 | void extractMaskDirect(const std::vector& frames, std::vector &masks); 38 | 39 | private: 40 | inline static void propagate_mask(cv::Mat &mask, cv::Mat &next_mask, cv::Mat &flow) { 41 | cv::Mat map = cv::Mat(flow.size(), CV_32FC2); 42 | for (int y = 0; y(y, x); 45 | // if (mask.at(y,x) > 0) 46 | // map.at(y,x) = Point2f(x, y); 47 | // else 48 | map.at(y,x) = cv::Point2f(x+f.x, y+f.y); 49 | } 50 | } 51 | remap(mask, next_mask, map, cv::Mat(), cv::INTER_NEAREST, cv::BORDER_CONSTANT, cv::Scalar(255)); 52 | next_mask = mask & next_mask; 53 | } 54 | 55 | 56 | // do real mask extraction under CNN 57 | cv::Mat extractMask(const cv::Mat &frame); 58 | // return non-zero if the corresponding class is dynamic 59 | bool is_dynamic(int classId) { 60 | return dynamicClasses.count(classes[classId]); 61 | } 62 | 63 | float confThreshold; // Confidence threshold 64 | float maskThreshold; // Mask threshold 65 | int maskUsage; // prevMask usage counter 66 | int maxUsage; // max number of mask synchronization 67 | bool useOpticalFlow; 68 | std::vector classes; // classId --> className 69 | std::unordered_set dynamicClasses; // name of dynamic classes 70 | cv::dnn::Net net; // mask-rcnn model 71 | cv::Mat prevMask; 72 | cv::Mat prevFrame; 73 | }; 74 | 75 | } 76 | 77 | #endif //EDGE_SLAM_DYNAMICEXTRACTOR_H 78 | -------------------------------------------------------------------------------- /code/include/DynamicRunner.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zjl on 11/4/2020. 3 | // 4 | 5 | #ifndef EDGE_SLAM_DYNAMICRUNNER_H 6 | #define EDGE_SLAM_DYNAMICRUNNER_H 7 | 8 | #include "DynamicExtractor.h" 9 | #include "KeyFrame.h" 10 | #include "CLogger.h" 11 | #include 12 | #include 13 | #include 14 | 15 | 16 | 17 | namespace ORB_SLAM2 { 18 | 19 | typedef pair imKFPair; 20 | 21 | struct PairCmp{ 22 | bool operator() (imKFPair& a, imKFPair& b) 23 | { 24 | return a.first->mScore < b.first->mScore; 25 | } 26 | }; 27 | 28 | class DynamicRunner { 29 | public: 30 | static DynamicRunner * getInstance(); 31 | 32 | void Run(); 33 | void Initialize(const std::string &strModelPath, int maxUsage=1, bool useOpticalFlow=false, 34 | float confThreshold = 0.5, float maskThreshold = 0.3, int batchSize = 8); 35 | void InsertKeyFrame(KeyFrame* pKF); 36 | void InsertImage(KeyFrame* pKF, cv::Mat imGray); 37 | void RequestFinish(); 38 | bool CheckFinish(); 39 | void RequestReset(); 40 | void ResetIfRequested(); 41 | 42 | protected: 43 | int mBatchSize; 44 | bool mbFinishRequested; 45 | bool mbResetRequested; 46 | std::mutex mMutexReset; 47 | std::mutex mMutexQueue; 48 | DynamicExtractor* mMaskExtractor; 49 | // KeyFrame pending to extract mask 50 | std::list mlKeyFrames; 51 | std::priority_queue, PairCmp> mqImageQueue; 52 | // std::priority_queue> mqImageQueue; 53 | DynamicRunner(){} 54 | }; 55 | } 56 | 57 | #endif //EDGE_SLAM_DYNAMICRUNNER_H 58 | -------------------------------------------------------------------------------- /code/include/FrameDrawer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef FRAMEDRAWER_H 22 | #define FRAMEDRAWER_H 23 | 24 | #include "Tracking.h" 25 | #include "MapPoint.h" 26 | #include "Map.h" 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | 33 | 34 | namespace ORB_SLAM2 35 | { 36 | 37 | class Tracking; 38 | class Viewer; 39 | 40 | class FrameDrawer 41 | { 42 | public: 43 | FrameDrawer(Map* pMap); 44 | 45 | // Update info from the last processed frame. 46 | void Update(Tracking *pTracker); 47 | 48 | // Draw last processed frame. 49 | cv::Mat DrawFrame(); 50 | 51 | protected: 52 | 53 | void DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText); 54 | 55 | // Info of the frame to be drawn 56 | cv::Mat mIm; 57 | int N; 58 | vector mvCurrentKeys; 59 | vector mvbMap, mvbVO; 60 | bool mbOnlyTracking; 61 | int mnTracked, mnTrackedVO; 62 | vector mvIniKeys; 63 | vector mvIniMatches; 64 | int mState; 65 | 66 | Map* mpMap; 67 | 68 | std::mutex mMutex; 69 | }; 70 | 71 | } //namespace ORB_SLAM 72 | 73 | #endif // FRAMEDRAWER_H 74 | -------------------------------------------------------------------------------- /code/include/KeyFrameDatabase.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef KEYFRAMEDATABASE_H 22 | #define KEYFRAMEDATABASE_H 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include "KeyFrame.h" 29 | #include "Frame.h" 30 | #include "ORBVocabulary.h" 31 | 32 | #include 33 | 34 | 35 | namespace ORB_SLAM2 36 | { 37 | 38 | class KeyFrame; 39 | class Frame; 40 | class Map; 41 | 42 | 43 | class KeyFrameDatabase 44 | { 45 | public: 46 | KeyFrameDatabase() {} 47 | 48 | KeyFrameDatabase(const ORBVocabulary &voc); 49 | 50 | void add(KeyFrame* pKF); 51 | 52 | void erase(KeyFrame* pKF); 53 | 54 | void clear(); 55 | 56 | // Loop Detection 57 | std::vector DetectLoopCandidates(KeyFrame* pKF, float minScore); 58 | 59 | // Relocalization 60 | std::vector DetectRelocalizationCandidates(Frame* F); 61 | 62 | void SetORBvocabulary(ORBVocabulary *porbv) { 63 | mpVoc=porbv; 64 | mvInvertedFile.resize(mpVoc->size()); 65 | } 66 | 67 | //protected: 68 | public: 69 | // Associated vocabulary 70 | const ORBVocabulary* mpVoc; 71 | 72 | // Inverted file 73 | std::vector > mvInvertedFile; 74 | std::vector> mvInvertedFileId; 75 | 76 | // Mutex 77 | std::mutex mMutex; 78 | 79 | void SetupSerializationVariable(); 80 | void RestoreSerializationVariable(Map* pMap); 81 | }; 82 | 83 | } //namespace ORB_SLAM 84 | 85 | #endif 86 | -------------------------------------------------------------------------------- /code/include/LandmarkScoring.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Halcao on 2020/7/31. 3 | // 4 | 5 | #ifndef EDGE_SLAM_LANDMARKSCORING_H 6 | #define EDGE_SLAM_LANDMARKSCORING_H 7 | 8 | #include 9 | #include 10 | #include "MapSlice.h" 11 | 12 | namespace ORB_SLAM2 { 13 | 14 | class KeyFrame; 15 | class MapPoint; 16 | 17 | struct MapPointScoreItem { 18 | unsigned long mnId = 0; 19 | // - landmark creation time 20 | // - 创建时间 21 | double createdTime = 0; 22 | // - score item created time 23 | // - 评分项创建时间 24 | double timestamp = 0; 25 | // - landmark last tracked time 26 | // - 上次被观测到的时间 27 | double lastTrackedTime = 0; 28 | // - observed keyframe count 29 | // - 被观察的关键帧数量 30 | int observedCount = 0; 31 | // - number of the map-point was modified or updated in the last round 32 | int updateFreq = 0; 33 | // - velocity when map-point was created 34 | double velocity = 0; 35 | // - observed keyframes total tracked length 36 | // - 被观察的关键帧的路径长度之和 37 | double trackedLength = 0; 38 | // - max observing distance between keyframes 39 | // - 观测着landmark的关键帧之间最大的距离 40 | double maxDistance = 0; 41 | // - max observing keyframes angle 42 | // - 观测着landmark的关键帧之间最大的夹角 43 | double maxAngle = 0; 44 | // - number of evaluation round 45 | // - 被评估的轮数 46 | int round = 0; 47 | // - total score 48 | double score = 0; 49 | }; 50 | 51 | struct KeyFrameScoreItem { 52 | unsigned long mnId = 0; 53 | // - keyframe creation time 54 | // - 创建时间 55 | double createdTime = 0; 56 | // - score item created time 57 | // - 评分项创建时间 58 | double timestamp = 0; 59 | // - keyframe speed 60 | double speed = 0; 61 | // - matched ratio: matched / total 62 | double matchedRatio = 0; 63 | // - total points 64 | double totalPoints = 0; 65 | double pointScore = 0; 66 | double connectedKF = 0; 67 | double bestCovisibleCount = 0; 68 | // - number of evaluation round 69 | // - 被评估的轮数 70 | int round = 0; 71 | // - total score 72 | double score = 0; 73 | }; 74 | 75 | class LandmarkScoring { 76 | public: 77 | 78 | static std::map > mappointMap; 79 | static std::map> keyframeMap; 80 | static void Save(const std::string &filename); 81 | 82 | static int round; 83 | 84 | static double GetRequestPriority(const ORB_SLAM2::MapSlice &slice); 85 | static std::vector Rank(const MapSlice &slice); 86 | static std::vector GetScores(const std::vectorKFs); 87 | static std::vector GetScores(const std::vector MPs); 88 | private: 89 | double startTime; 90 | // mp related 91 | static std::map maxTrackedLength; 92 | static std::map maxAngle; 93 | static std::map maxDistance; 94 | static std::map maxVelocity; 95 | static std::map maxUpdateFreq; 96 | static std::map maxObservedCount; 97 | 98 | static double maxMG; 99 | static double maxMS; 100 | 101 | 102 | static void CalcFinalScore(MapPointScoreItem &item, unsigned long mapId); 103 | }; 104 | } 105 | 106 | #endif //EDGE_SLAM_LANDMARKSCORING_H 107 | -------------------------------------------------------------------------------- /code/include/LocalMapping.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef LOCALMAPPING_H 22 | #define LOCALMAPPING_H 23 | 24 | #include "KeyFrame.h" 25 | #include "Map.h" 26 | #include "LoopClosing.h" 27 | #include "Tracking.h" 28 | #include "KeyFrameDatabase.h" 29 | 30 | #include 31 | 32 | 33 | namespace ORB_SLAM2 34 | { 35 | 36 | class Tracking; 37 | class LoopClosing; 38 | class Map; 39 | class KeyFrameDatabase; 40 | 41 | class LocalMapping 42 | { 43 | public: 44 | LocalMapping(Map* pMap, const float bMonocular); 45 | 46 | void SetLoopCloser(LoopClosing* pLoopCloser); 47 | 48 | void SetTracker(Tracking* pTracker); 49 | 50 | void SetKeyFrameDB(KeyFrameDatabase* pKFDB); 51 | 52 | // Main function 53 | void Run(); 54 | 55 | void InsertKeyFrame(KeyFrame* pKF); 56 | 57 | // Thread Synch 58 | void RequestStop(); 59 | void RequestReset(); 60 | bool Stop(); 61 | void Release(); 62 | bool isStopped(); 63 | bool stopRequested(); 64 | bool AcceptKeyFrames(); 65 | void SetAcceptKeyFrames(bool flag); 66 | bool SetNotStop(bool flag); 67 | 68 | void InterruptBA(); 69 | 70 | void RequestFinish(); 71 | bool isFinished(); 72 | 73 | int KeyframesInQueue(); 74 | 75 | protected: 76 | bool CheckNewKeyFrames(); 77 | void ProcessNewKeyFrame(); 78 | void CreateNewMapPoints(); 79 | 80 | void MapPointCulling(); 81 | void SearchInNeighbors(); 82 | 83 | void KeyFrameCulling(); 84 | 85 | cv::Mat ComputeF12(KeyFrame* &pKF1, KeyFrame* &pKF2); 86 | 87 | cv::Mat SkewSymmetricMatrix(const cv::Mat &v); 88 | 89 | bool mbMonocular; 90 | 91 | void ResetIfRequested(); 92 | bool mbResetRequested; 93 | std::mutex mMutexReset; 94 | 95 | bool CheckFinish(); 96 | void SetFinish(); 97 | bool mbFinishRequested; 98 | bool mbFinished; 99 | std::mutex mMutexFinish; 100 | 101 | Map* mpMap; 102 | 103 | KeyFrameDatabase *mpKeyFrameDB; 104 | LoopClosing* mpLoopCloser; 105 | Tracking* mpTracker; 106 | 107 | std::list mlNewKeyFrames; 108 | 109 | KeyFrame* mpCurrentKeyFrame; 110 | 111 | std::list mlpRecentAddedMapPoints; 112 | 113 | std::mutex mMutexNewKFs; 114 | 115 | bool mbAbortBA; 116 | 117 | bool mbStopped; 118 | bool mbStopRequested; 119 | bool mbNotStop; 120 | std::mutex mMutexStop; 121 | 122 | bool mbAcceptKeyFrames; 123 | std::mutex mMutexAccept; 124 | }; 125 | 126 | } //namespace ORB_SLAM 127 | 128 | #endif // LOCALMAPPING_H 129 | -------------------------------------------------------------------------------- /code/include/MapDrawer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef MAPDRAWER_H 22 | #define MAPDRAWER_H 23 | 24 | #include"Map.h" 25 | #include"MapPoint.h" 26 | #include"KeyFrame.h" 27 | #include 28 | 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(const bool bDrawKF, pangolin::OpenGlMatrix &Twc); 44 | void SetCurrentCameraPose(const cv::Mat &Tcw); 45 | void SetReferenceKeyFrame(KeyFrame *pKF); 46 | void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M); 47 | 48 | static vector pointColors; 49 | static vector frameColors; 50 | 51 | double frameScaleFactor = 1; 52 | private: 53 | 54 | float mKeyFrameSize; 55 | float mKeyFrameLineWidth; 56 | float mGraphLineWidth; 57 | float mPointSize; 58 | float mCameraSize; 59 | float mCameraLineWidth; 60 | 61 | cv::Mat mCameraPose; 62 | 63 | std::mutex mMutexCamera; 64 | }; 65 | 66 | } //namespace ORB_SLAM 67 | 68 | #endif // MAPDRAWER_H 69 | -------------------------------------------------------------------------------- /code/include/MapElementUpdate.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | // 6 | // Created by Halcao on 2020/4/18. 7 | // 8 | 9 | #ifndef EDGE_SLAM_KEYFRAMEUPDATE_H 10 | #define EDGE_SLAM_KEYFRAMEUPDATE_H 11 | namespace ORB_SLAM2 { 12 | 13 | enum MapElementUpdateType { 14 | MapPointType, 15 | KeyFrameType, 16 | MapEventType 17 | }; 18 | 19 | class MapElementUpdateBase { 20 | public: 21 | static unsigned long curId; 22 | unsigned long id = -1ul; 23 | unsigned long mnId = -1ul; 24 | // keyframe function name 25 | std::string funcName; 26 | // MapElementUpdateType type; 27 | 28 | MapElementUpdateBase() = default; 29 | MapElementUpdateBase(unsigned long _mnId, std::string _func) : id(++curId), mnId(_mnId), 30 | funcName(std::move(_func)) {}; 31 | 32 | virtual MapElementUpdateType getType() { return MapEventType; }; 33 | virtual ~MapElementUpdateBase() = default; 34 | }; 35 | 36 | template 37 | class KeyFrameUpdate : public MapElementUpdateBase { 38 | 39 | public: 40 | KeyFrameUpdate() = default; 41 | KeyFrameUpdate(unsigned long _mnId, std::string _func, T _arg) : MapElementUpdateBase(_mnId, _func), 42 | arg(_arg) {}; 43 | T arg; 44 | 45 | MapElementUpdateType getType() override { 46 | return MapElementUpdateType::KeyFrameType; 47 | } 48 | 49 | ~KeyFrameUpdate() override = default; 50 | }; 51 | 52 | template 53 | class MapPointUpdate : public MapElementUpdateBase { 54 | 55 | public: 56 | MapPointUpdate() = default; 57 | MapPointUpdate(unsigned long _mnId, std::string _func, T _arg) : MapElementUpdateBase(_mnId, _func), 58 | arg(_arg) {}; 59 | T arg; 60 | 61 | MapElementUpdateType getType() override { 62 | return MapElementUpdateType::MapPointType; 63 | } 64 | 65 | ~MapPointUpdate() override = default; 66 | }; 67 | 68 | template 69 | class MapEventUpdate : public MapElementUpdateBase { 70 | 71 | public: 72 | MapEventUpdate() = default; 73 | MapEventUpdate(unsigned long _mnId, std::string _func, T _arg) : MapElementUpdateBase(_mnId, _func), 74 | arg(_arg) {}; 75 | T arg; 76 | 77 | MapElementUpdateType getType() override { 78 | return MapElementUpdateType::MapEventType; 79 | } 80 | 81 | ~MapEventUpdate() override = default; 82 | }; 83 | } 84 | 85 | #endif //EDGE_SLAM_KEYFRAMEUPDATE_H 86 | -------------------------------------------------------------------------------- /code/include/MapEnhancer.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zjl on 11/16/2020. 3 | // 4 | 5 | #ifndef EDGE_SLAM_MAPENHANCER_H 6 | #define EDGE_SLAM_MAPENHANCER_H 7 | 8 | #include 9 | #include 10 | 11 | #include "MapPoint.h" 12 | #include "KeyFrame.h" 13 | #include "Frame.h" 14 | 15 | namespace ORB_SLAM2 { 16 | 17 | class MapEnhancer { 18 | public: 19 | MapEnhancer() = delete; 20 | static KeyFrame *GetVirtualKeyFrame(const MapSlice &slice); 21 | static void Compress(Map *pMap); 22 | 23 | private: 24 | static KeyFrame *GenerateKeyFrame(const cv::Mat& Scw, KeyFrame *pRefKF, bool isIdentical=false); 25 | }; 26 | 27 | } 28 | 29 | #endif //EDGE_SLAM_MAPENHANCER_H 30 | -------------------------------------------------------------------------------- /code/include/MapManager.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Halcao on 2020/6/19. 3 | // 4 | 5 | #ifndef EDGE_SLAM_MAPMANAGER_HPP 6 | #define EDGE_SLAM_MAPMANAGER_HPP 7 | 8 | #include 9 | #include 10 | #include"Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" 11 | #include"Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" 12 | 13 | namespace ORB_SLAM2 { 14 | using std::vector; 15 | using std::map; 16 | 17 | class MapManager { 18 | public: 19 | vector GetAllMaps(); 20 | static void MergeMap(Map *pMap1, Map *pMap2, const g2o::Sim3 &T12); 21 | static void Fuse(Map *pMap, const vector& otherMapPoints); 22 | static void Fuse(Map *pMap1, Map *pMap2); 23 | static void Register(Map *pMap); 24 | static bool IsInSameGroup(Map *pMap1, Map *pMap2); 25 | static void MoveToMapGroup(Map *src, Map *dst); 26 | static void SaveGlobalMap(const string& filename); 27 | static vector GetGroup(Map *pMap); 28 | private: 29 | // group base map Id -> map group 30 | static map > mapGroups; 31 | // global base map id 32 | static unsigned long baseMapId; 33 | static map mapDict; 34 | // map id -> group id 35 | static void KeyFrameCulling(KeyFrame *&mpCurrentKeyFrame, vector &retainedKeyframes); 36 | }; 37 | } 38 | 39 | #endif //EDGE_SLAM_MAPMANAGER_HPP 40 | -------------------------------------------------------------------------------- /code/include/MapSlice.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Halcao on 2020/7/2. 3 | // 4 | 5 | #ifndef EDGE_SLAM_MAPSLICE_HPP 6 | #define EDGE_SLAM_MAPSLICE_HPP 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | namespace ORB_SLAM2 { 13 | class KeyFrame; 14 | class MapPoint; 15 | class MapElementUpdateBase; 16 | 17 | class MapSlice { 18 | public: 19 | MapSlice(const std::vector &keyframes, const std::vector &mappoints, 20 | const std::vector &updates); 21 | MapSlice() = default; 22 | 23 | std::vector KFs; 24 | std::vector MPs; 25 | std::vector updates; 26 | 27 | friend class boost::serialization::access; 28 | template 29 | void serialize(Archive & ar, const unsigned int version) { 30 | ar & KFs; 31 | ar & MPs; 32 | ar & updates; 33 | } 34 | }; 35 | } 36 | 37 | #endif //EDGE_SLAM_MAPSLICE_HPP 38 | -------------------------------------------------------------------------------- /code/include/MapUpdater.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Halcao on 2020/4/20. 3 | // 4 | 5 | #ifndef EDGE_SLAM_MAPUPDATER_H 6 | #define EDGE_SLAM_MAPUPDATER_H 7 | 8 | #include "Map.h" 9 | #include "MapElementUpdate.h" 10 | #include "KeyFrameDatabase.h" 11 | #include "LoopClosing.h" 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace ORB_SLAM2 { 19 | /** 20 | * Steps: 21 | * 1. Structure for MapUpdater 22 | * 2. Serialization: KeyFrameUpdate 23 | * 3. Apply Update 24 | */ 25 | 26 | using KeyFrameUpdateHandler = std::function; 27 | using MapPointUpdateHandler = std::function; 28 | using MapEventUpdateHandler = std::function; 29 | class KeyFrame; 30 | class MapSlice; 31 | 32 | class MapUpdater { 33 | public: 34 | MapUpdater() = delete; 35 | 36 | static vector Apply(Map *pMap, vector &updates); 37 | 38 | template 39 | static void RegisterType(Archive &ar); 40 | 41 | static void Serialize(const MapSlice &slice, string &result); 42 | static void Deserialize(MapSlice &slice, const string &result); 43 | 44 | private: 45 | static map kfHandlerMap; 46 | // helps to initialize the handler map 47 | static map initKeyFrameHandler(); 48 | 49 | static map mpHandlerMap; 50 | static map initMapPointHandler(); 51 | 52 | static map mapHandlerMap; 53 | static map initMapHandler(); 54 | 55 | }; 56 | } 57 | 58 | #endif //EDGE_SLAM_MAPUPDATER_H 59 | -------------------------------------------------------------------------------- /code/include/Mapit.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Halcao on 2022/2/24. 3 | // 4 | 5 | #ifndef EDGE_SLAM_MAPIT_H 6 | #define EDGE_SLAM_MAPIT_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include "MapSlice.h" 12 | #include "Thirdparty/g2o/g2o/types/sim3.h" 13 | 14 | namespace ORB_SLAM2 { 15 | 16 | class MapPoint; 17 | class KeyFrame; 18 | class Map; 19 | class MapElementUpdateBase; 20 | 21 | using std::map; 22 | using std::vector; 23 | using std::mutex; 24 | 25 | class Mapit { 26 | public: 27 | explicit Mapit(Map *_pMap); 28 | 29 | // add map element update item 30 | void Add(MapElementUpdateBase *update); 31 | 32 | std::vector DequeueUpdates(); 33 | 34 | 35 | // delete redundant updates and compress it 36 | static void Aggregate(map &allKFs, 37 | map &allMPs, 38 | vector &updates); 39 | 40 | void Push(std::string &result); 41 | 42 | static void Merge(Map *pMap1, Map *pMap2, const g2o::Sim3 &T12); 43 | 44 | // clear map updates 45 | void Clear(bool bAddUpdate=false); 46 | 47 | void ReceivePush(const MapSlice &slice); 48 | 49 | private: 50 | Map *pMap; 51 | vector mapUpdates; 52 | mutex mMutexElementUpdate; 53 | 54 | void Pull(); 55 | 56 | void ReplyPull(); 57 | 58 | void ReplyPull(MapSlice &slice); 59 | }; 60 | 61 | } 62 | #endif //EDGE_SLAM_MAPIT_H 63 | -------------------------------------------------------------------------------- /code/include/MediatorScheduler.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Halcao on 2020/12/20. 3 | // 4 | 5 | #ifndef EDGE_SLAM_MEDIATORSCHEDULER_H 6 | #define EDGE_SLAM_MEDIATORSCHEDULER_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace ORB_SLAM2 { 16 | 17 | class AgentMediator; 18 | class System; 19 | class KeyFrame; 20 | class MapPoint; 21 | using namespace std; 22 | 23 | struct MediatorRequest { 24 | size_t src; 25 | size_t dst; 26 | MapSlice slice; 27 | AgentMediator *mediator; 28 | double contribScore; 29 | 30 | bool operator <(const MediatorRequest &other) const; 31 | }; 32 | 33 | struct MediatorRequestComparator { 34 | bool operator()(const MediatorRequest *a, const MediatorRequest *b) { 35 | return *a < *b; 36 | } 37 | }; 38 | 39 | class MediatorScheduler { 40 | public: 41 | // TODO(halcao): make it static, not singleton 42 | static MediatorScheduler& GetInstance() { 43 | static MediatorScheduler instance; 44 | return instance; 45 | } 46 | 47 | MediatorScheduler(MediatorScheduler const&) = delete; 48 | void operator=(MediatorScheduler const&) = delete; 49 | 50 | void RegisterMediator(AgentMediator *mediator); 51 | AgentMediator *GetMediator(size_t id); 52 | void EnqueueRequest(unsigned long id, const string &message); 53 | 54 | KeyFrame *GetKeyFrame(unsigned long id); 55 | MapPoint *GetMapPoint(unsigned long id); 56 | 57 | void Run(); 58 | void RequestStop(); 59 | static void MapDistribute(AgentMediator *mediator); 60 | public: 61 | AgentMediator *globalMediator = nullptr; 62 | private: 63 | MediatorScheduler(); 64 | // TODO(halcao): make it static 65 | unordered_map mediatorMap; 66 | unordered_map threadPool; 67 | priority_queue, MediatorRequestComparator> queue; 68 | bool ShouldStop(); 69 | bool mbShouldStop = false; 70 | std::mutex mMutexShouldStop; 71 | std::mutex mMutexQueue; 72 | std::thread *mptRunning; 73 | 74 | void ProcessRequest(const MediatorRequest *request) const; 75 | 76 | bool CheckQueueEmpty(); 77 | 78 | MediatorRequest *FetchRequest(); 79 | }; 80 | } 81 | 82 | #endif //EDGE_SLAM_MEDIATORSCHEDULER_H 83 | -------------------------------------------------------------------------------- /code/include/ORBVocabulary.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #ifndef ORBVOCABULARY_H 23 | #define ORBVOCABULARY_H 24 | 25 | #include"Thirdparty/DBoW2/DBoW2/FORB.h" 26 | #include"Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h" 27 | 28 | namespace ORB_SLAM2 29 | { 30 | 31 | typedef DBoW2::TemplatedVocabulary 32 | ORBVocabulary; 33 | 34 | } //namespace ORB_SLAM 35 | 36 | #endif // ORBVOCABULARY_H 37 | -------------------------------------------------------------------------------- /code/include/Optimizer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef OPTIMIZER_H 22 | #define OPTIMIZER_H 23 | 24 | #include "Map.h" 25 | #include "MapPoint.h" 26 | #include "KeyFrame.h" 27 | #include "LoopClosing.h" 28 | #include "Frame.h" 29 | 30 | #include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" 31 | 32 | 33 | namespace ORB_SLAM2 34 | { 35 | 36 | class LoopClosing; 37 | 38 | class Optimizer 39 | { 40 | public: 41 | void static BundleAdjustment(const std::vector &vpKF, const std::vector &vpMP, 42 | int nIterations = 5, bool *pbStopFlag=NULL, const unsigned long nLoopKF=0, 43 | const bool bRobust = true, const bool bGlobal=false); 44 | void static GlobalBundleAdjustment(Map *pMap, int nIterations = 5, bool *pbStopFlag = NULL, 45 | const unsigned long nLoopKF = 0, const bool bRobust = true); 46 | void static LocalBundleAdjustment(KeyFrame* pKF, bool *pbStopFlag, Map *pMap); 47 | int static PoseOptimization(Frame* pFrame, const bool bGlobal=false); 48 | 49 | // if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise (mono) 50 | void static OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF, 51 | const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, 52 | const LoopClosing::KeyFrameAndPose &CorrectedSim3, 53 | const map > &LoopConnections, 54 | const bool &bFixScale); 55 | 56 | // if bFixScale is true, optimize SE3 (stereo,rgbd), Sim3 otherwise (mono) 57 | static int OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector &vpMatches1, 58 | g2o::Sim3 &g2oS12, const float th2, const bool bFixScale); 59 | 60 | // static int OptimizeSim3(vector vpKFs, const float th2, g2o::Sim3 &T12); 61 | static int OptimizeSim3(KeyFrame *pKF1, KeyFrame *pTargetKF, const float th2, g2o::Sim3 &T12); 62 | 63 | static int OptimizeSim3ByMapPoint(KeyFrame *pKF1, KeyFrame *pTargetKF, const float th2, g2o::Sim3 &Twl); 64 | 65 | static int OptimizeSim3ByMapPoint(KeyFrame *pKF1, KeyFrame *pTargetKF, vector> pairs, 66 | const float th2, 67 | g2o::Sim3 &Twl); 68 | }; 69 | 70 | } //namespace ORB_SLAM 71 | 72 | #endif // OPTIMIZER_H 73 | -------------------------------------------------------------------------------- /code/include/STS.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Halcao on 2022/2/28. 3 | // 4 | 5 | #ifndef EDGE_SLAM_STS_H 6 | #define EDGE_SLAM_STS_H 7 | 8 | namespace ORB_SLAM2 { 9 | 10 | //class ConnectionService; 11 | 12 | class STS { 13 | public: 14 | void ReportState(); 15 | 16 | // void SetConnectionService(ConnectionService *connectionService); 17 | private: 18 | // ConnectionService *mpConnectionService; 19 | }; 20 | 21 | } 22 | 23 | #endif //EDGE_SLAM_STS_H 24 | -------------------------------------------------------------------------------- /code/include/ServerService.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Halcao on 2022/8/25. 3 | // 4 | 5 | #ifndef SWARM_MAP_SERVERSERVICE_H 6 | #define SWARM_MAP_SERVERSERVICE_H 7 | 8 | 9 | #include 10 | #include 11 | #include 12 | //#include "SystemState.h" 13 | #include "WebSocket.h" 14 | //#include "ConnectionService.h" 15 | 16 | namespace ORB_SLAM2 { 17 | class AgentMediator; 18 | struct SystemState; 19 | using std::vector; 20 | 21 | class ServerService: public ConnectionService { 22 | public: 23 | // ServerService(AgentMediator *); 24 | explicit ServerService(AgentMediator *); 25 | // void Connect(const std::string &host, unsigned int port); 26 | // void ReportState(const SystemState &state); 27 | 28 | // server sends to client 29 | void DistributeMap(const std::string &content); 30 | 31 | unsigned int GetPort(); 32 | bool isConnected() { return mbConnected; } 33 | protected: 34 | // content to be sent 35 | std::queue contentQueue; 36 | std::mutex mContentMutex; 37 | std::thread *mThread; 38 | bool mbConnected = false; 39 | AgentMediator *mpMediator{}; 40 | 41 | std::shared_ptr service; 42 | 43 | void Bind(unsigned int port); 44 | void SendRequest(const Request &req); 45 | void OnRequest(const std::string &msg); 46 | }; 47 | } 48 | 49 | 50 | #endif //SWARM_MAP_SERVERSERVICE_H 51 | -------------------------------------------------------------------------------- /code/include/SystemState.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Halcao on 2020/12/23. 3 | // 4 | 5 | #ifndef EDGE_SLAM_SYSTEMSTATE_H 6 | #define EDGE_SLAM_SYSTEMSTATE_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | namespace ORB_SLAM2 { 13 | 14 | enum TrackingState : int; 15 | 16 | struct SystemState { 17 | cv::Mat location; 18 | bool bVelocityBurst; 19 | bool bStable; 20 | uint8_t nTracked; 21 | size_t lostCount; 22 | 23 | // friend class boost::serialization::access; 24 | // template 25 | // void serialize(Archive & ar, __attribute__((unused)) const unsigned int version) { 26 | // ar & location; 27 | // ar & bVelocityBurst; 28 | // ar & bStable; 29 | // ar & nTracked; 30 | // ar & lostCount; 31 | // } 32 | 33 | // std::string toString() const { 34 | // std::stringstream out; 35 | // boost::archive::text_oarchive oa(out); 36 | //// boost::archive::binary_oarchive oa(out); 37 | // // start to serialize 38 | // try { 39 | // oa << this; 40 | // } catch (boost::archive::archive_exception &e) { 41 | // } 42 | // 43 | // return out.str(); 44 | // } 45 | }; 46 | 47 | } 48 | #endif //EDGE_SLAM_SYSTEMSTATE_H 49 | -------------------------------------------------------------------------------- /code/include/Timer.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Halcao on 2020/5/26. 3 | // 4 | 5 | #ifndef EDGE_SLAM_TIMER_H 6 | #define EDGE_SLAM_TIMER_H 7 | #include 8 | 9 | namespace ORB_SLAM2 { 10 | #define FuncTimer() Timer t(__func__, true); 11 | 12 | class Timer { 13 | public: 14 | explicit Timer(std::string funcName_="", bool logTime=true); 15 | ~Timer(); 16 | 17 | double get(); 18 | void setInfoLog(bool isInfoLog_); 19 | static Timer &globalInstance(); 20 | private: 21 | bool logTime = true; 22 | bool isInfoLog = false; 23 | std::string funcName; 24 | std::chrono::steady_clock::time_point start; 25 | };} 26 | 27 | #endif //EDGE_SLAM_TIMER_H 28 | -------------------------------------------------------------------------------- /code/include/Utils.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef __UTILS_HPP__ 3 | #define __UTILS_HPP__ 4 | 5 | #include 6 | #include 7 | 8 | #define SET_CLOCK(t0) \ 9 | std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now(); 10 | 11 | #define TIME_DIFF(t1, t0) \ 12 | (std::chrono::duration_cast>((t1) - (t0)).count()) 13 | 14 | #define PRINT_CLOCK(msg, t1, t0) \ 15 | std::cerr << msg << TIME_DIFF(t1, t0) << endl; 16 | 17 | #ifdef USE_NVTX 18 | #include "nvToolsExt.h" 19 | 20 | static const uint32_t colors[] = { 0x0000ff00, 0x000000ff, 0x00ffff00, 0x00ff00ff, 0x0000ffff, 0x00ff0000, 0x00ffffff }; 21 | static const int num_colors = sizeof(colors)/sizeof(uint32_t); 22 | 23 | #define PUSH_RANGE(name,cid) { \ 24 | int color_id = cid; \ 25 | color_id = color_id%num_colors;\ 26 | nvtxEventAttributes_t eventAttrib; \ 27 | memset(&eventAttrib, 0, NVTX_EVENT_ATTRIB_STRUCT_SIZE); \ 28 | eventAttrib.version = NVTX_VERSION; \ 29 | eventAttrib.size = NVTX_EVENT_ATTRIB_STRUCT_SIZE; \ 30 | eventAttrib.colorType = NVTX_COLOR_ARGB; \ 31 | eventAttrib.color = colors[color_id]; \ 32 | eventAttrib.messageType = NVTX_MESSAGE_TYPE_ASCII; \ 33 | eventAttrib.message.ascii = name; \ 34 | nvtxRangePushEx(&eventAttrib); \ 35 | } 36 | #define POP_RANGE nvtxRangePop(); 37 | #else 38 | #define PUSH_RANGE(name,cid) 39 | #define POP_RANGE 40 | #endif 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /code/include/Viewer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #ifndef VIEWER_H 23 | #define VIEWER_H 24 | 25 | #include "FrameDrawer.h" 26 | #include "MapDrawer.h" 27 | #include "Tracking.h" 28 | #include "System.h" 29 | 30 | #include 31 | 32 | namespace ORB_SLAM2 33 | { 34 | 35 | class Tracking; 36 | class FrameDrawer; 37 | class MapDrawer; 38 | class System; 39 | 40 | class Viewer 41 | { 42 | public: 43 | Viewer(System* pSystem, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Tracking *pTracking, const string &strSettingPath 44 | , const bool bUseViewer = true, const bool bUseMapViewer=true); 45 | 46 | // Main thread function. Draw points, keyframes, the current camera pose and the last processed 47 | // frame. Drawing is refreshed according to the camera fps. We use Pangolin. 48 | void Run(); 49 | 50 | void RequestFinish(); 51 | 52 | void RequestStop(); 53 | 54 | bool isFinished(); 55 | 56 | bool isStopped(); 57 | 58 | void Release(); 59 | 60 | void SetTitle(const string& name, const string& cvName); 61 | 62 | private: 63 | 64 | bool Stop(); 65 | 66 | System* mpSystem; 67 | FrameDrawer* mpFrameDrawer; 68 | MapDrawer* mpMapDrawer; 69 | Tracking* mpTracker; 70 | 71 | // 1/fps in ms 72 | double mT; 73 | float mImageWidth, mImageHeight; 74 | 75 | float mViewpointX, mViewpointY, mViewpointZ, mViewpointF; 76 | 77 | bool CheckFinish(); 78 | void SetFinish(); 79 | bool mbFinishRequested; 80 | bool mbFinished; 81 | std::mutex mMutexFinish; 82 | 83 | bool mbStopped; 84 | bool mbStopRequested; 85 | std::mutex mMutexStop; 86 | 87 | bool mbUseViewer; 88 | bool mbUseMapViewer; 89 | 90 | string windowTitle = "ORB-SLAM2: Map Viewer"; 91 | string cvWindowName = "ORB-SLAM2: Current Frame"; 92 | }; 93 | 94 | } 95 | 96 | 97 | #endif // VIEWER_H 98 | 99 | 100 | -------------------------------------------------------------------------------- /code/include/cuda/Allocator.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef __ALLOCATOR_HPP__ 3 | #define __ALLOCATOR_HPP__ 4 | 5 | #include 6 | 7 | namespace ORB_SLAM2 { namespace cuda { 8 | extern cv::cuda::GpuMat::Allocator * gpu_mat_allocator; 9 | 10 | class Allocator : public cv::cuda::GpuMat::Allocator 11 | { 12 | const int allocatorPitchBase = 128; 13 | size_t getPitch(size_t widthSize); 14 | 15 | public: 16 | 17 | bool allocate(cv::cuda::GpuMat* mat, int rows, int cols, size_t elemSize); 18 | void free(cv::cuda::GpuMat* mat); 19 | }; 20 | } } 21 | 22 | #endif 23 | -------------------------------------------------------------------------------- /code/include/cuda/Cuda.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef __ORB_SLAM2_CUDA_HPP__ 3 | #define __ORB_SLAM2_CUDA_HPP__ 4 | 5 | namespace ORB_SLAM2 { namespace cuda { 6 | void deviceSynchronize(); 7 | } } 8 | 9 | #endif 10 | -------------------------------------------------------------------------------- /code/include/cuda/Fast.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef __FAST_HPP__ 3 | #define __FAST_HPP__ 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace ORB_SLAM2 { namespace cuda { 12 | using namespace std; 13 | using namespace cv; 14 | using namespace cv::cuda; 15 | 16 | const float FEATURE_SIZE = 7.0; 17 | 18 | class GpuFast { 19 | short2 * kpLoc; 20 | float * kpScore; 21 | unsigned int * counter_ptr; 22 | unsigned int highThreshold; 23 | unsigned int lowThreshold; 24 | unsigned int maxKeypoints; 25 | unsigned int count; 26 | cv::cuda::GpuMat scoreMat; 27 | cudaStream_t stream; 28 | Stream cvStream; 29 | public: 30 | GpuFast(int highThreshold, int lowThreshold, int maxKeypoints = 10000); 31 | ~GpuFast(); 32 | 33 | void detect(InputArray, std::vector&); 34 | 35 | void detectAsync(InputArray); 36 | void joinDetectAsync(std::vector&); 37 | }; 38 | 39 | class IC_Angle { 40 | unsigned int maxKeypoints; 41 | KeyPoint * keypoints; 42 | cudaStream_t stream; 43 | Stream _cvStream; 44 | public: 45 | IC_Angle(unsigned int maxKeypoints = 10000); 46 | ~IC_Angle(); 47 | void launch_async(InputArray _image, KeyPoint * _keypoints, int npoints, int half_k, int minBorderX, int minBorderY, int octave, int size); 48 | void join(KeyPoint * _keypoints, int npoints); 49 | 50 | Stream& cvStream() { return _cvStream;} 51 | static void loadUMax(const int* u_max, int count); 52 | }; 53 | } } 54 | #endif 55 | -------------------------------------------------------------------------------- /code/include/cuda/Orb.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef __ORB_HPP__ 3 | #define __ORB_HPP__ 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace ORB_SLAM2 { namespace cuda { 12 | using namespace std; 13 | using namespace cv; 14 | using namespace cv::cuda; 15 | 16 | class GpuOrb { 17 | unsigned int maxKeypoints; 18 | KeyPoint * keypoints; 19 | GpuMat descriptors; 20 | GpuMat desc; 21 | cudaStream_t stream; 22 | Stream cvStream; 23 | public: 24 | GpuOrb(int maxKeypoints = 10000); 25 | ~GpuOrb(); 26 | 27 | void launch_async(InputArray _image, const KeyPoint * _keypoints, const int npoints); 28 | void join(Mat &_descriptors); 29 | 30 | static void loadPattern(const Point * _pattern); 31 | }; 32 | } } 33 | #endif 34 | -------------------------------------------------------------------------------- /code/include/g2o/EdgeSim3RelativeXYZ.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Halcao on 2020/6/9. 3 | // 4 | 5 | #ifndef EDGE_SLAM_EDGESIM3RELATIVEXYZ_H 6 | #define EDGE_SLAM_EDGESIM3RELATIVEXYZ_H 7 | #include "Thirdparty/g2o/g2o/core/base_vertex.h" 8 | #include "Thirdparty/g2o/g2o/core/base_binary_edge.h" 9 | #include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" 10 | #include "Thirdparty/g2o/g2o/types/sim3.h" 11 | 12 | namespace g2o { 13 | class EdgeSim3RelativeXYZ : public BaseBinaryEdge<3, Vector3d, VertexSBAPointXYZ, VertexSim3Expmap> 14 | { 15 | public: 16 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 17 | EdgeSim3RelativeXYZ(): BaseBinaryEdge<3, Vector3d, VertexSBAPointXYZ, VertexSim3Expmap>() 18 | { 19 | }; 20 | virtual bool read(std::istream& is) {}; 21 | virtual bool write(std::ostream& os) const {}; 22 | 23 | void computeError() 24 | { 25 | const VertexSim3Expmap* v1 = static_cast(_vertices[1]); 26 | const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]); 27 | 28 | Vector3d obs(_measurement); 29 | _error = obs-v1->estimate().map(v2->estimate()); 30 | } 31 | 32 | // virtual void linearizeOplus(); 33 | }; 34 | 35 | } // end namespace 36 | 37 | #endif //EDGE_SLAM_EDGESIM3RELATIVEXYZ_H 38 | -------------------------------------------------------------------------------- /code/src/DataSetUtil.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Halcao on 2020/5/17. 3 | // 4 | 5 | #include "DataSetUtil.h" 6 | #include 7 | #include 8 | #include 9 | 10 | namespace ORB_SLAM2 { 11 | void DataSetUtil::LoadEuRoC(const std::string &strImagePath, const std::string &strPathTimes, 12 | std::vector &vstrImages, std::vector &vTimeStamps) { 13 | std::ifstream fTimes; 14 | fTimes.open(strPathTimes.c_str()); 15 | vTimeStamps.reserve(5000); 16 | vstrImages.reserve(5000); 17 | while (!fTimes.eof()) { 18 | std::string s; 19 | getline(fTimes, s); 20 | if (!s.empty()) { 21 | std::stringstream ss; 22 | ss << s; 23 | vstrImages.push_back(strImagePath + "/" + ss.str() + ".png"); 24 | double t; 25 | ss >> t; 26 | vTimeStamps.push_back(t / 1e9); 27 | } 28 | } 29 | } 30 | 31 | void DataSetUtil::LoadTUM(const std::string &path, std::vector &vstrImageFilenames, 32 | std::vector &vTimestamps) { 33 | std::string strFile = path + "/rgb.txt"; 34 | std::ifstream f; 35 | f.open(strFile.c_str()); 36 | 37 | // skip first three lines 38 | std::string s0; 39 | getline(f, s0); 40 | getline(f, s0); 41 | getline(f, s0); 42 | 43 | std::string filename; 44 | while (!f.eof()) { 45 | std::string s; 46 | getline(f, s); 47 | if (!s.empty()) { 48 | std::stringstream ss; 49 | ss << s; 50 | double t; 51 | std::string sRGB; 52 | ss >> t; 53 | vTimestamps.push_back(t); 54 | ss >> sRGB; 55 | filename = path + "/" + sRGB; 56 | vstrImageFilenames.push_back(filename); 57 | } 58 | } 59 | } 60 | 61 | void DataSetUtil::LoadKITTI(const std::string &path, std::vector &vstrImageFilenames, 62 | std::vector &vTimestamps) { 63 | std::ifstream fTimes; 64 | std::string strPathTimeFile = path + "/times.txt"; 65 | fTimes.open(strPathTimeFile.c_str()); 66 | while (!fTimes.eof()) { 67 | std::string s; 68 | getline(fTimes, s); 69 | if (!s.empty()) { 70 | std::stringstream ss; 71 | ss << s; 72 | double t; 73 | ss >> t; 74 | vTimestamps.push_back(t); 75 | } 76 | } 77 | 78 | std::string strPrefixLeft = path + "/image_0/"; 79 | 80 | const int nTimes = vTimestamps.size(); 81 | vstrImageFilenames.resize(nTimes); 82 | 83 | for (int i = 0; i < nTimes; i++) { 84 | std::stringstream ss; 85 | ss << std::setfill('0') << std::setw(6) << i; 86 | vstrImageFilenames[i] = strPrefixLeft + ss.str() + ".png"; 87 | } 88 | } 89 | } -------------------------------------------------------------------------------- /code/src/MapSlice.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Halcao on 2020/7/2. 3 | // 4 | 5 | #include "MapSlice.h" 6 | 7 | ORB_SLAM2::MapSlice::MapSlice(const std::vector &keyframes, const std::vector &mappoints, 8 | const std::vector &updates) : KFs(keyframes), 9 | MPs(mappoints), updates(updates) {} 10 | -------------------------------------------------------------------------------- /code/src/Network/MapElementUpdate.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Halcao on 2020/4/22. 3 | // 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "MapElementUpdate.h" 9 | 10 | namespace ORB_SLAM2 { 11 | unsigned long MapElementUpdateBase::curId = 0; 12 | 13 | } 14 | 15 | -------------------------------------------------------------------------------- /code/src/Network/WebSocket.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Halcao on 2022/4/3. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include "WebSocket.h" 13 | #include "BoostArchiver.h" 14 | 15 | namespace ORB_SLAM2 { 16 | std::string Request::toString() const { 17 | std::stringstream out; 18 | boost::archive::text_oarchive oa(out); 19 | // boost::archive::binary_oarchive oa(out); 20 | // start to serialize 21 | try { 22 | oa << this; 23 | } catch (boost::archive::archive_exception &e) { 24 | } 25 | 26 | return out.str(); 27 | } 28 | 29 | namespace WS { 30 | // Report a failure 31 | void 32 | fail(boost::system::error_code ec, char const *what) { 33 | std::cerr << what << ": " << ec.message() << "\n"; 34 | } 35 | namespace Client { 36 | 37 | 38 | } 39 | 40 | 41 | namespace Server { 42 | 43 | // broadcast a message to all websocket client sessions 44 | void shared_state::send(const std::shared_ptr& ss) { 45 | // make a local list of all the weak pointers representing the sessions so that we an do the 46 | // actual sending without holding the mutex 47 | auto v = std::vector(); 48 | { 49 | std::lock_guard lock(mutex); 50 | v.reserve(sessions.size()); 51 | for (auto p : sessions) { 52 | v.emplace_back(p); 53 | } 54 | } 55 | 56 | // for each session in our local list, try to acquire a strong pointer. If successful, 57 | // then send the message on that session 58 | for (const auto& wp : v) { 59 | if (!wp) continue; 60 | 61 | // std::cout << "Sent message: " << *ss << std::endl; 62 | wp->send(ss); 63 | } 64 | }; 65 | } 66 | 67 | } 68 | } -------------------------------------------------------------------------------- /code/src/STS.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Halcao on 2022/2/28. 3 | // 4 | 5 | #include "STS.h" 6 | #include "System.h" 7 | #include "SystemState.h" 8 | 9 | namespace ORB_SLAM2 { 10 | 11 | void STS::ReportState() { 12 | } 13 | 14 | //void STS::SetConnectionService(ConnectionService *connectionService) { 15 | // this->mpConnectionService = connectionService; 16 | //} 17 | 18 | } -------------------------------------------------------------------------------- /code/src/Timer.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Halcao on 2020/5/26. 3 | // 4 | 5 | #include 6 | #include "Timer.h" 7 | 8 | namespace ORB_SLAM2 { 9 | Timer::Timer(std::string funcName_, bool _logTime) : 10 | logTime(_logTime), funcName(std::move(funcName_)), start(std::chrono::steady_clock::now()) {} 11 | 12 | Timer &Timer::globalInstance() { 13 | static Timer timer; 14 | timer.logTime = false; 15 | return timer; 16 | } 17 | 18 | double Timer::get() { 19 | std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); 20 | return std::chrono::duration_cast >(end - start).count(); 21 | } 22 | 23 | Timer::~Timer() { 24 | if (logTime) { 25 | if (isInfoLog) { 26 | info("{} track time: {}ms", funcName, get() * 1000); 27 | } else { 28 | trace("{} track time: {}ms", funcName, get() * 1000); 29 | } 30 | } 31 | } 32 | 33 | void Timer::setInfoLog(bool isInfoLog_) { 34 | this->isInfoLog = isInfoLog_; 35 | } 36 | }; -------------------------------------------------------------------------------- /code/src/cuda/Allocator_gpu.cu: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | namespace ORB_SLAM2 { namespace cuda { 6 | 7 | size_t Allocator::getPitch(size_t widthSize){ 8 | return 128 + widthSize - widthSize%128; 9 | } 10 | 11 | bool Allocator::allocate(cv::cuda::GpuMat* mat, int rows, int cols, size_t elemSize) 12 | { 13 | if (rows > 1 && cols > 1) 14 | { 15 | mat->step = getPitch(elemSize * cols); 16 | checkCudaErrors(cudaMallocManaged(&mat->data, mat->step * rows)); 17 | } 18 | else 19 | { 20 | // Single row or single column must be continuous 21 | checkCudaErrors(cudaMallocManaged(&mat->data, elemSize * cols * rows)); 22 | mat->step = elemSize * cols; 23 | } 24 | 25 | mat->refcount = (int*) new int(); 26 | 27 | return true; 28 | } 29 | 30 | void Allocator::free(cv::cuda::GpuMat* mat) 31 | { 32 | checkCudaErrors(cudaFree(mat->datastart)); 33 | delete mat->refcount; 34 | } 35 | 36 | cv::cuda::GpuMat::Allocator * gpu_mat_allocator; 37 | 38 | } } 39 | 40 | 41 | namespace { 42 | using namespace ORB_SLAM2; 43 | 44 | void __attribute__((constructor)) init() { 45 | // Setup GPU Memory Management 46 | cuda::gpu_mat_allocator = new cuda::Allocator(); 47 | // cv::cuda::GpuMat::setDefaultAllocator(cuda::gpu_mat_allocator); 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /code/src/cuda/Cuda.cu: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace ORB_SLAM2 { namespace cuda { 5 | void deviceSynchronize() { 6 | checkCudaErrors( cudaDeviceSynchronize() ); 7 | } 8 | } } 9 | -------------------------------------------------------------------------------- /config/fr2-desk.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | TYPE: 'tum' 4 | SETTING: '/Examples/Monocular/TUM2.yaml' 5 | IMAGES: ['/rgbd_dataset_freiburg2_desk'] 6 | -------------------------------------------------------------------------------- /config/fr2-large12.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | TYPE: 'tum' 4 | SETTING: '/Examples/Monocular/TUM2.yaml' 5 | IMAGES: ['/rgbd_dataset_freiburg2_large_no_loop', '/rgbd_dataset_freiburg2_large_with_loop'] 6 | -------------------------------------------------------------------------------- /config/kitti00-02.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | TYPE: 'kitti' 4 | SETTING: '/Examples/Monocular/KITTI00-02.yaml' 5 | IMAGES: ['/kitti/sequences/00', '/kitti/sequences/01', '/kitti/sequences/02'] 6 | -------------------------------------------------------------------------------- /config/mh1.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | TYPE: 'euroc' 4 | SETTING: '/Examples/Monocular/EuRoC.yaml' 5 | IMAGES: ['/MH_01_easy/mav0/cam0/data'] 6 | TIMES: ['/home/slam/edge-slam-new/src/Examples/Monocular/EuRoC_TimeStamps/MH01.txt'] 7 | 8 | # ID dispatch address and port 9 | HOST: '0.0.0.0' 10 | PORT: 10088 -------------------------------------------------------------------------------- /config/mh123.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | TYPE: 'euroc' 4 | SETTING: '/Examples/Monocular/EuRoC.yaml' 5 | IMAGES: ['/MH_01_easy/mav0/cam0/data', '/MH_02_easy/mav0/cam0/data', '/MH_03_medium/mav0/cam0/data'] 6 | TIMES: ['/Examples/Monocular/EuRoC_TimeStamps/MH01.txt', '/Examples/Monocular/EuRoC_TimeStamps/MH02.txt', '/Examples/Monocular/EuRoC_TimeStamps/MH03.txt'] -------------------------------------------------------------------------------- /config/mh12345.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | TYPE: 'euroc' 4 | SETTING: '/Examples/Monocular/EuRoC.yaml' 5 | IMAGES: ['/MH_01_easy/mav0/cam0/data', '/MH_02_easy/mav0/cam0/data', '/MH_03_medium/mav0/cam0/data', '/MH_04_difficult/mav0/cam0/data', '/MH_05_difficult/mav0/cam0/data'] 6 | TIMES: ['/Examples/Monocular/EuRoC_TimeStamps/MH01.txt', '/Examples/Monocular/EuRoC_TimeStamps/MH02.txt', '/Examples/Monocular/EuRoC_TimeStamps/MH03.txt', '/Examples/Monocular/EuRoC_TimeStamps/MH04.txt', '/Examples/Monocular/EuRoC_TimeStamps/MH05.txt'] -------------------------------------------------------------------------------- /config/mh2.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | TYPE: 'euroc' 4 | SETTING: '/Examples/Monocular/EuRoC.yaml' 5 | IMAGES: ['/MH_02_easy/mav0/cam0/data'] 6 | TIMES: ['/home/slam/edge-slam-new/src/Examples/Monocular/EuRoC_TimeStamps/MH02.txt'] 7 | 8 | # ID dispatch address and port 9 | HOST: '0.0.0.0' 10 | PORT: 10088 -------------------------------------------------------------------------------- /config/v1-123.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | TYPE: 'euroc' 4 | SETTING: '/Examples/Monocular/EuRoC.yaml' 5 | IMAGES: ['/V1_01_easy/mav0/cam0/data', '/V1_02_medium/mav0/cam0/data', '/V1_03_difficult/mav0/cam0/data'] 6 | TIMES: ['/Examples/Monocular/EuRoC_TimeStamps/V101.txt', '/Examples/Monocular/EuRoC_TimeStamps/V102.txt', '/Examples/Monocular/EuRoC_TimeStamps/V103.txt'] -------------------------------------------------------------------------------- /config/v2-123.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | TYPE: 'euroc' 4 | SETTING: '/Examples/Monocular/EuRoC.yaml' 5 | IMAGES: ['/V2_01_easy/mav0/cam0/data', '/V2_02_medium/mav0/cam0/data', '/V2_03_difficult/mav0/cam0/data'] 6 | TIMES: ['/Examples/Monocular/EuRoC_TimeStamps/V201.txt', '/Examples/Monocular/EuRoC_TimeStamps/V202.txt', '/Examples/Monocular/EuRoC_TimeStamps/V203.txt'] --------------------------------------------------------------------------------