├── .gitignore ├── CMakeLists.txt ├── Dependencies.md ├── Examples ├── Monocular │ ├── EuRoC.yaml │ ├── EuRoC_TimeStamps │ │ ├── MH01.txt │ │ ├── MH02.txt │ │ ├── MH03.txt │ │ ├── MH04.txt │ │ ├── MH05.txt │ │ ├── V101.txt │ │ ├── V102.txt │ │ ├── V103.txt │ │ ├── V201.txt │ │ ├── V202.txt │ │ └── V203.txt │ ├── KITTI00-02.yaml │ ├── KITTI03.yaml │ ├── KITTI04-12.yaml │ ├── TUM1.yaml │ ├── TUM2.yaml │ ├── TUM3.yaml │ ├── mono_euroc.cc │ ├── mono_kitti.cc │ └── mono_tum.cc ├── PL │ ├── EuRoC_TimeStamps │ │ ├── MH01.txt │ │ ├── MH02.txt │ │ ├── MH03.txt │ │ ├── MH04.txt │ │ ├── MH05.txt │ │ ├── V101.txt │ │ ├── V102.txt │ │ ├── V103.txt │ │ ├── V201.txt │ │ ├── V202.txt │ │ └── V203.txt │ ├── PL_EuRoC.yaml │ ├── PL_KITTI00-02.yaml │ ├── PL_KITTI03.yaml │ ├── PL_KITTI04-12.yaml │ ├── PL_stereo_euroc.cc │ ├── PL_stereo_kitti.cc │ ├── ZED_HD1080.yaml │ ├── ZED_HD720.yaml │ └── ZED_VGA.yaml ├── PlaceRecognition │ ├── PR_EuRoC.yaml │ ├── PR_KITTI00-02.yaml │ ├── PR_KITTI03.yaml │ ├── PR_KITTI04-12.yaml │ ├── PR_stereo_kitti.cc │ └── placeRecognition.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_Line_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 │ │ └── ros_stereo_compressed.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 │ ├── 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 └── line_descriptor │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ ├── line_descriptor │ │ └── descriptor_custom.hpp │ └── line_descriptor_custom.hpp │ └── src │ ├── LSDDetector_custom.cpp │ ├── binary_descriptor_custom.cpp │ ├── binary_descriptor_matcher.cpp │ ├── bitarray_custom.hpp │ ├── bitops_custom.hpp │ ├── draw_custom.cpp │ ├── precomp_custom.hpp │ └── types_custom.hpp ├── Vocabulary ├── LSvoc.txt.tar.gz └── ORBvoc.txt.tar.gz ├── build.sh ├── build_ros.sh ├── cmake_modules └── FindEigen3.cmake ├── include ├── Auxiliar.h ├── Config.h ├── Converter.h ├── Frame.h ├── FrameDrawer.h ├── InitKeyFrame.h ├── Initializer.h ├── KDTree.h ├── KeyFrame.h ├── KeyFrameDatabase.h ├── LineExtractor.h ├── LineIterator.h ├── LineMatcher.h ├── LocalMapping.h ├── LoopClosing.h ├── Map.h ├── MapDrawer.h ├── MapLine.h ├── MapPoint.h ├── ORBVocabulary.h ├── ORBextractor.h ├── ORBmatcher.h ├── Optimizer.h ├── PnPsolver.h ├── Sim3Solver.h ├── System.h ├── SystemSetting.h ├── Tracking.h ├── Viewer.h └── gridStructure.h ├── snapshot1.jpg └── src ├── Auxiliar.cc ├── Config.cpp ├── Converter.cc ├── Frame.cc ├── FrameDrawer.cc ├── InitKeyFrame.cc ├── Initializer.cc ├── KeyFrame.cc ├── KeyFrameDatabase.cc ├── LineExtractor.cc ├── LineIterator.cpp ├── LineMatcher.cpp ├── LocalMapping.cc ├── LoopClosing.cc ├── Map.cc ├── MapDrawer.cc ├── MapLine.cc ├── MapPoint.cc ├── ORBextractor.cc ├── ORBmatcher.cc ├── Optimizer.cc ├── PnPsolver.cc ├── Sim3Solver.cc ├── System.cc ├── SystemSetting.cc ├── Tracking.cc ├── Viewer.cc └── gridStructure.cpp /.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/PL/PL_stereo_euroc 7 | Examples/PL/PL_stereo_kitti 8 | Examples/ROS/ORB_SLAM2/CMakeLists.txt.user 9 | Examples/ROS/ORB_SLAM2/Mono 10 | Examples/ROS/ORB_SLAM2/MonoAR 11 | Examples/ROS/ORB_SLAM2/RGBD 12 | Examples/ROS/ORB_SLAM2/Stereo 13 | Examples/ROS/ORB_SLAM2/Stereo_compressed 14 | Examples/ROS/ORB_SLAM2/build/ 15 | Examples/ROS/ORB_Line_SLAM2/CMakeLists.txt.user 16 | Examples/ROS/ORB_Line_SLAM2/Mono 17 | Examples/ROS/ORB_Line_SLAM2/MonoAR 18 | Examples/ROS/ORB_Line_SLAM2/RGBD 19 | Examples/ROS/ORB_Line_SLAM2/Stereo 20 | Examples/ROS/ORB_Line_SLAM2/Stereo_compressed 21 | Examples/ROS/ORB_Line_SLAM2/build/ 22 | Examples/Stereo/stereo_euroc 23 | Examples/Stereo/stereo_kitti 24 | Examples/PlaceRecognition/placeRecognition 25 | KeyFrameTrajectory.txt 26 | KeyFrameTrajectory_TUM_Format.txt 27 | FrameTrajectory_TUM_Format.txt 28 | FrameTrajectory_KITTI_Format.txt 29 | CameraTrajectory.txt 30 | Thirdparty/DBoW2/build/ 31 | Thirdparty/DBoW2/lib/ 32 | Thirdparty/g2o/build/ 33 | Thirdparty/g2o/config.h 34 | Thirdparty/g2o/lib/ 35 | Thirdparty/line_descriptor/lib/ 36 | Thirdparty/line_descriptor/build/ 37 | Vocabulary/ORBvoc.txt 38 | Vocabulary/LSDvoc.txt 39 | Vocabulary/FLDvoc.txt 40 | Vocabulary/mapir_fld.yml 41 | Vocabulary/mapir_lsd.yml 42 | Vocabulary/mapir_orb.yml 43 | Vocabulary/PLvoc.tar.gz 44 | build/ 45 | .vscode/ 46 | *~ 47 | lib/ 48 | 49 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /Examples/Monocular/EuRoC.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 458.654 9 | Camera.fy: 457.296 10 | Camera.cx: 367.215 11 | Camera.cy: 248.375 12 | 13 | Camera.k1: -0.28340811 14 | Camera.k2: 0.07395907 15 | Camera.p1: 0.00019359 16 | Camera.p2: 1.76187114e-05 17 | 18 | # Camera frames per second 19 | Camera.fps: 20.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 1000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #--------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.05 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 0.9 50 | Viewer.PointSize:2 51 | Viewer.CameraSize: 0.08 52 | Viewer.CameraLineWidth: 3 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -0.7 55 | Viewer.ViewpointZ: -1.8 56 | Viewer.ViewpointF: 500 57 | 58 | -------------------------------------------------------------------------------- /Examples/Monocular/KITTI00-02.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 718.856 9 | Camera.fy: 718.856 10 | Camera.cx: 607.1928 11 | Camera.cy: 185.2157 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | # Camera frames per second 19 | Camera.fps: 10.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 2000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #-------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.1 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 1 50 | Viewer.PointSize:2 51 | Viewer.CameraSize: 0.15 52 | Viewer.CameraLineWidth: 2 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -10 55 | Viewer.ViewpointZ: -0.1 56 | Viewer.ViewpointF: 2000 57 | 58 | -------------------------------------------------------------------------------- /Examples/Monocular/KITTI03.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 721.5377 9 | Camera.fy: 721.5377 10 | Camera.cx: 609.5593 11 | Camera.cy: 172.854 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | # Camera frames per second 19 | Camera.fps: 10.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 2000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #-------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.1 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 1 50 | Viewer.PointSize:2 51 | Viewer.CameraSize: 0.15 52 | Viewer.CameraLineWidth: 2 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -10 55 | Viewer.ViewpointZ: -0.1 56 | Viewer.ViewpointF: 2000 57 | 58 | -------------------------------------------------------------------------------- /Examples/Monocular/KITTI04-12.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 707.0912 9 | Camera.fy: 707.0912 10 | Camera.cx: 601.8873 11 | Camera.cy: 183.1104 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | # Camera frames per second 19 | Camera.fps: 10.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 2000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #-------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.1 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 1 50 | Viewer.PointSize:2 51 | Viewer.CameraSize: 0.15 52 | Viewer.CameraLineWidth: 2 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -10 55 | Viewer.ViewpointZ: -0.1 56 | Viewer.ViewpointF: 2000 57 | 58 | -------------------------------------------------------------------------------- /Examples/Monocular/TUM1.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 517.306408 9 | Camera.fy: 516.469215 10 | Camera.cx: 318.643040 11 | Camera.cy: 255.313989 12 | 13 | Camera.k1: 0.262383 14 | Camera.k2: -0.953104 15 | Camera.p1: -0.005358 16 | Camera.p2: 0.002628 17 | Camera.k3: 1.163314 18 | 19 | # Camera frames per second 20 | Camera.fps: 30.0 21 | 22 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 23 | Camera.RGB: 1 24 | 25 | #-------------------------------------------------------------------------------------------- 26 | # ORB Parameters 27 | #-------------------------------------------------------------------------------------------- 28 | 29 | # ORB Extractor: Number of features per image 30 | ORBextractor.nFeatures: 1000 31 | 32 | # ORB Extractor: Scale factor between levels in the scale pyramid 33 | ORBextractor.scaleFactor: 1.2 34 | 35 | # ORB Extractor: Number of levels in the scale pyramid 36 | ORBextractor.nLevels: 8 37 | 38 | # ORB Extractor: Fast threshold 39 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 40 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 41 | # You can lower these values if your images have low contrast 42 | ORBextractor.iniThFAST: 20 43 | ORBextractor.minThFAST: 7 44 | 45 | #-------------------------------------------------------------------------------------------- 46 | # Viewer Parameters 47 | #-------------------------------------------------------------------------------------------- 48 | Viewer.KeyFrameSize: 0.05 49 | Viewer.KeyFrameLineWidth: 1 50 | Viewer.GraphLineWidth: 0.9 51 | Viewer.PointSize:2 52 | Viewer.CameraSize: 0.08 53 | Viewer.CameraLineWidth: 3 54 | Viewer.ViewpointX: 0 55 | Viewer.ViewpointY: -0.7 56 | Viewer.ViewpointZ: -1.8 57 | Viewer.ViewpointF: 500 58 | 59 | -------------------------------------------------------------------------------- /Examples/Monocular/TUM2.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 520.908620 9 | Camera.fy: 521.007327 10 | Camera.cx: 325.141442 11 | Camera.cy: 249.701764 12 | 13 | Camera.k1: 0.231222 14 | Camera.k2: -0.784899 15 | Camera.p1: -0.003257 16 | Camera.p2: -0.000105 17 | Camera.k3: 0.917205 18 | 19 | # Camera frames per second 20 | Camera.fps: 30.0 21 | 22 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 23 | Camera.RGB: 1 24 | 25 | #-------------------------------------------------------------------------------------------- 26 | # ORB Parameters 27 | #-------------------------------------------------------------------------------------------- 28 | 29 | # ORB Extractor: Number of features per image 30 | ORBextractor.nFeatures: 1000 31 | 32 | # ORB Extractor: Scale factor between levels in the scale pyramid 33 | ORBextractor.scaleFactor: 1.2 34 | 35 | # ORB Extractor: Number of levels in the scale pyramid 36 | ORBextractor.nLevels: 8 37 | 38 | # ORB Extractor: Fast threshold 39 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 40 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 41 | # You can lower these values if your images have low contrast 42 | ORBextractor.iniThFAST: 20 43 | ORBextractor.minThFAST: 7 44 | 45 | #-------------------------------------------------------------------------------------------- 46 | # Viewer Parameters 47 | #-------------------------------------------------------------------------------------------- 48 | Viewer.KeyFrameSize: 0.05 49 | Viewer.KeyFrameLineWidth: 1 50 | Viewer.GraphLineWidth: 0.9 51 | Viewer.PointSize:2 52 | Viewer.CameraSize: 0.08 53 | Viewer.CameraLineWidth: 3 54 | Viewer.ViewpointX: 0 55 | Viewer.ViewpointY: -0.7 56 | Viewer.ViewpointZ: -1.8 57 | Viewer.ViewpointF: 500 58 | 59 | -------------------------------------------------------------------------------- /Examples/Monocular/TUM3.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 535.4 9 | Camera.fy: 539.2 10 | Camera.cx: 320.1 11 | Camera.cy: 247.6 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | # Camera frames per second 19 | Camera.fps: 30.0 20 | 21 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 22 | Camera.RGB: 1 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # ORB Parameters 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # ORB Extractor: Number of features per image 29 | ORBextractor.nFeatures: 1000 30 | 31 | # ORB Extractor: Scale factor between levels in the scale pyramid 32 | ORBextractor.scaleFactor: 1.2 33 | 34 | # ORB Extractor: Number of levels in the scale pyramid 35 | ORBextractor.nLevels: 8 36 | 37 | # ORB Extractor: Fast threshold 38 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 39 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 40 | # You can lower these values if your images have low contrast 41 | ORBextractor.iniThFAST: 20 42 | ORBextractor.minThFAST: 7 43 | 44 | #-------------------------------------------------------------------------------------------- 45 | # Viewer Parameters 46 | #-------------------------------------------------------------------------------------------- 47 | Viewer.KeyFrameSize: 0.05 48 | Viewer.KeyFrameLineWidth: 1 49 | Viewer.GraphLineWidth: 0.9 50 | Viewer.PointSize:2 51 | Viewer.CameraSize: 0.08 52 | Viewer.CameraLineWidth: 3 53 | Viewer.ViewpointX: 0 54 | Viewer.ViewpointY: -0.7 55 | Viewer.ViewpointZ: -1.8 56 | Viewer.ViewpointF: 500 57 | 58 | -------------------------------------------------------------------------------- /Examples/RGB-D/TUM1.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 517.306408 9 | Camera.fy: 516.469215 10 | Camera.cx: 318.643040 11 | Camera.cy: 255.313989 12 | 13 | Camera.k1: 0.262383 14 | Camera.k2: -0.953104 15 | Camera.p1: -0.005358 16 | Camera.p2: 0.002628 17 | Camera.k3: 1.163314 18 | 19 | Camera.width: 640 20 | Camera.height: 480 21 | 22 | # Camera frames per second 23 | Camera.fps: 30.0 24 | 25 | # IR projector baseline times fx (aprox.) 26 | Camera.bf: 40.0 27 | 28 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 29 | Camera.RGB: 1 30 | 31 | # Close/Far threshold. Baseline times. 32 | ThDepth: 40.0 33 | 34 | # Deptmap values factor 35 | DepthMapFactor: 5000.0 36 | 37 | #-------------------------------------------------------------------------------------------- 38 | # ORB Parameters 39 | #-------------------------------------------------------------------------------------------- 40 | 41 | # ORB Extractor: Number of features per image 42 | ORBextractor.nFeatures: 1000 43 | 44 | # ORB Extractor: Scale factor between levels in the scale pyramid 45 | ORBextractor.scaleFactor: 1.2 46 | 47 | # ORB Extractor: Number of levels in the scale pyramid 48 | ORBextractor.nLevels: 8 49 | 50 | # ORB Extractor: Fast threshold 51 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 52 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 53 | # You can lower these values if your images have low contrast 54 | ORBextractor.iniThFAST: 20 55 | ORBextractor.minThFAST: 7 56 | 57 | #-------------------------------------------------------------------------------------------- 58 | # Viewer Parameters 59 | #-------------------------------------------------------------------------------------------- 60 | Viewer.KeyFrameSize: 0.05 61 | Viewer.KeyFrameLineWidth: 1 62 | Viewer.GraphLineWidth: 0.9 63 | Viewer.PointSize:2 64 | Viewer.CameraSize: 0.08 65 | Viewer.CameraLineWidth: 3 66 | Viewer.ViewpointX: 0 67 | Viewer.ViewpointY: -0.7 68 | Viewer.ViewpointZ: -1.8 69 | Viewer.ViewpointF: 500 70 | 71 | -------------------------------------------------------------------------------- /Examples/RGB-D/TUM2.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 520.908620 9 | Camera.fy: 521.007327 10 | Camera.cx: 325.141442 11 | Camera.cy: 249.701764 12 | 13 | Camera.k1: 0.231222 14 | Camera.k2: -0.784899 15 | Camera.p1: -0.003257 16 | Camera.p2: -0.000105 17 | Camera.k3: 0.917205 18 | 19 | Camera.width: 640 20 | Camera.height: 480 21 | 22 | # Camera frames per second 23 | Camera.fps: 30.0 24 | 25 | # IR projector baseline times fx (aprox.) 26 | Camera.bf: 40.0 27 | 28 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 29 | Camera.RGB: 1 30 | 31 | # Close/Far threshold. Baseline times. 32 | ThDepth: 40.0 33 | 34 | # Deptmap values factor 35 | DepthMapFactor: 5208.0 36 | 37 | #-------------------------------------------------------------------------------------------- 38 | # ORB Parameters 39 | #-------------------------------------------------------------------------------------------- 40 | 41 | # ORB Extractor: Number of features per image 42 | ORBextractor.nFeatures: 1000 43 | 44 | # ORB Extractor: Scale factor between levels in the scale pyramid 45 | ORBextractor.scaleFactor: 1.2 46 | 47 | # ORB Extractor: Number of levels in the scale pyramid 48 | ORBextractor.nLevels: 8 49 | 50 | # ORB Extractor: Fast threshold 51 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 52 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 53 | # You can lower these values if your images have low contrast 54 | ORBextractor.iniThFAST: 20 55 | ORBextractor.minThFAST: 7 56 | 57 | #-------------------------------------------------------------------------------------------- 58 | # Viewer Parameters 59 | #-------------------------------------------------------------------------------------------- 60 | Viewer.KeyFrameSize: 0.05 61 | Viewer.KeyFrameLineWidth: 1 62 | Viewer.GraphLineWidth: 0.9 63 | Viewer.PointSize:2 64 | Viewer.CameraSize: 0.08 65 | Viewer.CameraLineWidth: 3 66 | Viewer.ViewpointX: 0 67 | Viewer.ViewpointY: -0.7 68 | Viewer.ViewpointZ: -1.8 69 | Viewer.ViewpointF: 500 70 | 71 | -------------------------------------------------------------------------------- /Examples/RGB-D/TUM3.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 535.4 9 | Camera.fy: 539.2 10 | Camera.cx: 320.1 11 | Camera.cy: 247.6 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 640 19 | Camera.height: 480 20 | 21 | # Camera frames per second 22 | Camera.fps: 30.0 23 | 24 | # IR projector baseline times fx (aprox.) 25 | Camera.bf: 40.0 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 40.0 32 | 33 | # Deptmap values factor 34 | DepthMapFactor: 5000.0 35 | 36 | #-------------------------------------------------------------------------------------------- 37 | # ORB Parameters 38 | #-------------------------------------------------------------------------------------------- 39 | 40 | # ORB Extractor: Number of features per image 41 | ORBextractor.nFeatures: 1000 42 | 43 | # ORB Extractor: Scale factor between levels in the scale pyramid 44 | ORBextractor.scaleFactor: 1.2 45 | 46 | # ORB Extractor: Number of levels in the scale pyramid 47 | ORBextractor.nLevels: 8 48 | 49 | # ORB Extractor: Fast threshold 50 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 51 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 52 | # You can lower these values if your images have low contrast 53 | ORBextractor.iniThFAST: 20 54 | ORBextractor.minThFAST: 7 55 | 56 | #-------------------------------------------------------------------------------------------- 57 | # Viewer Parameters 58 | #-------------------------------------------------------------------------------------------- 59 | Viewer.KeyFrameSize: 0.05 60 | Viewer.KeyFrameLineWidth: 1 61 | Viewer.GraphLineWidth: 0.9 62 | Viewer.PointSize:2 63 | Viewer.CameraSize: 0.08 64 | Viewer.CameraLineWidth: 3 65 | Viewer.ViewpointX: 0 66 | Viewer.ViewpointY: -0.7 67 | Viewer.ViewpointZ: -1.8 68 | Viewer.ViewpointF: 500 69 | 70 | -------------------------------------------------------------------------------- /Examples/ROS/ORB_Line_SLAM2/Asus.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 535.4 9 | Camera.fy: 539.2 10 | Camera.cx: 320.1 11 | Camera.cy: 247.6 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 640 19 | Camera.height: 480 20 | 21 | # Camera frames per second 22 | Camera.fps: 30.0 23 | 24 | # IR projector baseline times fx (aprox.) 25 | Camera.bf: 40.0 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 40.0 32 | 33 | # Deptmap values factor 34 | DepthMapFactor: 1.0 35 | 36 | #-------------------------------------------------------------------------------------------- 37 | # ORB Parameters 38 | #-------------------------------------------------------------------------------------------- 39 | 40 | # ORB Extractor: Number of features per image 41 | ORBextractor.nFeatures: 1000 42 | 43 | # ORB Extractor: Scale factor between levels in the scale pyramid 44 | ORBextractor.scaleFactor: 1.2 45 | 46 | # ORB Extractor: Number of levels in the scale pyramid 47 | ORBextractor.nLevels: 8 48 | 49 | # ORB Extractor: Fast threshold 50 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 51 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 52 | # You can lower these values if your images have low contrast 53 | ORBextractor.iniThFAST: 20 54 | ORBextractor.minThFAST: 7 55 | 56 | #-------------------------------------------------------------------------------------------- 57 | # Viewer Parameters 58 | #-------------------------------------------------------------------------------------------- 59 | Viewer.KeyFrameSize: 0.05 60 | Viewer.KeyFrameLineWidth: 1 61 | Viewer.GraphLineWidth: 0.9 62 | Viewer.PointSize:2 63 | Viewer.CameraSize: 0.08 64 | Viewer.CameraLineWidth: 3 65 | Viewer.ViewpointX: 0 66 | Viewer.ViewpointY: -0.7 67 | Viewer.ViewpointZ: -1.8 68 | Viewer.ViewpointF: 500 69 | 70 | -------------------------------------------------------------------------------- /Examples/ROS/ORB_Line_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 | find_package(Boost COMPONENTS system) 44 | 45 | include_directories( 46 | ${PROJECT_SOURCE_DIR} 47 | ${PROJECT_SOURCE_DIR}/../../../ 48 | ${PROJECT_SOURCE_DIR}/../../../include 49 | ${PROJECT_SOURCE_DIR}/../../../Thirdparty/line_descriptor/include/ 50 | ${Pangolin_INCLUDE_DIRS} 51 | ${Boost_INCLUDE_DIRS} 52 | ) 53 | 54 | set(LIBS 55 | ${OpenCV_LIBS} 56 | ${EIGEN3_LIBS} 57 | ${Pangolin_LIBRARIES} 58 | ${Boost_LIBRARIES} 59 | ${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so 60 | ${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so 61 | ${PROJECT_SOURCE_DIR}/../../../Thirdparty/line_descriptor/lib/liblinedesc.so 62 | ${PROJECT_SOURCE_DIR}/../../../lib/libORB_Line_SLAM2.so 63 | ) 64 | 65 | # Node for monocular camera 66 | rosbuild_add_executable(Mono 67 | src/ros_mono.cc 68 | ) 69 | 70 | target_link_libraries(Mono 71 | ${LIBS} 72 | ) 73 | 74 | # Node for monocular camera (Augmented Reality Demo) 75 | rosbuild_add_executable(MonoAR 76 | src/AR/ros_mono_ar.cc 77 | src/AR/ViewerAR.h 78 | src/AR/ViewerAR.cc 79 | ) 80 | 81 | target_link_libraries(MonoAR 82 | ${LIBS} 83 | ) 84 | 85 | # Node for stereo camera 86 | rosbuild_add_executable(Stereo 87 | src/ros_stereo.cc 88 | ) 89 | 90 | target_link_libraries(Stereo 91 | ${LIBS} 92 | ) 93 | 94 | # Node for RGB-D camera 95 | rosbuild_add_executable(RGBD 96 | src/ros_rgbd.cc 97 | ) 98 | 99 | target_link_libraries(RGBD 100 | ${LIBS} 101 | ) 102 | 103 | # Node for compressed stereo images 104 | rosbuild_add_executable(Stereo_compressed 105 | src/ros_stereo_compressed.cc 106 | ) 107 | 108 | target_link_libraries(Stereo_compressed 109 | ${LIBS} 110 | ) 111 | 112 | -------------------------------------------------------------------------------- /Examples/ROS/ORB_Line_SLAM2/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ORB_Line_SLAM2 5 | 6 | 7 | Wei Zhao 8 | GPLv3 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /Examples/ROS/ORB_Line_SLAM2/src/ros_mono.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | #include 31 | 32 | #include"../../../include/System.h" 33 | 34 | using namespace std; 35 | 36 | class ImageGrabber 37 | { 38 | public: 39 | ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){} 40 | 41 | void GrabImage(const sensor_msgs::ImageConstPtr& msg); 42 | 43 | ORB_SLAM2::System* mpSLAM; 44 | }; 45 | 46 | int main(int argc, char **argv) 47 | { 48 | ros::init(argc, argv, "Mono"); 49 | ros::start(); 50 | 51 | if(argc != 3) 52 | { 53 | cerr << endl << "Usage: rosrun ORB_SLAM2 Mono path_to_vocabulary path_to_settings" << endl; 54 | ros::shutdown(); 55 | return 1; 56 | } 57 | 58 | // Create SLAM system. It initializes all system threads and gets ready to process frames. 59 | ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true); 60 | 61 | ImageGrabber igb(&SLAM); 62 | 63 | ros::NodeHandle nodeHandler; 64 | ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb); 65 | 66 | ros::spin(); 67 | 68 | // Stop all threads 69 | SLAM.Shutdown(); 70 | 71 | // Save camera trajectory 72 | SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); 73 | 74 | ros::shutdown(); 75 | 76 | return 0; 77 | } 78 | 79 | void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg) 80 | { 81 | // Copy the ros image message to cv::Mat. 82 | cv_bridge::CvImageConstPtr cv_ptr; 83 | try 84 | { 85 | cv_ptr = cv_bridge::toCvShare(msg); 86 | } 87 | catch (cv_bridge::Exception& e) 88 | { 89 | ROS_ERROR("cv_bridge exception: %s", e.what()); 90 | return; 91 | } 92 | 93 | mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec()); 94 | } 95 | 96 | 97 | -------------------------------------------------------------------------------- /Examples/ROS/ORB_Line_SLAM2/src/ros_rgbd.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | #include"../../../include/System.h" 36 | 37 | using namespace std; 38 | 39 | class ImageGrabber 40 | { 41 | public: 42 | ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){} 43 | 44 | void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD); 45 | 46 | ORB_SLAM2::System* mpSLAM; 47 | }; 48 | 49 | int main(int argc, char **argv) 50 | { 51 | ros::init(argc, argv, "RGBD"); 52 | ros::start(); 53 | 54 | if(argc != 3) 55 | { 56 | cerr << endl << "Usage: rosrun ORB_SLAM2 RGBD path_to_vocabulary path_to_settings" << endl; 57 | ros::shutdown(); 58 | return 1; 59 | } 60 | 61 | // Create SLAM system. It initializes all system threads and gets ready to process frames. 62 | ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true); 63 | 64 | ImageGrabber igb(&SLAM); 65 | 66 | ros::NodeHandle nh; 67 | 68 | message_filters::Subscriber rgb_sub(nh, "/camera/rgb/image_raw", 1); 69 | message_filters::Subscriber depth_sub(nh, "camera/depth_registered/image_raw", 1); 70 | typedef message_filters::sync_policies::ApproximateTime sync_pol; 71 | message_filters::Synchronizer sync(sync_pol(10), rgb_sub,depth_sub); 72 | sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2)); 73 | 74 | ros::spin(); 75 | 76 | // Stop all threads 77 | SLAM.Shutdown(); 78 | 79 | // Save camera trajectory 80 | SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); 81 | 82 | ros::shutdown(); 83 | 84 | return 0; 85 | } 86 | 87 | void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD) 88 | { 89 | // Copy the ros image message to cv::Mat. 90 | cv_bridge::CvImageConstPtr cv_ptrRGB; 91 | try 92 | { 93 | cv_ptrRGB = cv_bridge::toCvShare(msgRGB); 94 | } 95 | catch (cv_bridge::Exception& e) 96 | { 97 | ROS_ERROR("cv_bridge exception: %s", e.what()); 98 | return; 99 | } 100 | 101 | cv_bridge::CvImageConstPtr cv_ptrD; 102 | try 103 | { 104 | cv_ptrD = cv_bridge::toCvShare(msgD); 105 | } 106 | catch (cv_bridge::Exception& e) 107 | { 108 | ROS_ERROR("cv_bridge exception: %s", e.what()); 109 | return; 110 | } 111 | 112 | mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec()); 113 | } 114 | 115 | 116 | -------------------------------------------------------------------------------- /Examples/Stereo/KITTI00-02.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 718.856 9 | Camera.fy: 718.856 10 | Camera.cx: 607.1928 11 | Camera.cy: 185.2157 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 1241 19 | Camera.height: 376 20 | 21 | # Camera frames per second 22 | Camera.fps: 10.0 23 | 24 | # stereo baseline times fx 25 | Camera.bf: 386.1448 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 35 32 | 33 | #-------------------------------------------------------------------------------------------- 34 | # ORB Parameters 35 | #-------------------------------------------------------------------------------------------- 36 | 37 | # ORB Extractor: Number of features per image 38 | ORBextractor.nFeatures: 2000 39 | 40 | # ORB Extractor: Scale factor between levels in the scale pyramid 41 | ORBextractor.scaleFactor: 1.2 42 | 43 | # ORB Extractor: Number of levels in the scale pyramid 44 | ORBextractor.nLevels: 8 45 | 46 | # ORB Extractor: Fast threshold 47 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 48 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 49 | # You can lower these values if your images have low contrast 50 | ORBextractor.iniThFAST: 20 51 | ORBextractor.minThFAST: 7 52 | 53 | #-------------------------------------------------------------------------------------------- 54 | # Viewer Parameters 55 | #-------------------------------------------------------------------------------------------- 56 | Viewer.KeyFrameSize: 0.6 57 | Viewer.KeyFrameLineWidth: 2 58 | Viewer.GraphLineWidth: 1 59 | Viewer.PointSize:2 60 | Viewer.CameraSize: 0.7 61 | Viewer.CameraLineWidth: 3 62 | Viewer.ViewpointX: 0 63 | Viewer.ViewpointY: -100 64 | Viewer.ViewpointZ: -0.1 65 | Viewer.ViewpointF: 2000 66 | 67 | -------------------------------------------------------------------------------- /Examples/Stereo/KITTI03.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 721.5377 9 | Camera.fy: 721.5377 10 | Camera.cx: 609.5593 11 | Camera.cy: 172.854 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 1241 19 | Camera.height: 376 20 | 21 | # Camera frames per second 22 | Camera.fps: 10.0 23 | 24 | # stereo baseline times fx 25 | Camera.bf: 387.5744 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 40 32 | 33 | #-------------------------------------------------------------------------------------------- 34 | # ORB Parameters 35 | #-------------------------------------------------------------------------------------------- 36 | 37 | # ORB Extractor: Number of features per image 38 | ORBextractor.nFeatures: 2000 39 | 40 | # ORB Extractor: Scale factor between levels in the scale pyramid 41 | ORBextractor.scaleFactor: 1.2 42 | 43 | # ORB Extractor: Number of levels in the scale pyramid 44 | ORBextractor.nLevels: 8 45 | 46 | # ORB Extractor: Fast threshold 47 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 48 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 49 | # You can lower these values if your images have low contrast 50 | ORBextractor.iniThFAST: 20 51 | ORBextractor.minThFAST: 7 52 | 53 | #-------------------------------------------------------------------------------------------- 54 | # Viewer Parameters 55 | #-------------------------------------------------------------------------------------------- 56 | Viewer.KeyFrameSize: 0.6 57 | Viewer.KeyFrameLineWidth: 2 58 | Viewer.GraphLineWidth: 1 59 | Viewer.PointSize:2 60 | Viewer.CameraSize: 0.7 61 | Viewer.CameraLineWidth: 3 62 | Viewer.ViewpointX: 0 63 | Viewer.ViewpointY: -100 64 | Viewer.ViewpointZ: -0.1 65 | Viewer.ViewpointF: 2000 66 | 67 | -------------------------------------------------------------------------------- /Examples/Stereo/KITTI04-12.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 707.0912 9 | Camera.fy: 707.0912 10 | Camera.cx: 601.8873 11 | Camera.cy: 183.1104 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 1241 19 | Camera.height: 376 20 | 21 | # Camera frames per second 22 | Camera.fps: 10.0 23 | 24 | # stereo baseline times fx 25 | Camera.bf: 379.8145 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 40 32 | 33 | #-------------------------------------------------------------------------------------------- 34 | # ORB Parameters 35 | #-------------------------------------------------------------------------------------------- 36 | 37 | # ORB Extractor: Number of features per image 38 | ORBextractor.nFeatures: 2000 39 | 40 | # ORB Extractor: Scale factor between levels in the scale pyramid 41 | ORBextractor.scaleFactor: 1.2 42 | 43 | # ORB Extractor: Number of levels in the scale pyramid 44 | ORBextractor.nLevels: 8 45 | 46 | # ORB Extractor: Fast threshold 47 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 48 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 49 | # You can lower these values if your images have low contrast 50 | ORBextractor.iniThFAST: 12 51 | ORBextractor.minThFAST: 7 52 | 53 | #-------------------------------------------------------------------------------------------- 54 | # Viewer Parameters 55 | #-------------------------------------------------------------------------------------------- 56 | Viewer.KeyFrameSize: 0.6 57 | Viewer.KeyFrameLineWidth: 2 58 | Viewer.GraphLineWidth: 1 59 | Viewer.PointSize:2 60 | Viewer.CameraSize: 0.7 61 | Viewer.CameraLineWidth: 3 62 | Viewer.ViewpointX: 0 63 | Viewer.ViewpointY: -100 64 | Viewer.ViewpointZ: -0.1 65 | Viewer.ViewpointF: 2000 66 | 67 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | #ORB_Line_SLAM 2 | 3 | ORB_line_SLAM is an extension to the classic ORB_SLAM2 that make use of both point and line features. 4 | 5 | Abstract: 6 | ------------ 7 | 8 |
9 | 10 | 11 | The combination of wearable sensors and Augmented Reality (AR) technology has shown promising prospects in assisting substation inspection, especially when the tasks are infrequent and unfamiliar to human operators. In this paper, a wearable-assisted system (WAS) is proposed for task-specific global navigation and position-aware workflow guidance using a head-mounted stereo camera. The key of our proposal is an extended stereo visual SLAM approach is proposed with Bags of Point and Line Word (BoPLW) pairs, which ensures more accurate and robust dynamic ego-localization performance against several challenges that are typical in substations, such as similar facilities, low-textured scenes and lighting changes. In addition, the paper proposes to learn a global Metric-Topological Abstract map (MTA-map) for supporting map-based re-localization and navigation guidance. A prototype of WAS is developed in which augmentations of guidance are delivered to users via an optical see-through display. The system is evaluated in field experiments designed as an substation inspection scenario and other similar industrial inspection applications. The results validate the practicability and effectiveness of the system. Our code is available at https://github.com/robotseu/ORB_Line_SLAM. 12 | 13 | Citation: 14 | ------------ 15 | K. Qian, W. Zhao, K. Li, X. Ma and H. Yu, "Visual SLAM with BoPLW Pairs using Egocentric Stereo Camera for Wearable-assisted Substation Inspection," in IEEE Sensors Journal. doi: 10.1109/JSEN.2019.2947275. [PDF](https://ieeexplore.ieee.org/document/8868098) 16 | 17 | @article{Qian2019,
18 | title = {Visual SLAM with BoPLW Pairs using Egocentric Stereo Camera for Wearable-assisted Substation Inspection},
19 | journal = {IEEE Sensors Journal},
20 | author = {Qian, Kun and Zhao, Wei and Li, Kai and Ma, Xudong and Yu, Hai},
21 | volume = {doi: 10.1109/JSEN.2019.2947275},
22 | year = {2019}
23 | }
24 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(DBoW2) 3 | 4 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 6 | 7 | set(HDRS_DBOW2 8 | DBoW2/BowVector.h 9 | DBoW2/FORB.h 10 | DBoW2/FClass.h 11 | DBoW2/FeatureVector.h 12 | DBoW2/ScoringObject.h 13 | DBoW2/TemplatedVocabulary.h) 14 | set(SRCS_DBOW2 15 | DBoW2/BowVector.cpp 16 | DBoW2/FORB.cpp 17 | DBoW2/FeatureVector.cpp 18 | DBoW2/ScoringObject.cpp) 19 | 20 | set(HDRS_DUTILS 21 | DUtils/Random.h 22 | DUtils/Timestamp.h) 23 | set(SRCS_DUTILS 24 | DUtils/Random.cpp 25 | DUtils/Timestamp.cpp) 26 | 27 | find_package(OpenCV 3.0 QUIET) 28 | if(NOT OpenCV_FOUND) 29 | find_package(OpenCV 2.4.3 QUIET) 30 | if(NOT OpenCV_FOUND) 31 | message(FATAL_ERROR "OpenCV > 2.4.3 not found.") 32 | endif() 33 | endif() 34 | 35 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 36 | 37 | include_directories(${OpenCV_INCLUDE_DIRS}) 38 | add_library(DBoW2 SHARED ${SRCS_DBOW2} ${SRCS_DUTILS}) 39 | target_link_libraries(DBoW2 ${OpenCV_LIBS}) 40 | 41 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | namespace DBoW2 { 18 | 19 | /// Id of words 20 | typedef unsigned int WordId; 21 | 22 | /// Value of a word 23 | typedef double WordValue; 24 | 25 | /// Id of nodes in the vocabulary treee 26 | typedef unsigned int NodeId; 27 | 28 | /// L-norms for normalization 29 | enum LNorm 30 | { 31 | L1, 32 | L2 33 | }; 34 | 35 | /// Weighting type 36 | enum WeightingType 37 | { 38 | TF_IDF, 39 | TF, 40 | IDF, 41 | BINARY 42 | }; 43 | 44 | /// Scoring type 45 | enum ScoringType 46 | { 47 | L1_NORM, 48 | L2_NORM, 49 | CHI_SQUARE, 50 | KL, 51 | BHATTACHARYYA, 52 | DOT_PRODUCT, 53 | }; 54 | 55 | /// Vector of words to represent images 56 | class BowVector: 57 | public std::map 58 | { 59 | public: 60 | 61 | /** 62 | * Constructor 63 | */ 64 | BowVector(void); 65 | 66 | /** 67 | * Destructor 68 | */ 69 | ~BowVector(void); 70 | 71 | /** 72 | * Adds a value to a word value existing in the vector, or creates a new 73 | * word with the given value 74 | * @param id word id to look for 75 | * @param v value to create the word with, or to add to existing word 76 | */ 77 | void addWeight(WordId id, WordValue v); 78 | 79 | /** 80 | * Adds a word with a value to the vector only if this does not exist yet 81 | * @param id word id to look for 82 | * @param v value to give to the word if this does not exist 83 | */ 84 | void addIfNotExist(WordId id, WordValue v); 85 | 86 | /** 87 | * L1-Normalizes the values in the vector 88 | * @param norm_type norm used 89 | */ 90 | void normalize(LNorm norm_type); 91 | 92 | /** 93 | * Prints the content of the bow vector 94 | * @param out stream 95 | * @param v 96 | */ 97 | friend std::ostream& operator<<(std::ostream &out, const BowVector &v); 98 | 99 | /** 100 | * Saves the bow vector as a vector in a matlab file 101 | * @param filename 102 | * @param W number of words in the vocabulary 103 | */ 104 | void saveM(const std::string &filename, size_t W) const; 105 | }; 106 | 107 | } // namespace DBoW2 108 | 109 | #endif 110 | -------------------------------------------------------------------------------- /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 | namespace DBoW2 { 18 | 19 | /// Generic class to encapsulate functions to manage descriptors. 20 | /** 21 | * This class must be inherited. Derived classes can be used as the 22 | * parameter F when creating Templated structures 23 | * (TemplatedVocabulary, TemplatedDatabase, ...) 24 | */ 25 | class FClass 26 | { 27 | class TDescriptor; 28 | typedef const TDescriptor *pDescriptor; 29 | 30 | /** 31 | * Calculates the mean value of a set of descriptors 32 | * @param descriptors 33 | * @param mean mean descriptor 34 | */ 35 | virtual void meanValue(const std::vector &descriptors, 36 | TDescriptor &mean) = 0; 37 | 38 | /** 39 | * Calculates the distance between two descriptors 40 | * @param a 41 | * @param b 42 | * @return distance 43 | */ 44 | static double distance(const TDescriptor &a, const TDescriptor &b); 45 | 46 | /** 47 | * Returns a string version of the descriptor 48 | * @param a descriptor 49 | * @return string version 50 | */ 51 | static std::string toString(const TDescriptor &a); 52 | 53 | /** 54 | * Returns a descriptor from a string 55 | * @param a descriptor 56 | * @param s string version 57 | */ 58 | static void fromString(TDescriptor &a, const std::string &s); 59 | 60 | /** 61 | * Returns a mat with the descriptors in float format 62 | * @param descriptors 63 | * @param mat (out) NxL 32F matrix 64 | */ 65 | static void toMat32F(const std::vector &descriptors, 66 | cv::Mat &mat); 67 | }; 68 | 69 | } // namespace DBoW2 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /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 | namespace DBoW2 { 20 | 21 | /// Functions to manipulate ORB descriptors 22 | class FORB: protected FClass 23 | { 24 | public: 25 | 26 | /// Descriptor type 27 | typedef cv::Mat TDescriptor; // CV_8U 28 | /// Pointer to a single descriptor 29 | typedef const TDescriptor *pDescriptor; 30 | /// Descriptor length (in bytes) 31 | static const int L; 32 | 33 | /** 34 | * Calculates the mean value of a set of descriptors 35 | * @param descriptors 36 | * @param mean mean descriptor 37 | */ 38 | static void meanValue(const std::vector &descriptors, 39 | TDescriptor &mean); 40 | 41 | /** 42 | * Calculates the distance between two descriptors 43 | * @param a 44 | * @param b 45 | * @return distance 46 | */ 47 | static int distance(const TDescriptor &a, const TDescriptor &b); 48 | 49 | /** 50 | * Returns a string version of the descriptor 51 | * @param a descriptor 52 | * @return string version 53 | */ 54 | static std::string toString(const TDescriptor &a); 55 | 56 | /** 57 | * Returns a descriptor from a string 58 | * @param a descriptor 59 | * @param s string version 60 | */ 61 | static void fromString(TDescriptor &a, const std::string &s); 62 | 63 | /** 64 | * Returns a mat with the descriptors in float format 65 | * @param descriptors 66 | * @param mat (out) NxL 32F matrix 67 | */ 68 | static void toMat32F(const std::vector &descriptors, 69 | cv::Mat &mat); 70 | 71 | static void toMat8U(const std::vector &descriptors, 72 | cv::Mat &mat); 73 | 74 | }; 75 | 76 | } // namespace DBoW2 77 | 78 | #endif 79 | 80 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | namespace DBoW2 { 19 | 20 | /// Vector of nodes with indexes of local features 21 | class FeatureVector: 22 | public std::map > 23 | { 24 | public: 25 | 26 | /** 27 | * Constructor 28 | */ 29 | FeatureVector(void); 30 | 31 | /** 32 | * Destructor 33 | */ 34 | ~FeatureVector(void); 35 | 36 | /** 37 | * Adds a feature to an existing node, or adds a new node with an initial 38 | * feature 39 | * @param id node id to add or to modify 40 | * @param i_feature index of feature to add to the given node 41 | */ 42 | void addFeature(NodeId id, unsigned int i_feature); 43 | 44 | /** 45 | * Sends a string versions of the feature vector through the stream 46 | * @param out stream 47 | * @param v feature vector 48 | */ 49 | friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v); 50 | 51 | }; 52 | 53 | } // namespace DBoW2 54 | 55 | #endif 56 | 57 | -------------------------------------------------------------------------------- /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 | namespace DBoW2 { 16 | 17 | /// Base class of scoring functions 18 | class GeneralScoring 19 | { 20 | public: 21 | /** 22 | * Computes the score between two vectors. Vectors must be sorted and 23 | * normalized if necessary 24 | * @param v (in/out) 25 | * @param w (in/out) 26 | * @return score 27 | */ 28 | virtual double score(const BowVector &v, const BowVector &w) const = 0; 29 | 30 | /** 31 | * Returns whether a vector must be normalized before scoring according 32 | * to the scoring scheme 33 | * @param norm norm to use 34 | * @return true iff must normalize 35 | */ 36 | virtual bool mustNormalize(LNorm &norm) const = 0; 37 | 38 | /// Log of epsilon 39 | static const double LOG_EPS; 40 | // If you change the type of WordValue, make sure you change also the 41 | // epsilon value (this is needed by the KL method) 42 | 43 | virtual ~GeneralScoring() {} //!< Required for virtual base classes 44 | 45 | }; 46 | 47 | /** 48 | * Macro for defining Scoring classes 49 | * @param NAME name of class 50 | * @param MUSTNORMALIZE if vectors must be normalized to compute the score 51 | * @param NORM type of norm to use when MUSTNORMALIZE 52 | */ 53 | #define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ 54 | NAME: public GeneralScoring \ 55 | { public: \ 56 | /** \ 57 | * Computes score between two vectors \ 58 | * @param v \ 59 | * @param w \ 60 | * @return score between v and w \ 61 | */ \ 62 | virtual double score(const BowVector &v, const BowVector &w) const; \ 63 | \ 64 | /** \ 65 | * Says if a vector must be normalized according to the scoring function \ 66 | * @param norm (out) if true, norm to use 67 | * @return true iff vectors must be normalized \ 68 | */ \ 69 | virtual inline bool mustNormalize(LNorm &norm) const \ 70 | { norm = NORM; return MUSTNORMALIZE; } \ 71 | } 72 | 73 | /// L1 Scoring object 74 | class __SCORING_CLASS(L1Scoring, true, L1); 75 | 76 | /// L2 Scoring object 77 | class __SCORING_CLASS(L2Scoring, true, L2); 78 | 79 | /// Chi square Scoring object 80 | class __SCORING_CLASS(ChiSquareScoring, true, L1); 81 | 82 | /// KL divergence Scoring object 83 | class __SCORING_CLASS(KLScoring, true, L1); 84 | 85 | /// Bhattacharyya Scoring object 86 | class __SCORING_CLASS(BhattacharyyaScoring, true, L1); 87 | 88 | /// Dot product Scoring object 89 | class __SCORING_CLASS(DotProductScoring, false, L1); 90 | 91 | #undef __SCORING_CLASS 92 | 93 | } // namespace DBoW2 94 | 95 | #endif 96 | 97 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/robust_kernel_factory.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_factory.h" 28 | #include "robust_kernel.h" 29 | 30 | #include 31 | 32 | using namespace std; 33 | 34 | namespace g2o { 35 | 36 | RobustKernelFactory* RobustKernelFactory::factoryInstance = 0; 37 | 38 | RobustKernelFactory::RobustKernelFactory() 39 | { 40 | } 41 | 42 | RobustKernelFactory::~RobustKernelFactory() 43 | { 44 | for (CreatorMap::iterator it = _creator.begin(); it != _creator.end(); ++it) { 45 | delete it->second; 46 | } 47 | _creator.clear(); 48 | } 49 | 50 | RobustKernelFactory* RobustKernelFactory::instance() 51 | { 52 | if (factoryInstance == 0) { 53 | factoryInstance = new RobustKernelFactory; 54 | } 55 | 56 | return factoryInstance; 57 | } 58 | 59 | void RobustKernelFactory::registerRobustKernel(const std::string& tag, AbstractRobustKernelCreator* c) 60 | { 61 | CreatorMap::const_iterator foundIt = _creator.find(tag); 62 | if (foundIt != _creator.end()) { 63 | cerr << "RobustKernelFactory WARNING: Overwriting robust kernel tag " << tag << endl; 64 | assert(0); 65 | } 66 | 67 | _creator[tag] = c; 68 | } 69 | 70 | void RobustKernelFactory::unregisterType(const std::string& tag) 71 | { 72 | CreatorMap::iterator tagPosition = _creator.find(tag); 73 | if (tagPosition != _creator.end()) { 74 | AbstractRobustKernelCreator* c = tagPosition->second; 75 | delete c; 76 | _creator.erase(tagPosition); 77 | } 78 | } 79 | 80 | RobustKernel* RobustKernelFactory::construct(const std::string& tag) const 81 | { 82 | CreatorMap::const_iterator foundIt = _creator.find(tag); 83 | if (foundIt != _creator.end()) { 84 | return foundIt->second->construct(); 85 | } 86 | return 0; 87 | } 88 | 89 | AbstractRobustKernelCreator* RobustKernelFactory::creator(const std::string& tag) const 90 | { 91 | CreatorMap::const_iterator foundIt = _creator.find(tag); 92 | if (foundIt != _creator.end()) { 93 | return foundIt->second; 94 | } 95 | return 0; 96 | } 97 | 98 | void RobustKernelFactory::fillKnownKernels(std::vector& types) const 99 | { 100 | types.clear(); 101 | for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it) 102 | types.push_back(it->first); 103 | } 104 | 105 | void RobustKernelFactory::destroy() 106 | { 107 | delete factoryInstance; 108 | factoryInstance = 0; 109 | } 110 | 111 | } // end namespace 112 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/stuff/property.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 "property.h" 28 | 29 | #include 30 | #include 31 | 32 | #include "macros.h" 33 | 34 | #include "string_tools.h" 35 | using namespace std; 36 | 37 | namespace g2o { 38 | 39 | BaseProperty::BaseProperty(const std::string name_) :_name(name_){ 40 | } 41 | 42 | BaseProperty::~BaseProperty(){} 43 | 44 | bool PropertyMap::addProperty(BaseProperty* p) { 45 | std::pair result = insert(make_pair(p->name(), p)); 46 | return result.second; 47 | } 48 | 49 | bool PropertyMap::eraseProperty(const std::string& name) { 50 | PropertyMapIterator it=find(name); 51 | if (it==end()) 52 | return false; 53 | delete it->second; 54 | erase(it); 55 | return true; 56 | } 57 | 58 | PropertyMap::~PropertyMap() { 59 | for (PropertyMapIterator it=begin(); it!=end(); it++){ 60 | if (it->second) 61 | delete it->second; 62 | } 63 | } 64 | 65 | bool PropertyMap::updatePropertyFromString(const std::string& name, const std::string& value) 66 | { 67 | PropertyMapIterator it = find(name); 68 | if (it == end()) 69 | return false; 70 | it->second->fromString(value); 71 | return true; 72 | } 73 | 74 | void PropertyMap::writeToCSV(std::ostream& os) const 75 | { 76 | for (PropertyMapConstIterator it=begin(); it!=end(); it++){ 77 | BaseProperty* p =it->second; 78 | os << p->name() << ", "; 79 | } 80 | os << std::endl; 81 | for (PropertyMapConstIterator it=begin(); it!=end(); it++){ 82 | BaseProperty* p =it->second; 83 | os << p->toString() << ", "; 84 | } 85 | os << std::endl; 86 | } 87 | 88 | bool PropertyMap::updateMapFromString(const std::string& values) 89 | { 90 | bool status = true; 91 | vector valuesMap = strSplit(values, ","); 92 | for (size_t i = 0; i < valuesMap.size(); ++i) { 93 | vector m = strSplit(valuesMap[i], "="); 94 | if (m.size() != 2) { 95 | cerr << __PRETTY_FUNCTION__ << ": unable to extract name=value pair from " << valuesMap[i] << endl; 96 | continue; 97 | } 98 | string name = trim(m[0]); 99 | string value = trim(m[1]); 100 | status = status && updatePropertyFromString(name, value); 101 | } 102 | return status; 103 | } 104 | 105 | } // end namespace 106 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /Thirdparty/line_descriptor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project( line-descriptor ) 2 | 3 | cmake_minimum_required(VERSION 2.7) 4 | find_package( OpenCV 3 REQUIRED) 5 | 6 | if(COMMAND cmake_policy) 7 | cmake_policy(SET CMP0003 NEW) 8 | endif(COMMAND cmake_policy) 9 | link_directories(${OpenCV_LIBS_DIR}) 10 | include_directories(${OpenCV2_INCLUDE_DIRS}) 11 | 12 | SET(BUILD_SHARED_LIBS ON) 13 | SET(CMAKE_MODULE_PATH $ENV{CMAKE_MODULE_PATH}) 14 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -O3 -mtune=native -march=native") 15 | 16 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/build) 17 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | include_directories( include ${OpenCV_INCLUDE_DIRS} ) 20 | list(APPEND LINK_LIBS ${OpenCV_LIBS} ) 21 | file(GLOB_RECURSE all_include_files RELATIVE "${CMAKE_SOURCE_DIR}" *.h *.hpp) 22 | 23 | link_directories(${CMAKE_SOURCE_DIR}/src/) 24 | file(GLOB_RECURSE all_source_files RELATIVE "${CMAKE_SOURCE_DIR}src/" *.cpp ) 25 | 26 | add_custom_target( linedesc_includes DEPENDS ${all_include_files} SOURCES ${all_source_files} ) 27 | 28 | add_library( linedesc ${all_source_files} ) 29 | target_link_libraries( linedesc ${LINK_LIBS} ) 30 | 31 | -------------------------------------------------------------------------------- /Thirdparty/line_descriptor/README.md: -------------------------------------------------------------------------------- 1 | Binary descriptors for lines extracted from an image 2 | ==================================================== -------------------------------------------------------------------------------- /Thirdparty/line_descriptor/src/precomp_custom.hpp: -------------------------------------------------------------------------------- 1 | /*M/////////////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this license. 6 | // If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // 10 | // License Agreement 11 | // For Open Source Computer Vision Library 12 | // 13 | // Copyright (C) 2014, Biagio Montesano, all rights reserved. 14 | // Third party copyrights are property of their respective owners. 15 | // 16 | // Redistribution and use in source and binary forms, with or without modification, 17 | // are permitted provided that the following conditions are met: 18 | // 19 | // * Redistribution's of source code must retain the above copyright notice, 20 | // this list of conditions and the following disclaimer. 21 | // 22 | // * Redistribution's in binary form must reproduce the above copyright notice, 23 | // this list of conditions and the following disclaimer in the documentation 24 | // and/or other materials provided with the distribution. 25 | // 26 | // * The name of the copyright holders may not be used to endorse or promote products 27 | // derived from this software without specific prior written permission. 28 | // 29 | // This software is provided by the copyright holders and contributors "as is" and 30 | // any express or implied warranties, including, but not limited to, the implied 31 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 32 | // In no event shall the Intel Corporation or contributors be liable for any direct, 33 | // indirect, incidental, special, exemplary, or consequential damages 34 | // (including, but not limited to, procurement of substitute goods or services; 35 | // loss of use, data, or profits; or business interruption) however caused 36 | // and on any theory of liability, whether in contract, strict liability, 37 | // or tort (including negligence or otherwise) arising in any way out of 38 | // the use of this software, even if advised of the possibility of such damage. 39 | // 40 | //M*/ 41 | 42 | #ifndef __OPENCV_PRECOMP_H__ 43 | #define __OPENCV_PRECOMP_H__ 44 | 45 | #ifdef _MSC_VER 46 | #pragma warning( disable : 4267 ) 47 | #endif 48 | 49 | #define _USE_MATH_DEFINES 50 | 51 | #include 52 | #include "opencv2/core/utility.hpp" 53 | #include 54 | #include 55 | #include 56 | #include "opencv2/core.hpp" 57 | 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | #include 67 | #include 68 | #include 69 | 70 | #include "bitarray_custom.hpp" 71 | #include "bitops_custom.hpp" 72 | #include "types_custom.hpp" 73 | 74 | #include "line_descriptor_custom.hpp" 75 | 76 | #endif 77 | -------------------------------------------------------------------------------- /Thirdparty/line_descriptor/src/types_custom.hpp: -------------------------------------------------------------------------------- 1 | /*M/////////////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this license. 6 | // If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // 10 | // License Agreement 11 | // For Open Source Computer Vision Library 12 | // 13 | // Copyright (C) 2014, Mohammad Norouzi, Ali Punjani, David J. Fleet, 14 | // all rights reserved. 15 | // Third party copyrights are property of their respective owners. 16 | // 17 | // Redistribution and use in source and binary forms, with or without modification, 18 | // are permitted provided that the following conditions are met: 19 | // 20 | // * Redistribution's of source code must retain the above copyright notice, 21 | // this list of conditions and the following disclaimer. 22 | // 23 | // * Redistribution's in binary form must reproduce the above copyright notice, 24 | // this list of conditions and the following disclaimer in the documentation 25 | // and/or other materials provided with the distribution. 26 | // 27 | // * The name of the copyright holders may not be used to endorse or promote products 28 | // derived from this software without specific prior written permission. 29 | // 30 | // This software is provided by the copyright holders and contributors "as is" and 31 | // any express or implied warranties, including, but not limited to, the implied 32 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 33 | // In no event shall the Intel Corporation or contributors be liable for any direct, 34 | // indirect, incidental, special, exemplary, or consequential damages 35 | // (including, but not limited to, procurement of substitute goods or services; 36 | // loss of use, data, or profits; or business interruption) however caused 37 | // and on any theory of liability, whether in contract, strict liability, 38 | // or tort (including negligence or otherwise) arising in any way out of 39 | // the use of this software, even if advised of the possibility of such damage. 40 | // 41 | //M*/ 42 | 43 | #if defined _MSC_VER && _MSC_VER <= 1700 44 | #include 45 | #else 46 | #include 47 | #endif 48 | 49 | #ifndef __OPENCV_TYPES_HPP 50 | #define __OPENCV_TYPES_HPP 51 | 52 | #ifdef _MSC_VER 53 | #pragma warning( disable : 4267 ) 54 | #endif 55 | 56 | /* define data types */ 57 | typedef uint64_t UINT64; 58 | typedef uint32_t UINT32; 59 | typedef uint16_t UINT16; 60 | typedef uint8_t UINT8; 61 | 62 | /* define constants */ 63 | #define UINT64_1 ((UINT64)0x01) 64 | #define UINT32_1 ((UINT32)0x01) 65 | 66 | #endif 67 | -------------------------------------------------------------------------------- /Vocabulary/LSvoc.txt.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotseu/ORB_Line_SLAM/c3872f5eccde746f2356160e6599786eae2a04a8/Vocabulary/LSvoc.txt.tar.gz -------------------------------------------------------------------------------- /Vocabulary/ORBvoc.txt.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotseu/ORB_Line_SLAM/c3872f5eccde746f2356160e6599786eae2a04a8/Vocabulary/ORBvoc.txt.tar.gz -------------------------------------------------------------------------------- /build.sh: -------------------------------------------------------------------------------- 1 | echo "Configuring and building Thirdparty/line_descriptor ..." 2 | 3 | cd Thirdparty/line_descriptor 4 | mkdir build 5 | cd build 6 | cmake .. -DCMAKE_BUILD_TYPE=Release 7 | make -j 8 | 9 | cd ../../DBoW2 10 | 11 | echo "Configuring and building Thirdparty/DBoW2 ..." 12 | 13 | mkdir build 14 | cd build 15 | cmake .. -DCMAKE_BUILD_TYPE=Release 16 | make -j 17 | 18 | cd ../../g2o 19 | 20 | echo "Configuring and building Thirdparty/g2o ..." 21 | 22 | mkdir build 23 | cd build 24 | cmake .. -DCMAKE_BUILD_TYPE=Release 25 | make -j 26 | 27 | cd ../../../ 28 | 29 | echo "Uncompress vocabulary ..." 30 | 31 | cd Vocabulary 32 | tar -xf ORBvoc.txt.tar.gz 33 | tar -xf LSvoc.txt.tar.gz 34 | cd .. 35 | 36 | echo "Configuring and building ORB_SLAM2 ..." 37 | 38 | mkdir build 39 | cd build 40 | cmake .. -DCMAKE_BUILD_TYPE=Release 41 | make -j8 42 | -------------------------------------------------------------------------------- /build_ros.sh: -------------------------------------------------------------------------------- 1 | echo "Building ROS nodes" 2 | 3 | cd Examples/ROS/ORB_Line_SLAM2 4 | mkdir build 5 | cd build 6 | cmake .. -DROS_BUILD_TYPE=Release 7 | make -j8 8 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 28 | #include 29 | #include"Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" 30 | #include"Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" 31 | 32 | namespace ORB_SLAM2 33 | { 34 | 35 | class Converter 36 | { 37 | public: 38 | static std::vector toDescriptorVector(const cv::Mat &Descriptors); 39 | 40 | static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT); 41 | static g2o::SE3Quat toSE3Quat(const g2o::Sim3 &gSim3); 42 | 43 | static cv::Mat toCvMat(const g2o::SE3Quat &SE3); 44 | static cv::Mat toCvMat(const g2o::Sim3 &Sim3); 45 | static cv::Mat toCvMat(const Eigen::Matrix &m); 46 | static cv::Mat toCvMat(const Eigen::Matrix3d &m); 47 | static cv::Mat toCvMat(const Eigen::Matrix &m); 48 | static cv::Mat toCvSE3(const Eigen::Matrix &R, const Eigen::Matrix &t); 49 | static cv::Mat toInvCvMat(const cv::Mat &T_cvMat); 50 | static cv::Mat toInvCvMat(const Eigen::Matrix &T_eigenMtrx); 51 | 52 | static Eigen::Matrix toVector3d(const cv::Point3f &cvPoint); 53 | static Eigen::Matrix toVector3d(const cv::Mat &t); 54 | static Eigen::Matrix toMatrix3d(const cv::Mat &R); 55 | static Eigen::Matrix toMatrix4d(const cv::Mat &T); 56 | static Eigen::Matrix toInvMatrix4d(const cv::Mat &T_cvMat); 57 | static Eigen::Matrix toInvMatrix4d(const Eigen::Matrix &T_eigenMtrx); 58 | 59 | static std::vector toQuaternion(const cv::Mat &M); 60 | 61 | static void RmatOfQuat(cv::Mat &M, const cv::Mat &q); 62 | }; 63 | 64 | }// namespace ORB_SLAM 65 | 66 | #endif // CONVERTER_H 67 | -------------------------------------------------------------------------------- /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 "MapLine.h" 27 | #include "Map.h" 28 | 29 | #include 30 | #include 31 | 32 | #include 33 | 34 | 35 | namespace ORB_SLAM2 36 | { 37 | 38 | class Tracking; 39 | class Viewer; 40 | 41 | class FrameDrawer 42 | { 43 | public: 44 | FrameDrawer(Map* pMap); 45 | 46 | // Update info from the last processed frame. 47 | void Update(Tracking *pTracker); 48 | 49 | // Draw last processed frame. 50 | cv::Mat DrawFrame(); 51 | 52 | protected: 53 | 54 | void DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText); 55 | 56 | // Info of the frame to be drawn 57 | cv::Mat mIm; 58 | long unsigned int mnId; 59 | int N, N_l; 60 | vector mvCurrentKeys; 61 | vector mvCurrentKeys_l; 62 | vector mvbMap, mvbVO, mvbMap_l, mvbVO_l; 63 | bool mbOnlyTracking; 64 | int mnTracked, mnTrackedVO, mnTracked_l, mnTrackedVO_l; 65 | vector mvIniKeys; 66 | vector mvIniMatches; 67 | int mState; 68 | 69 | Map* mpMap; 70 | 71 | std::mutex mMutex; 72 | }; 73 | 74 | } //namespace ORB_SLAM 75 | 76 | #endif // FRAMEDRAWER_H 77 | -------------------------------------------------------------------------------- /include/InitKeyFrame.h: -------------------------------------------------------------------------------- 1 | #ifndef INITKF_H 2 | #define INITKF_H 3 | 4 | #include "Thirdparty/DBoW2/DBoW2/BowVector.h" 5 | #include "Thirdparty/DBoW2/DBoW2/FeatureVector.h" 6 | #include "SystemSetting.h" 7 | #include 8 | #include "ORBVocabulary.h" 9 | #include "KeyFrameDatabase.h" 10 | #include "Frame.h" 11 | #include 12 | #include 13 | #include "ORBextractor.h" 14 | #include "LineExtractor.h" 15 | 16 | namespace ORB_SLAM2 17 | { 18 | #define FRAME_GRID_ROWS 48 19 | #define FRAME_GRID_COLS 64 20 | 21 | class SystemSetting; 22 | class KeyFrameDatabase; 23 | 24 | class InitKeyFrame 25 | { 26 | public: 27 | 28 | // InitKeyFrame(const Frame &frame); 29 | InitKeyFrame(SystemSetting &SS); 30 | 31 | void UndistortKeyPoints(); 32 | void UndistortKeyLines(); 33 | bool PosInGrid(const cv::KeyPoint& kp, int &posX, int &posY); 34 | void AssignFeaturesToGrid(); 35 | 36 | public: 37 | ORBVocabulary* pORBvocabulary; 38 | LineVocabulary* pLinevocabulary; 39 | 40 | long unsigned int nId; 41 | long unsigned int mnFrameId; 42 | double TimeStamp; 43 | 44 | float fGridElementWidthInv; 45 | float fGridElementHeightInv; 46 | std::vector vGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS]; 47 | 48 | float fx; 49 | float fy; 50 | float cx; 51 | float cy; 52 | float invfx; 53 | float invfy; 54 | float bf; 55 | float b; 56 | float ThDepth; 57 | int N; 58 | int N_l; 59 | std::vector vKps; 60 | std::vector vKpsUn; 61 | cv::Mat Descriptors; 62 | 63 | std::vector vRight; 64 | std::vector vDepth; 65 | 66 | DBoW2::BowVector BowVec; 67 | DBoW2::FeatureVector FeatVec; 68 | 69 | std::vector vKeys_Line; 70 | std::vector vKeysUn_Line; 71 | std::vector> vDisparity_l; 72 | std::vector vle_l; 73 | cv::Mat Descriptors_Line; 74 | 75 | //DBoW2::BowVector BowVec_l; 76 | //DBoW2::FeatureVector FeatVec_l; 77 | 78 | int nScaleLevels; 79 | float fScaleFactor; 80 | float fLogScaleFactor; 81 | std::vector vScaleFactors; 82 | std::vector vLevelSigma2; 83 | std::vector vInvLevelSigma2; 84 | std::vector vInvScaleFactors; 85 | 86 | int nMinX; 87 | int nMinY; 88 | int nMaxX; 89 | int nMaxY; 90 | cv::Mat K; 91 | cv::Mat DistCoef; 92 | }; 93 | } 94 | 95 | #endif 96 | -------------------------------------------------------------------------------- /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 | #include "KDTree.h" 32 | 33 | #include 34 | 35 | namespace ORB_SLAM2 36 | { 37 | 38 | class KeyFrame; 39 | class Frame; 40 | 41 | class point_kdtree : public std::array 42 | { 43 | public: 44 | static const int DIM = 2; // dimension of space (or "k" of k-d tree) 45 | 46 | point_kdtree() { (*this)[0]=0; (*this)[1]=0; r=0; } 47 | point_kdtree(float x, float y, float _r=0) { (*this)[0]=x; (*this)[1]=y; r=_r; } 48 | point_kdtree(const cv::KeyPoint& kp) : point_kdtree(kp.pt, kp.size) { } 49 | point_kdtree(const cv::Point2f& p, float _r=0) { (*this)[0]=p.x; (*this)[1]=p.y; r=_r; } 50 | 51 | float radius() { return r; } 52 | 53 | private: 54 | float r; 55 | }; 56 | 57 | 58 | 59 | class KeyFrameDatabase 60 | { 61 | typedef unsigned int WordId; 62 | public: 63 | 64 | KeyFrameDatabase(const ORBVocabulary &voc); 65 | KeyFrameDatabase(const ORBVocabulary &voc, const LineVocabulary &voc_l); 66 | 67 | void add(KeyFrame* pKF); 68 | 69 | void erase(KeyFrame* pKF); 70 | 71 | void clear(); 72 | 73 | // Loop Detection 74 | std::vector DetectLoopCandidates(KeyFrame* pKF, float minScore); 75 | std::vector DetectLoopCandidatesWithLine(KeyFrame* pKF, float minScore); 76 | 77 | // Relocalization 78 | std::vector DetectRelocalizationCandidates(Frame* F); 79 | std::vector DetectRelocalizationCandidatesWithLine(Frame* F); 80 | 81 | protected: 82 | 83 | // Associated vocabulary 84 | const ORBVocabulary* mpVoc; 85 | const LineVocabulary* mpVoc_l; 86 | 87 | // Inverted file 88 | std::vector > mvInvertedFile; 89 | std::vector > mvInvertedFile_l; 90 | 91 | // Mutex 92 | std::mutex mMutex; 93 | }; 94 | 95 | } //namespace ORB_SLAM 96 | 97 | #endif 98 | -------------------------------------------------------------------------------- /include/LineExtractor.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 LINEEXTRACTOR_H 22 | #define LINEEXTRACTOR_H 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include "Auxiliar.h" 29 | #include "Config.h" 30 | 31 | // #include 32 | // #include 33 | 34 | #include 35 | #include 36 | 37 | namespace ORB_SLAM2 38 | { 39 | 40 | class Lineextractor 41 | { 42 | public: 43 | Lineextractor(int _lsd_nfeatures, double _llength_th, bool _bFLD = false); 44 | Lineextractor(int _lsd_nfeatures, double _llength_th, int _lsd_refine, double _lsd_scale, double _lsd_sigma_scale, 45 | double _lsd_quant, double _lsd_ang_th, double _lsd_log_eps, double _lsd_density_th, int _lsd_n_bins, bool _bFLD = false); 46 | 47 | ~Lineextractor(){} 48 | 49 | void operator()( const cv::Mat& image, const cv::Mat& mask, 50 | std::vector& keylines, 51 | cv::Mat& descriptors_line); 52 | 53 | protected: 54 | int lsd_nfeatures; 55 | double min_line_length; 56 | 57 | // lines detection and matching 58 | int lsd_refine; 59 | double lsd_scale; 60 | double lsd_sigma_scale; 61 | double lsd_quant; 62 | double lsd_ang_th; 63 | double lsd_log_eps; 64 | double lsd_density_th; 65 | int lsd_n_bins; 66 | // double line_horiz_th; 67 | 68 | bool bFLD; 69 | 70 | double min_ratio_12_l; 71 | double ls_min_disp_ratio; 72 | }; 73 | 74 | } //namespace ORB_SLAM 75 | 76 | #endif 77 | 78 | -------------------------------------------------------------------------------- /include/LineIterator.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #pragma once 24 | 25 | //STL 26 | #include 27 | 28 | namespace ORB_SLAM2 { 29 | 30 | class LineIterator { 31 | public: 32 | 33 | LineIterator(const double x1_, const double y1_, const double x2_, const double y2_); 34 | 35 | bool getNext(std::pair &pixel); 36 | 37 | private: 38 | 39 | const bool steep; 40 | double x1, y1, x2, y2; 41 | 42 | double dx, dy, error; 43 | 44 | int maxX, ystep; 45 | int y, x; 46 | }; 47 | 48 | } // namespace ORB_SLAM2 49 | -------------------------------------------------------------------------------- /include/LineMatcher.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #pragma once 24 | 25 | //STL 26 | #include 27 | #include 28 | 29 | //OpenCV 30 | #include 31 | 32 | #include "gridStructure.h" 33 | #include "Frame.h" 34 | #include "MapPoint.h" 35 | #include "MapLine.h" 36 | 37 | namespace ORB_SLAM2 { 38 | 39 | class Frame; 40 | class MapPoint; 41 | class MapLine; 42 | 43 | typedef std::pair point_2d; 44 | typedef std::pair line_2d; 45 | 46 | inline double dot(const std::pair &a, const std::pair &b) { 47 | return (a.first*b.first + a.second*b.second); 48 | } 49 | 50 | inline void normalize(std::pair &v) { 51 | double magnitude = std::sqrt(dot(v, v)); 52 | 53 | v.first /= magnitude; 54 | v.second /= magnitude; 55 | } 56 | 57 | int matchNNR(const cv::Mat &desc1, const cv::Mat &desc2, float nnr, std::vector &matches_12); 58 | 59 | int match(const std::vector &mvpLocalMapLines, Frame &CurrentFrame, float nnr, std::vector &matches_12); 60 | 61 | int match(const cv::Mat &desc1, const cv::Mat &desc2, float nnr, std::vector &matches_12); 62 | 63 | int distance(const cv::Mat &a, const cv::Mat &b); 64 | 65 | //Points 66 | int matchGrid(const std::vector &points1, const cv::Mat &desc1, const GridStructure &grid, const cv::Mat &desc2, const GridWindow &w, std::vector &matches_12); 67 | 68 | //Lines 69 | int matchGrid(const std::vector &lines1, const cv::Mat &desc1, const GridStructure &grid, const cv::Mat &desc2, const std::vector> &directions2, const GridWindow &w, std::vector &matches_12); 70 | 71 | } // namesapce ORB_SLAM2 72 | -------------------------------------------------------------------------------- /include/LocalMapping.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef LOCALMAPPING_H 22 | #define LOCALMAPPING_H 23 | 24 | #include "KeyFrame.h" 25 | #include "Map.h" 26 | #include "LoopClosing.h" 27 | #include "Tracking.h" 28 | #include "KeyFrameDatabase.h" 29 | 30 | #include 31 | 32 | 33 | namespace ORB_SLAM2 34 | { 35 | 36 | class Tracking; 37 | class LoopClosing; 38 | class Map; 39 | 40 | class LocalMapping 41 | { 42 | public: 43 | LocalMapping(Map* pMap, const float bMonocular); 44 | 45 | void SetLoopCloser(LoopClosing* pLoopCloser); 46 | 47 | void SetTracker(Tracking* pTracker); 48 | 49 | // Main function 50 | void Run(); 51 | 52 | void InsertKeyFrame(KeyFrame* pKF); 53 | 54 | // Thread Synch 55 | void RequestStop(); 56 | void RequestReset(); 57 | bool Stop(); 58 | void Release(); 59 | bool isStopped(); 60 | bool stopRequested(); 61 | bool AcceptKeyFrames(); 62 | void SetAcceptKeyFrames(bool flag); 63 | bool SetNotStop(bool flag); 64 | 65 | void InterruptBA(); 66 | 67 | void RequestFinish(); 68 | bool isFinished(); 69 | 70 | int KeyframesInQueue(){ 71 | unique_lock lock(mMutexNewKFs); 72 | return mlNewKeyFrames.size(); 73 | } 74 | 75 | protected: 76 | 77 | bool CheckNewKeyFrames(); 78 | void ProcessNewKeyFrame(); 79 | void CreateNewMapPoints(); 80 | 81 | void MapPointCulling(); 82 | void SearchInNeighbors(); 83 | 84 | void KeyFrameCulling(); 85 | 86 | cv::Mat ComputeF12(KeyFrame* &pKF1, KeyFrame* &pKF2); 87 | 88 | cv::Mat SkewSymmetricMatrix(const cv::Mat &v); 89 | 90 | bool mbMonocular; 91 | 92 | void ResetIfRequested(); 93 | bool mbResetRequested; 94 | std::mutex mMutexReset; 95 | 96 | bool CheckFinish(); 97 | void SetFinish(); 98 | bool mbFinishRequested; 99 | bool mbFinished; 100 | std::mutex mMutexFinish; 101 | 102 | Map* mpMap; 103 | 104 | LoopClosing* mpLoopCloser; 105 | Tracking* mpTracker; 106 | 107 | std::list mlNewKeyFrames; 108 | 109 | KeyFrame* mpCurrentKeyFrame; 110 | 111 | std::list mlpRecentAddedMapPoints; 112 | 113 | std::mutex mMutexNewKFs; 114 | 115 | bool mbAbortBA; 116 | 117 | bool mbStopped; 118 | bool mbStopRequested; 119 | bool mbNotStop; 120 | std::mutex mMutexStop; 121 | 122 | bool mbAcceptKeyFrames; 123 | std::mutex mMutexAccept; 124 | }; 125 | 126 | } //namespace ORB_SLAM 127 | 128 | #endif // LOCALMAPPING_H 129 | -------------------------------------------------------------------------------- /include/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"MapLine.h" 27 | #include"KeyFrame.h" 28 | #include 29 | 30 | #include 31 | 32 | namespace ORB_SLAM2 33 | { 34 | 35 | class MapDrawer 36 | { 37 | public: 38 | MapDrawer(Map* pMap, const string &strSettingPath); 39 | 40 | Map* mpMap; 41 | 42 | void DrawMapPoints(); 43 | void DrawMapLines(); 44 | void DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph); 45 | void DrawCurrentCamera(pangolin::OpenGlMatrix &Twc); 46 | void SetCurrentCameraPose(const cv::Mat &Tcw); 47 | void SetReferenceKeyFrame(KeyFrame *pKF); 48 | void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M); 49 | 50 | private: 51 | 52 | float mKeyFrameSize; 53 | float mKeyFrameLineWidth; 54 | float mGraphLineWidth; 55 | float mPointSize; 56 | float mCameraSize; 57 | float mCameraLineWidth; 58 | 59 | cv::Mat mCameraPose; 60 | 61 | std::mutex mMutexCamera; 62 | }; 63 | 64 | } //namespace ORB_SLAM 65 | 66 | #endif // MAPDRAWER_H 67 | -------------------------------------------------------------------------------- /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 | typedef DBoW2::TemplatedVocabulary 35 | LineVocabulary; 36 | 37 | } //namespace ORB_SLAM 38 | 39 | #endif // ORBVOCABULARY_H 40 | -------------------------------------------------------------------------------- /include/ORBextractor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef ORBEXTRACTOR_H 22 | #define ORBEXTRACTOR_H 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include "Auxiliar.h" 29 | #include "Config.h" 30 | 31 | // #include 32 | // #include 33 | 34 | using namespace cv; 35 | 36 | namespace ORB_SLAM2 37 | { 38 | 39 | class ExtractorNode 40 | { 41 | public: 42 | ExtractorNode():bNoMore(false){} 43 | 44 | void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4); 45 | 46 | std::vector vKeys; 47 | cv::Point2i UL, UR, BL, BR; 48 | std::list::iterator lit; 49 | bool bNoMore; 50 | }; 51 | 52 | class ORBextractor 53 | { 54 | public: 55 | 56 | enum {HARRIS_SCORE=0, FAST_SCORE=1 }; 57 | 58 | ORBextractor(int nfeatures, float scaleFactor, int nlevels, 59 | int iniThFAST, int minThFAST); 60 | 61 | ~ORBextractor(){} 62 | 63 | // Compute the ORB features and descriptors on an image. 64 | // ORB are dispersed on the image using an octree. 65 | // Mask is ignored in the current implementation. 66 | void operator()( cv::InputArray image, cv::InputArray mask, 67 | std::vector& keypoints, 68 | cv::OutputArray descriptors); 69 | 70 | int inline GetLevels(){ 71 | return nlevels;} 72 | 73 | float inline GetScaleFactor(){ 74 | return scaleFactor;} 75 | 76 | std::vector inline GetScaleFactors(){ 77 | return mvScaleFactor; 78 | } 79 | 80 | std::vector inline GetInverseScaleFactors(){ 81 | return mvInvScaleFactor; 82 | } 83 | 84 | std::vector inline GetScaleSigmaSquares(){ 85 | return mvLevelSigma2; 86 | } 87 | 88 | std::vector inline GetInverseScaleSigmaSquares(){ 89 | return mvInvLevelSigma2; 90 | } 91 | 92 | std::vector mvImagePyramid; 93 | 94 | protected: 95 | 96 | void ComputePyramid(cv::Mat image); 97 | void ComputeKeyPointsOctTree(std::vector >& allKeypoints); 98 | std::vector DistributeOctTree(const std::vector& vToDistributeKeys, const int &minX, 99 | const int &maxX, const int &minY, const int &maxY, const int &nFeatures, const int &level); 100 | 101 | void ComputeKeyPointsOld(std::vector >& allKeypoints); 102 | std::vector pattern; 103 | 104 | int nfeatures; 105 | double scaleFactor; 106 | int nlevels; 107 | int iniThFAST; 108 | int minThFAST; 109 | 110 | std::vector mnFeaturesPerLevel; 111 | 112 | std::vector umax; 113 | 114 | std::vector mvScaleFactor; 115 | std::vector mvInvScaleFactor; 116 | std::vector mvLevelSigma2; 117 | std::vector mvInvLevelSigma2; 118 | }; 119 | 120 | } //namespace ORB_SLAM 121 | 122 | #endif 123 | 124 | -------------------------------------------------------------------------------- /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 "MapLine.h" 27 | #include "KeyFrame.h" 28 | #include "LoopClosing.h" 29 | #include "Frame.h" 30 | #include "Auxiliar.h" 31 | #include "Converter.h" 32 | 33 | #include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" 34 | 35 | #include 36 | using namespace Eigen; 37 | 38 | namespace ORB_SLAM2 39 | { 40 | 41 | class LoopClosing; 42 | 43 | class Optimizer 44 | { 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 47 | void static BundleAdjustment(const std::vector &vpKF, const std::vector &vpMP, 48 | int nIterations = 5, bool *pbStopFlag=NULL, const unsigned long nLoopKF=0, 49 | const bool bRobust = true); 50 | void static GlobalBundleAdjustemnt(Map* pMap, int nIterations=5, bool *pbStopFlag=NULL, 51 | const unsigned long nLoopKF=0, const bool bRobust = true); 52 | void static LocalBundleAdjustment(KeyFrame* pKF, bool *pbStopFlag, Map *pMap); 53 | int static PoseOptimization(Frame* pFrame); 54 | 55 | void static removeOutliers(Matrix4d DT, Frame *pFrame); 56 | bool static PoseOptimizationWithLine(Frame* pFrame); 57 | 58 | // if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise (mono) 59 | void static OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF, 60 | const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, 61 | const LoopClosing::KeyFrameAndPose &CorrectedSim3, 62 | const map > &LoopConnections, 63 | const bool &bFixScale); 64 | 65 | // if bFixScale is true, optimize SE3 (stereo,rgbd), Sim3 otherwise (mono) 66 | static int OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector &vpMatches1, 67 | g2o::Sim3 &g2oS12, const float th2, const bool bFixScale); 68 | 69 | private: 70 | static bool isGoodSolution( Matrix4d DT, Matrix6d DTcov, double err); 71 | static void gaussNewtonOptimization(Matrix4d &DT, Matrix6d &DT_cov, double &err_, int max_iters, Frame* pFrame); 72 | static void gaussNewtonOptimizationRobust(Matrix4d &DT, Matrix6d &DT_cov, double &err_, int max_iters, Frame* pFrame); 73 | static void levenbergMarquardtOptimization(Matrix4d &DT, Matrix6d &DT_cov, double &err_, int max_iters, Frame* pFrame); 74 | static void optimizeFunctions(Matrix4d DT, Matrix6d &H, Vector6d &g, double &e, Frame* pFrame); 75 | static void optimizeFunctionsRobust(Matrix4d DT, Matrix6d &H, Vector6d &g, double &e, Frame* pFrame); 76 | }; 77 | 78 | } //namespace ORB_SLAM 79 | 80 | #endif // OPTIMIZER_H 81 | -------------------------------------------------------------------------------- /include/SystemSetting.h: -------------------------------------------------------------------------------- 1 | #ifndef SYSTEMSETTING_H 2 | #define SYSTEMSETTING_H 3 | 4 | #include 5 | #include "ORBVocabulary.h" 6 | #include 7 | 8 | namespace ORB_SLAM2 9 | { 10 | class SystemSetting 11 | { 12 | public: 13 | SystemSetting(ORBVocabulary* pORBVoc, LineVocabulary* pLineVoc); 14 | bool LoadSystemSetting(const std::string strSettingPath); 15 | ~SystemSetting(); 16 | 17 | ORBVocabulary* pORBvocabulary; 18 | LineVocabulary* pLinevocabulary; 19 | 20 | float width; 21 | float height; 22 | float fx; 23 | float fy; 24 | float cx; 25 | float cy; 26 | float invfx; 27 | float invfy; 28 | float bf; 29 | float b; 30 | float fps; 31 | cv::Mat K; 32 | cv::Mat DistCoef; 33 | bool initialized; 34 | 35 | //camera feature parameters 36 | int nRGB; 37 | 38 | //ORB feature parameters 39 | int nFeatures; 40 | float fScaleFactor; 41 | int nLevels; 42 | float fIniThFAST; 43 | float fMinThFAST; 44 | 45 | //other 46 | float ThDepth = -1; 47 | float DepthMapFactor = -1; 48 | 49 | }; 50 | } 51 | #endif 52 | -------------------------------------------------------------------------------- /include/Viewer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #ifndef VIEWER_H 23 | #define VIEWER_H 24 | 25 | #include "FrameDrawer.h" 26 | #include "MapDrawer.h" 27 | #include "Tracking.h" 28 | #include "System.h" 29 | 30 | #include 31 | 32 | namespace ORB_SLAM2 33 | { 34 | 35 | class Tracking; 36 | class FrameDrawer; 37 | class MapDrawer; 38 | class System; 39 | 40 | class Viewer 41 | { 42 | public: 43 | Viewer(System* pSystem, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Tracking *pTracking, const string &strSettingPath); 44 | 45 | // Main thread function. Draw points, keyframes, the current camera pose and the last processed 46 | // frame. Drawing is refreshed according to the camera fps. We use Pangolin. 47 | void Run(); 48 | 49 | void RequestFinish(); 50 | 51 | void RequestStop(); 52 | 53 | bool isFinished(); 54 | 55 | bool isStopped(); 56 | 57 | void Release(); 58 | 59 | private: 60 | 61 | bool Stop(); 62 | 63 | System* mpSystem; 64 | FrameDrawer* mpFrameDrawer; 65 | MapDrawer* mpMapDrawer; 66 | Tracking* mpTracker; 67 | 68 | // 1/fps in ms 69 | double mT; 70 | float mImageWidth, mImageHeight; 71 | 72 | float mViewpointX, mViewpointY, mViewpointZ, mViewpointF; 73 | 74 | bool CheckFinish(); 75 | void SetFinish(); 76 | bool mbFinishRequested; 77 | bool mbFinished; 78 | std::mutex mMutexFinish; 79 | 80 | bool mbStopped; 81 | bool mbStopRequested; 82 | std::mutex mMutexStop; 83 | 84 | }; 85 | 86 | } 87 | 88 | 89 | #endif // VIEWER_H 90 | 91 | 92 | -------------------------------------------------------------------------------- /include/gridStructure.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #pragma once 24 | 25 | //STL 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace ORB_SLAM2 { 32 | 33 | void getLineCoords(double x1, double y1, double x2, double y2, std::list> &line_coords); 34 | 35 | struct GridWindow { 36 | std::pair width, height; 37 | }; 38 | 39 | class GridStructure { 40 | public: 41 | 42 | int rows, cols; 43 | 44 | GridStructure(int rows, int cols); 45 | 46 | ~GridStructure(); 47 | 48 | std::list& at(int x, int y); 49 | 50 | void get(int x, int y, const GridWindow &w, std::unordered_set &indices) const; 51 | 52 | void clear(); 53 | 54 | private: 55 | 56 | std::vector>> grid; 57 | std::list out_of_bounds; 58 | }; 59 | 60 | } // namespace ORB_SLAM2 61 | -------------------------------------------------------------------------------- /snapshot1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotseu/ORB_Line_SLAM/c3872f5eccde746f2356160e6599786eae2a04a8/snapshot1.jpg -------------------------------------------------------------------------------- /src/LineIterator.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #include "LineIterator.h" 24 | 25 | //STL 26 | #include 27 | #include 28 | 29 | //Implementation of Bresenham's line drawing Algorithm 30 | //Adapted from: https://rosettacode.org/wiki/Bitmap/Bresenham%27s_line_algorithm#C.2B.2B 31 | 32 | namespace ORB_SLAM2 { 33 | 34 | LineIterator::LineIterator(const double x1_, const double y1_, const double x2_, const double y2_) 35 | : x1(x1_), y1(y1_), x2(x2_), y2(y2_), steep(std::abs(y2_ - y1_) > std::abs(x2_ - x1_)) { 36 | 37 | if (steep) { 38 | std::swap(x1, y1); 39 | std::swap(x2, y2); 40 | } 41 | 42 | if(x1 > x2) { 43 | std::swap(x1, x2); 44 | std::swap(y1, y2); 45 | } 46 | 47 | dx = x2 - x1; 48 | dy = std::abs(y2 - y1); 49 | 50 | error = dx / 2.0; 51 | ystep = (y1 < y2) ? 1 : -1; 52 | 53 | x = static_cast(x1); 54 | y = static_cast(y1); 55 | 56 | maxX = static_cast(x2); 57 | } 58 | 59 | bool LineIterator::getNext(std::pair &pixel) { 60 | 61 | if (x > maxX) 62 | return false; 63 | 64 | if (steep) 65 | pixel = std::make_pair(y, x); 66 | else 67 | pixel = std::make_pair(x, y); 68 | 69 | error -= dy; 70 | if (error < 0) { 71 | y += ystep; 72 | error += dx; 73 | } 74 | 75 | x++; 76 | return true; 77 | } 78 | 79 | } // namespace ORB_SLAM2 80 | -------------------------------------------------------------------------------- /src/SystemSetting.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include "SystemSetting.h" 3 | 4 | using namespace std; 5 | 6 | namespace ORB_SLAM2 7 | { 8 | SystemSetting::SystemSetting(ORBVocabulary* pORBVoc, LineVocabulary* pLineVoc): 9 | pORBvocabulary(pORBVoc),pLinevocabulary(pLineVoc) 10 | { 11 | 12 | } 13 | 14 | bool SystemSetting::LoadSystemSetting(const std::string strSettingPath) 15 | { 16 | cout<(0,0) = fx; 27 | tmpK.at(1,1) = fy; 28 | tmpK.at(0,2) = cx; 29 | tmpK.at(1,2) = cy; 30 | tmpK.copyTo(K); 31 | 32 | cv::Mat tmpDistCoef(4,1,CV_32F); 33 | tmpDistCoef.at(0) = fSettings["Camera.k1"]; 34 | tmpDistCoef.at(1) = fSettings["Camera.k2"]; 35 | tmpDistCoef.at(2) = fSettings["Camera.p1"]; 36 | tmpDistCoef.at(3) = fSettings["Camera.p2"]; 37 | const float k3 = fSettings["Camera.k3"]; 38 | if( k3 != 0 ) 39 | { 40 | tmpDistCoef.resize(5); 41 | tmpDistCoef.at(4) = k3; 42 | } 43 | tmpDistCoef.copyTo( DistCoef ); 44 | 45 | bf = fSettings["Camera.bf"]; 46 | fps = fSettings["Camera.fps"]; 47 | 48 | invfx = 1.0f/fx; 49 | invfy = 1.0f/fy; 50 | b = bf/fx; 51 | initialized = true; 52 | 53 | cout << "-size:"<(0) << endl; 59 | cout << "- k2: " << DistCoef.at(1) << endl; 60 | if(DistCoef.rows==5) 61 | cout << "- k3: " << DistCoef.at(4) << endl; 62 | cout << "- p1: " << DistCoef.at(2) << endl; 63 | cout << "- p2: " << DistCoef.at(3) << endl; 64 | cout << "- bf: " << bf << endl; 65 | 66 | nRGB = fSettings["Camera.RGB"]; 67 | 68 | nFeatures = fSettings["ORBextractor.nFeatures"]; 69 | fScaleFactor = fSettings["ORBextractor.scaleFactor"]; 70 | nLevels = fSettings["ORBextractor.nLevels"]; 71 | fIniThFAST = fSettings["ORBextractor.iniThFAST"]; 72 | fMinThFAST = fSettings["ORBextractor.minThFAST"]; 73 | 74 | cout << endl << "ORB Extractor Parameters: " << endl; 75 | cout << "- Number of Features: " << nFeatures << endl; 76 | cout << "- Scale Levels: " << nLevels << endl; 77 | cout << "- Scale Factor: " << fScaleFactor << endl; 78 | cout << "- Initial Fast Threshold: " << fIniThFAST << endl; 79 | cout << "- Minimum Fast Threshold: " << fMinThFAST << endl << endl; 80 | 81 | fSettings.release(); 82 | return true; 83 | } 84 | } 85 | -------------------------------------------------------------------------------- /src/gridStructure.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #include "gridStructure.h" 24 | 25 | //STL 26 | #include 27 | #include 28 | 29 | #include "LineIterator.h" 30 | 31 | namespace ORB_SLAM2 { 32 | 33 | void getLineCoords(double x1, double y1, double x2, double y2, std::list> &line_coords) { 34 | line_coords.clear(); 35 | 36 | LineIterator it(x1, y1, x2, y2); 37 | 38 | std::pair p; 39 | while (it.getNext(p)) 40 | line_coords.push_back(p); 41 | } 42 | 43 | GridStructure::GridStructure(int rows, int cols) 44 | : rows(rows), cols(cols) { 45 | 46 | if (rows <= 0 || cols <= 0) 47 | throw std::runtime_error("[GridStructure] invalid dimension"); 48 | 49 | grid.resize(cols, std::vector>(rows)); 50 | } 51 | 52 | GridStructure::~GridStructure() { 53 | 54 | } 55 | 56 | std::list& GridStructure::at(int x, int y) { 57 | 58 | if (x >= 0 && x < cols && 59 | y >= 0 && y < rows) 60 | return grid[x][y]; 61 | else 62 | return out_of_bounds; 63 | } 64 | 65 | void GridStructure::get(int x, int y, const GridWindow &w, std::unordered_set &indices) const { 66 | 67 | int min_x = std::max(0, x - w.width.first); 68 | int max_x = std::min(cols, x + w.width.second + 1); 69 | 70 | int min_y = std::max(0, y - w.height.first); 71 | int max_y = std::min(rows, y + w.height.second + 1); 72 | 73 | for (int x_ = min_x; x_ < max_x; ++x_) 74 | for (int y_ = min_y; y_ < max_y; ++y_) 75 | indices.insert(grid[x_][y_].begin(), grid[x_][y_].end()); 76 | } 77 | 78 | void GridStructure::clear() { 79 | 80 | for (int x = 0; x < cols; ++x) 81 | for (int y = 0; y < rows; ++y) 82 | grid[x][y].clear(); 83 | } 84 | 85 | } //namesapce ORB_SLAM2 86 | --------------------------------------------------------------------------------