├── .gitignore ├── LICENSE.txt ├── ORB_SLAM2 ├── .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 │ ├── RGB-D │ │ ├── TUM1.yaml │ │ ├── TUM2.yaml │ │ ├── TUM3.yaml │ │ ├── associations │ │ │ ├── fr1_desk.txt │ │ │ ├── fr1_desk2.txt │ │ │ ├── fr1_room.txt │ │ │ ├── fr1_xyz.txt │ │ │ ├── fr2_desk.txt │ │ │ ├── fr2_xyz.txt │ │ │ ├── fr3_nstr_tex_near.txt │ │ │ ├── fr3_office.txt │ │ │ ├── fr3_office_val.txt │ │ │ ├── fr3_str_tex_far.txt │ │ │ └── fr3_str_tex_near.txt │ │ └── rgbd_tum.cc │ ├── ROS │ │ └── ORB_SLAM2 │ │ │ ├── Asus.yaml │ │ │ ├── CMakeLists.txt │ │ │ ├── launch │ │ │ └── reconstruction.launch │ │ │ ├── package.xml │ │ │ └── src │ │ │ ├── AR │ │ │ ├── ViewerAR.cc │ │ │ ├── ViewerAR.h │ │ │ └── ros_mono_ar.cc │ │ │ ├── ros_mono.cc │ │ │ ├── ros_rgbd.cc │ │ │ └── ros_stereo.cc │ └── Stereo │ │ ├── EuRoC.yaml │ │ ├── EuRoC_TimeStamps │ │ ├── MH01.txt │ │ ├── MH02.txt │ │ ├── MH03.txt │ │ ├── MH04.txt │ │ ├── MH05.txt │ │ ├── V101.txt │ │ ├── V102.txt │ │ ├── V103.txt │ │ ├── V201.txt │ │ ├── V202.txt │ │ └── V203.txt │ │ ├── KITTI00-02.yaml │ │ ├── KITTI03.yaml │ │ ├── KITTI04-12.yaml │ │ ├── stereo_euroc.cc │ │ └── stereo_kitti.cc ├── LICENSE.txt ├── License-gpl.txt ├── README.md ├── Thirdparty │ ├── DBoW2 │ │ ├── CMakeLists.txt │ │ ├── DBoW2 │ │ │ ├── BowVector.cpp │ │ │ ├── BowVector.h │ │ │ ├── FClass.h │ │ │ ├── FORB.cpp │ │ │ ├── FORB.h │ │ │ ├── FeatureVector.cpp │ │ │ ├── FeatureVector.h │ │ │ ├── ScoringObject.cpp │ │ │ ├── ScoringObject.h │ │ │ └── TemplatedVocabulary.h │ │ ├── DUtils │ │ │ ├── Random.cpp │ │ │ ├── Random.h │ │ │ ├── Timestamp.cpp │ │ │ └── Timestamp.h │ │ ├── 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 ├── Vocabulary │ └── ORBvoc.txt.tar.gz ├── build.sh ├── cmake_modules │ └── FindEigen3.cmake ├── include │ ├── Converter.h │ ├── Frame.h │ ├── FrameDrawer.h │ ├── Initializer.h │ ├── KeyFrame.h │ ├── KeyFrameDatabase.h │ ├── LocalMapping.h │ ├── LoopClosing.h │ ├── Map.h │ ├── MapDrawer.h │ ├── MapPoint.h │ ├── ORBVocabulary.h │ ├── ORBextractor.h │ ├── ORBmatcher.h │ ├── Optimizer.h │ ├── PnPsolver.h │ ├── Sim3Solver.h │ ├── System.h │ ├── Tracking.h │ └── Viewer.h └── src │ ├── Converter.cc │ ├── Frame.cc │ ├── FrameDrawer.cc │ ├── Initializer.cc │ ├── KeyFrame.cc │ ├── KeyFrameDatabase.cc │ ├── LocalMapping.cc │ ├── LoopClosing.cc │ ├── Map.cc │ ├── MapDrawer.cc │ ├── MapPoint.cc │ ├── ORBextractor.cc │ ├── ORBmatcher.cc │ ├── Optimizer.cc │ ├── PnPsolver.cc │ ├── Sim3Solver.cc │ ├── System.cc │ ├── Tracking.cc │ └── Viewer.cc ├── README.md ├── docs ├── Doxyfile └── images │ ├── max.png │ ├── overview.jpeg │ └── rgb.png ├── octomap_generator ├── CMakeLists.txt ├── include │ ├── octomap_generator │ │ ├── octomap_generator.h │ │ ├── octomap_generator_base.h │ │ └── octomap_generator_ros.h │ ├── semantics_octree │ │ ├── semantics_bayesian.h │ │ ├── semantics_max.h │ │ ├── semantics_octree.h │ │ ├── semantics_octree.hxx │ │ └── semantics_octree_node.h │ └── semantics_point_type │ │ └── semantics_point_type.h ├── package.xml └── src │ ├── octomap_generator │ ├── octomap_generator.cpp │ └── octomap_generator_ros.cpp │ └── semantic_octree │ ├── semantics_bayesian.cpp │ └── semantics_max.cpp ├── semantic_cloud ├── CMakeLists.txt ├── include │ ├── color_pcl_generator │ │ ├── __init__.py │ │ └── color_pcl_generator.py │ └── ptsemseg │ │ ├── __init__.py │ │ ├── augmentations.py │ │ ├── caffe.proto │ │ ├── caffe_pb2.py │ │ ├── loader │ │ ├── __init__.py │ │ ├── ade20k_loader.py │ │ ├── camvid_loader.py │ │ ├── cityscapes_loader.py │ │ ├── mit_sceneparsing_benchmark_loader.py │ │ ├── nyuv2_loader.py │ │ ├── pascal_voc_loader.py │ │ └── sunrgbd_loader.py │ │ ├── loss.py │ │ ├── metrics.py │ │ ├── models │ │ ├── __init__.py │ │ ├── fcn.py │ │ ├── frrn.py │ │ ├── icnet.py │ │ ├── linknet.py │ │ ├── models.py │ │ ├── pspnet.py │ │ ├── refinenet.py │ │ ├── segnet.py │ │ ├── unet.py │ │ └── utils.py │ │ └── utils.py ├── misc │ ├── ade20k_labels.png │ ├── eval_train.py │ ├── pascalVOC_lables.png │ ├── semantic_label.py │ ├── split_train_test.py │ ├── sunrgbd_labels.png │ └── test_freeze.py ├── package.xml ├── pcl_test │ ├── color_image.png │ ├── depth_image.png │ ├── depth_image.tiff │ ├── octomap_color_data.txt │ ├── ros_pcl_test.py │ ├── semantic_fusion_test.py │ └── semantic_fusion_test.rviz ├── setup.py ├── src │ └── semantic_cloud.py └── train_cnn │ ├── config.json │ ├── test.py │ ├── train.py │ └── train.sh └── semantic_slam ├── CMakeLists.txt ├── launch ├── camera.launch ├── octomap_generator.launch ├── semantic_cloud.launch ├── semantic_mapping.launch └── slam.launch ├── package.xml ├── params ├── octomap_generator.yaml ├── semantic_cloud.yaml └── xtion.yaml └── semantic_mapping.rviz /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | *.pth 3 | *.pkl 4 | .DS_Store 5 | -------------------------------------------------------------------------------- /ORB_SLAM2/.gitignore: -------------------------------------------------------------------------------- 1 | CMakeLists.txt.user 2 | Examples/Monocular/mono_euroc 3 | Examples/Monocular/mono_kitti 4 | Examples/Monocular/mono_tum 5 | Examples/RGB-D/rgbd_tum 6 | Examples/ROS/ORB_SLAM2/CMakeLists.txt.user 7 | Examples/ROS/ORB_SLAM2/Mono 8 | Examples/ROS/ORB_SLAM2/MonoAR 9 | Examples/ROS/ORB_SLAM2/RGBD 10 | Examples/ROS/ORB_SLAM2/Stereo 11 | Examples/ROS/ORB_SLAM2/build/ 12 | Examples/Stereo/stereo_euroc 13 | Examples/Stereo/stereo_kitti 14 | KeyFrameTrajectory.txt 15 | Thirdparty/DBoW2/build/ 16 | Thirdparty/DBoW2/lib/ 17 | Thirdparty/g2o/build/ 18 | Thirdparty/g2o/config.h 19 | Thirdparty/g2o/lib/ 20 | Vocabulary/ORBvoc.txt 21 | build/ 22 | *~ 23 | lib/ 24 | 25 | -------------------------------------------------------------------------------- /ORB_SLAM2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(ORB_SLAM2) 3 | 4 | IF(NOT CMAKE_BUILD_TYPE) 5 | SET(CMAKE_BUILD_TYPE Release) 6 | ENDIF() 7 | 8 | MESSAGE("Build type: " ${CMAKE_BUILD_TYPE}) 9 | 10 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 11 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 12 | 13 | # Check C++11 or C++0x support 14 | include(CheckCXXCompilerFlag) 15 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 16 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 17 | if(COMPILER_SUPPORTS_CXX11) 18 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 19 | add_definitions(-DCOMPILEDWITHC11) 20 | message(STATUS "Using flag -std=c++11.") 21 | elseif(COMPILER_SUPPORTS_CXX0X) 22 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 23 | add_definitions(-DCOMPILEDWITHC0X) 24 | message(STATUS "Using flag -std=c++0x.") 25 | else() 26 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 27 | endif() 28 | 29 | LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) 30 | 31 | find_package(OpenCV 3.0 QUIET) 32 | if(NOT OpenCV_FOUND) 33 | find_package(OpenCV 2.4.3 QUIET) 34 | if(NOT OpenCV_FOUND) 35 | message(FATAL_ERROR "OpenCV > 2.4.3 not found.") 36 | endif() 37 | endif() 38 | 39 | find_package(Eigen3 3.1.0 REQUIRED) 40 | find_package(Pangolin REQUIRED) 41 | 42 | include_directories( 43 | ${PROJECT_SOURCE_DIR} 44 | ${PROJECT_SOURCE_DIR}/include 45 | ${EIGEN3_INCLUDE_DIR} 46 | ${Pangolin_INCLUDE_DIRS} 47 | ) 48 | 49 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) 50 | 51 | add_library(${PROJECT_NAME} SHARED 52 | src/System.cc 53 | src/Tracking.cc 54 | src/LocalMapping.cc 55 | src/LoopClosing.cc 56 | src/ORBextractor.cc 57 | src/ORBmatcher.cc 58 | src/FrameDrawer.cc 59 | src/Converter.cc 60 | src/MapPoint.cc 61 | src/KeyFrame.cc 62 | src/Map.cc 63 | src/MapDrawer.cc 64 | src/Optimizer.cc 65 | src/PnPsolver.cc 66 | src/Frame.cc 67 | src/KeyFrameDatabase.cc 68 | src/Sim3Solver.cc 69 | src/Initializer.cc 70 | src/Viewer.cc 71 | ) 72 | 73 | target_link_libraries(${PROJECT_NAME} 74 | ${OpenCV_LIBS} 75 | ${EIGEN3_LIBS} 76 | ${Pangolin_LIBRARIES} 77 | ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so 78 | ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so 79 | ) 80 | 81 | # Build examples 82 | 83 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D) 84 | 85 | add_executable(rgbd_tum 86 | Examples/RGB-D/rgbd_tum.cc) 87 | target_link_libraries(rgbd_tum ${PROJECT_NAME}) 88 | 89 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo) 90 | 91 | add_executable(stereo_kitti 92 | Examples/Stereo/stereo_kitti.cc) 93 | target_link_libraries(stereo_kitti ${PROJECT_NAME}) 94 | 95 | add_executable(stereo_euroc 96 | Examples/Stereo/stereo_euroc.cc) 97 | target_link_libraries(stereo_euroc ${PROJECT_NAME}) 98 | 99 | 100 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular) 101 | 102 | add_executable(mono_tum 103 | Examples/Monocular/mono_tum.cc) 104 | target_link_libraries(mono_tum ${PROJECT_NAME}) 105 | 106 | add_executable(mono_kitti 107 | Examples/Monocular/mono_kitti.cc) 108 | target_link_libraries(mono_kitti ${PROJECT_NAME}) 109 | 110 | add_executable(mono_euroc 111 | Examples/Monocular/mono_euroc.cc) 112 | target_link_libraries(mono_euroc ${PROJECT_NAME}) 113 | 114 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/Examples/ROS/ORB_SLAM2/Asus.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 535.4 9 | Camera.fy: 539.2 10 | Camera.cx: 320.1 11 | Camera.cy: 247.6 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 640 19 | Camera.height: 480 20 | 21 | # Camera frames per second 22 | Camera.fps: 30.0 23 | 24 | # IR projector baseline times fx (aprox.) 25 | Camera.bf: 40.0 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 40.0 32 | 33 | # Deptmap values factor 34 | DepthMapFactor: 1.0 35 | 36 | #-------------------------------------------------------------------------------------------- 37 | # ORB Parameters 38 | #-------------------------------------------------------------------------------------------- 39 | 40 | # ORB Extractor: Number of features per image 41 | ORBextractor.nFeatures: 1000 42 | 43 | # ORB Extractor: Scale factor between levels in the scale pyramid 44 | ORBextractor.scaleFactor: 1.2 45 | 46 | # ORB Extractor: Number of levels in the scale pyramid 47 | ORBextractor.nLevels: 8 48 | 49 | # ORB Extractor: Fast threshold 50 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 51 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 52 | # You can lower these values if your images have low contrast 53 | ORBextractor.iniThFAST: 20 54 | ORBextractor.minThFAST: 7 55 | 56 | #-------------------------------------------------------------------------------------------- 57 | # Viewer Parameters 58 | #-------------------------------------------------------------------------------------------- 59 | Viewer.KeyFrameSize: 0.05 60 | Viewer.KeyFrameLineWidth: 1 61 | Viewer.GraphLineWidth: 0.9 62 | Viewer.PointSize:2 63 | Viewer.CameraSize: 0.08 64 | Viewer.CameraLineWidth: 3 65 | Viewer.ViewpointX: 0 66 | Viewer.ViewpointY: -0.7 67 | Viewer.ViewpointZ: -1.8 68 | Viewer.ViewpointF: 500 69 | 70 | -------------------------------------------------------------------------------- /ORB_SLAM2/Examples/ROS/ORB_SLAM2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(orb_slam2) 4 | 5 | IF(NOT CMAKE_BUILD_TYPE) 6 | SET(CMAKE_BUILD_TYPE Release) 7 | ENDIF() 8 | 9 | set(PACKAGE_DEPENDENCIES 10 | roscpp 11 | sensor_msgs 12 | std_msgs 13 | cv_bridge 14 | tf 15 | cmake_modules 16 | ) 17 | 18 | find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) 19 | 20 | # Check C++11 or C++0x support 21 | include(CheckCXXCompilerFlag) 22 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 23 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 24 | if(COMPILER_SUPPORTS_CXX11) 25 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 26 | add_definitions(-DCOMPILEDWITHC11) 27 | message(STATUS "Using flag -std=c++11.") 28 | elseif(COMPILER_SUPPORTS_CXX0X) 29 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 30 | add_definitions(-DCOMPILEDWITHC0X) 31 | message(STATUS "Using flag -std=c++0x.") 32 | else() 33 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 34 | endif() 35 | 36 | LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../../../cmake_modules) 37 | 38 | find_package(OpenCV 3.0 QUIET) 39 | if(NOT OpenCV_FOUND) 40 | find_package(OpenCV 2.4.3 QUIET) 41 | if(NOT OpenCV_FOUND) 42 | message(FATAL_ERROR "OpenCV > 2.4.3 not found.") 43 | endif() 44 | endif() 45 | 46 | find_package(Eigen3 3.1.0 REQUIRED) 47 | find_package(Pangolin REQUIRED) 48 | find_package(Boost COMPONENTS system) 49 | 50 | catkin_package() 51 | 52 | include_directories( 53 | ${catkin_INCLUDE_DIRS} 54 | ${PROJECT_SOURCE_DIR} 55 | ${PROJECT_SOURCE_DIR}/../../../ 56 | ${PROJECT_SOURCE_DIR}/../../../include 57 | ${Pangolin_INCLUDE_DIRS} 58 | ${Boost_INCLUDE_DIRS} 59 | ) 60 | 61 | set(LIBS 62 | ${catkin_LIBRARIES} 63 | ${OpenCV_LIBS} 64 | ${EIGEN3_LIBS} 65 | ${Pangolin_LIBRARIES} 66 | ${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so 67 | ${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so 68 | ${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM2.so 69 | ) 70 | 71 | # Node for monocular camera 72 | add_executable(mono 73 | src/ros_mono.cc 74 | ) 75 | 76 | target_link_libraries(mono 77 | ${LIBS} 78 | ) 79 | 80 | # Node for monocular camera (Augmented Reality Demo) 81 | add_executable(monoAR 82 | src/AR/ros_mono_ar.cc 83 | src/AR/ViewerAR.h 84 | src/AR/ViewerAR.cc 85 | ) 86 | 87 | target_link_libraries(monoAR 88 | ${LIBS} 89 | ) 90 | 91 | # Node for stereo camera 92 | add_executable(stereo 93 | src/ros_stereo.cc 94 | ) 95 | 96 | target_link_libraries(stereo 97 | ${LIBS} 98 | ) 99 | 100 | # Node for RGB-D camera 101 | add_executable(rgbd 102 | src/ros_rgbd.cc 103 | ) 104 | 105 | target_link_libraries(rgbd 106 | ${LIBS} 107 | ) 108 | 109 | -------------------------------------------------------------------------------- /ORB_SLAM2/Examples/ROS/ORB_SLAM2/launch/reconstruction.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /ORB_SLAM2/Examples/ROS/ORB_SLAM2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | orb_slam2 4 | 0.0.0 5 | ORB_SLAM2 6 | Raul Mur-Artal 7 | TODO 8 | GPLv3 9 | 10 | catkin 11 | 12 | roscpp 13 | tf 14 | sensor_msgs 15 | image_transport 16 | cv_bridge 17 | cmake_modules 18 | 19 | roscpp 20 | tf 21 | sensor_msgs 22 | image_transport 23 | cv_bridge 24 | 25 | 26 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/Vocabulary/ORBvoc.txt.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/floatlazer/semantic_slam/657814a1ba484de6b7f6f9d07c564566c8121f13/ORB_SLAM2/Vocabulary/ORBvoc.txt.tar.gz -------------------------------------------------------------------------------- /ORB_SLAM2/build.sh: -------------------------------------------------------------------------------- 1 | echo "Configuring and building Thirdparty/DBoW2 ..." 2 | 3 | cd Thirdparty/DBoW2 4 | mkdir build 5 | cd build 6 | cmake .. -DCMAKE_BUILD_TYPE=Release 7 | make -j 8 | 9 | cd ../../g2o 10 | 11 | echo "Configuring and building Thirdparty/g2o ..." 12 | 13 | mkdir build 14 | cd build 15 | cmake .. -DCMAKE_BUILD_TYPE=Release 16 | make -j 17 | 18 | cd ../../../ 19 | 20 | echo "Uncompress vocabulary ..." 21 | 22 | cd Vocabulary 23 | tar -xf ORBvoc.txt.tar.gz 24 | cd .. 25 | 26 | echo "Configuring and building ORB_SLAM2 ..." 27 | 28 | mkdir build 29 | cd build 30 | cmake .. -DCMAKE_BUILD_TYPE=Release 31 | make -j 32 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/include/Converter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef CONVERTER_H 22 | #define CONVERTER_H 23 | 24 | #include 25 | 26 | #include 27 | #include"Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" 28 | #include"Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" 29 | 30 | namespace ORB_SLAM2 31 | { 32 | 33 | class Converter 34 | { 35 | public: 36 | static std::vector toDescriptorVector(const cv::Mat &Descriptors); 37 | 38 | static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT); 39 | static g2o::SE3Quat toSE3Quat(const g2o::Sim3 &gSim3); 40 | 41 | static cv::Mat toCvMat(const g2o::SE3Quat &SE3); 42 | static cv::Mat toCvMat(const g2o::Sim3 &Sim3); 43 | static cv::Mat toCvMat(const Eigen::Matrix &m); 44 | static cv::Mat toCvMat(const Eigen::Matrix3d &m); 45 | static cv::Mat toCvMat(const Eigen::Matrix &m); 46 | static cv::Mat toCvSE3(const Eigen::Matrix &R, const Eigen::Matrix &t); 47 | 48 | static Eigen::Matrix toVector3d(const cv::Mat &cvVector); 49 | static Eigen::Matrix toVector3d(const cv::Point3f &cvPoint); 50 | static Eigen::Matrix toMatrix3d(const cv::Mat &cvMat3); 51 | 52 | static std::vector toQuaternion(const cv::Mat &M); 53 | }; 54 | 55 | }// namespace ORB_SLAM 56 | 57 | #endif // CONVERTER_H 58 | -------------------------------------------------------------------------------- /ORB_SLAM2/include/FrameDrawer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef FRAMEDRAWER_H 22 | #define FRAMEDRAWER_H 23 | 24 | #include "Tracking.h" 25 | #include "MapPoint.h" 26 | #include "Map.h" 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | 33 | 34 | namespace ORB_SLAM2 35 | { 36 | 37 | class Tracking; 38 | class Viewer; 39 | 40 | class FrameDrawer 41 | { 42 | public: 43 | FrameDrawer(Map* pMap); 44 | 45 | // Update info from the last processed frame. 46 | void Update(Tracking *pTracker); 47 | 48 | // Draw last processed frame. 49 | cv::Mat DrawFrame(); 50 | 51 | protected: 52 | 53 | void DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText); 54 | 55 | // Info of the frame to be drawn 56 | cv::Mat mIm; 57 | int N; 58 | vector mvCurrentKeys; 59 | vector mvbMap, mvbVO; 60 | bool mbOnlyTracking; 61 | int mnTracked, mnTrackedVO; 62 | vector mvIniKeys; 63 | vector mvIniMatches; 64 | int mState; 65 | 66 | Map* mpMap; 67 | 68 | std::mutex mMutex; 69 | }; 70 | 71 | } //namespace ORB_SLAM 72 | 73 | #endif // FRAMEDRAWER_H 74 | -------------------------------------------------------------------------------- /ORB_SLAM2/include/KeyFrameDatabase.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef KEYFRAMEDATABASE_H 22 | #define KEYFRAMEDATABASE_H 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include "KeyFrame.h" 29 | #include "Frame.h" 30 | #include "ORBVocabulary.h" 31 | 32 | #include 33 | 34 | 35 | namespace ORB_SLAM2 36 | { 37 | 38 | class KeyFrame; 39 | class Frame; 40 | 41 | 42 | class KeyFrameDatabase 43 | { 44 | public: 45 | 46 | KeyFrameDatabase(const ORBVocabulary &voc); 47 | 48 | void add(KeyFrame* pKF); 49 | 50 | void erase(KeyFrame* pKF); 51 | 52 | void clear(); 53 | 54 | // Loop Detection 55 | std::vector DetectLoopCandidates(KeyFrame* pKF, float minScore); 56 | 57 | // Relocalization 58 | std::vector DetectRelocalizationCandidates(Frame* F); 59 | 60 | protected: 61 | 62 | // Associated vocabulary 63 | const ORBVocabulary* mpVoc; 64 | 65 | // Inverted file 66 | std::vector > mvInvertedFile; 67 | 68 | // Mutex 69 | std::mutex mMutex; 70 | }; 71 | 72 | } //namespace ORB_SLAM 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /ORB_SLAM2/include/Map.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef MAP_H 22 | #define MAP_H 23 | 24 | #include "MapPoint.h" 25 | #include "KeyFrame.h" 26 | #include 27 | 28 | #include 29 | 30 | 31 | 32 | namespace ORB_SLAM2 33 | { 34 | 35 | class MapPoint; 36 | class KeyFrame; 37 | 38 | class Map 39 | { 40 | public: 41 | Map(); 42 | 43 | void AddKeyFrame(KeyFrame* pKF); 44 | void AddMapPoint(MapPoint* pMP); 45 | void EraseMapPoint(MapPoint* pMP); 46 | void EraseKeyFrame(KeyFrame* pKF); 47 | void SetReferenceMapPoints(const std::vector &vpMPs); 48 | void InformNewBigChange(); 49 | int GetLastBigChangeIdx(); 50 | 51 | std::vector GetAllKeyFrames(); 52 | std::vector GetAllMapPoints(); 53 | std::vector GetReferenceMapPoints(); 54 | 55 | long unsigned int MapPointsInMap(); 56 | long unsigned KeyFramesInMap(); 57 | 58 | long unsigned int GetMaxKFid(); 59 | 60 | void clear(); 61 | 62 | vector mvpKeyFrameOrigins; 63 | 64 | std::mutex mMutexMapUpdate; 65 | 66 | // This avoid that two points are created simultaneously in separate threads (id conflict) 67 | std::mutex mMutexPointCreation; 68 | 69 | protected: 70 | std::set mspMapPoints; 71 | std::set mspKeyFrames; 72 | 73 | std::vector mvpReferenceMapPoints; 74 | 75 | long unsigned int mnMaxKFid; 76 | 77 | // Index related to a big change in the map (loop closure, global BA) 78 | int mnBigChangeIdx; 79 | 80 | std::mutex mMutexMap; 81 | }; 82 | 83 | } //namespace ORB_SLAM 84 | 85 | #endif // MAP_H 86 | -------------------------------------------------------------------------------- /ORB_SLAM2/include/MapDrawer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef MAPDRAWER_H 22 | #define MAPDRAWER_H 23 | 24 | #include"Map.h" 25 | #include"MapPoint.h" 26 | #include"KeyFrame.h" 27 | #include 28 | 29 | #include 30 | 31 | namespace ORB_SLAM2 32 | { 33 | 34 | class MapDrawer 35 | { 36 | public: 37 | MapDrawer(Map* pMap, const string &strSettingPath); 38 | 39 | Map* mpMap; 40 | 41 | void DrawMapPoints(); 42 | void DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph); 43 | void DrawCurrentCamera(pangolin::OpenGlMatrix &Twc); 44 | void SetCurrentCameraPose(const cv::Mat &Tcw); 45 | void SetReferenceKeyFrame(KeyFrame *pKF); 46 | void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M); 47 | 48 | private: 49 | 50 | float mKeyFrameSize; 51 | float mKeyFrameLineWidth; 52 | float mGraphLineWidth; 53 | float mPointSize; 54 | float mCameraSize; 55 | float mCameraLineWidth; 56 | 57 | cv::Mat mCameraPose; 58 | 59 | std::mutex mMutexCamera; 60 | }; 61 | 62 | } //namespace ORB_SLAM 63 | 64 | #endif // MAPDRAWER_H 65 | -------------------------------------------------------------------------------- /ORB_SLAM2/include/ORBVocabulary.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #ifndef ORBVOCABULARY_H 23 | #define ORBVOCABULARY_H 24 | 25 | #include"Thirdparty/DBoW2/DBoW2/FORB.h" 26 | #include"Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h" 27 | 28 | namespace ORB_SLAM2 29 | { 30 | 31 | typedef DBoW2::TemplatedVocabulary 32 | ORBVocabulary; 33 | 34 | } //namespace ORB_SLAM 35 | 36 | #endif // ORBVOCABULARY_H 37 | -------------------------------------------------------------------------------- /ORB_SLAM2/include/Optimizer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef OPTIMIZER_H 22 | #define OPTIMIZER_H 23 | 24 | #include "Map.h" 25 | #include "MapPoint.h" 26 | #include "KeyFrame.h" 27 | #include "LoopClosing.h" 28 | #include "Frame.h" 29 | 30 | #include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" 31 | 32 | namespace ORB_SLAM2 33 | { 34 | 35 | class LoopClosing; 36 | 37 | class Optimizer 38 | { 39 | public: 40 | void static BundleAdjustment(const std::vector &vpKF, const std::vector &vpMP, 41 | int nIterations = 5, bool *pbStopFlag=NULL, const unsigned long nLoopKF=0, 42 | const bool bRobust = true); 43 | void static GlobalBundleAdjustemnt(Map* pMap, int nIterations=5, bool *pbStopFlag=NULL, 44 | const unsigned long nLoopKF=0, const bool bRobust = true); 45 | void static LocalBundleAdjustment(KeyFrame* pKF, bool *pbStopFlag, Map *pMap); 46 | int static PoseOptimization(Frame* pFrame); 47 | 48 | // if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise (mono) 49 | void static OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF, 50 | const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, 51 | const LoopClosing::KeyFrameAndPose &CorrectedSim3, 52 | const map > &LoopConnections, 53 | const bool &bFixScale); 54 | 55 | // if bFixScale is true, optimize SE3 (stereo,rgbd), Sim3 otherwise (mono) 56 | static int OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector &vpMatches1, 57 | g2o::Sim3 &g2oS12, const float th2, const bool bFixScale); 58 | }; 59 | 60 | } //namespace ORB_SLAM 61 | 62 | #endif // OPTIMIZER_H 63 | -------------------------------------------------------------------------------- /ORB_SLAM2/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 | -------------------------------------------------------------------------------- /docs/images/max.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/floatlazer/semantic_slam/657814a1ba484de6b7f6f9d07c564566c8121f13/docs/images/max.png -------------------------------------------------------------------------------- /docs/images/overview.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/floatlazer/semantic_slam/657814a1ba484de6b7f6f9d07c564566c8121f13/docs/images/overview.jpeg -------------------------------------------------------------------------------- /docs/images/rgb.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/floatlazer/semantic_slam/657814a1ba484de6b7f6f9d07c564566c8121f13/docs/images/rgb.png -------------------------------------------------------------------------------- /octomap_generator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(octomap_generator) 3 | 4 | # SET OCTOMAP_OMP to disable/enable OPENMP (experimental for octomap, default disabled) 5 | SET(OCTOMAP_OMP FALSE CACHE BOOL "Enable/disable OpenMP parallelization") 6 | IF(DEFINED ENV{OCTOMAP_OMP}) 7 | SET(OCTOMAP_OMP $ENV{OCTOMAP_OMP}) 8 | ENDIF(DEFINED ENV{OCTOMAP_OMP}) 9 | IF(OCTOMAP_OMP) 10 | FIND_PACKAGE( OpenMP REQUIRED) 11 | SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 12 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 13 | SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 14 | ENDIF(OCTOMAP_OMP) 15 | 16 | set(PACKAGE_DEPENDENCIES 17 | roscpp 18 | sensor_msgs 19 | pcl_ros 20 | pcl_conversions 21 | std_srvs 22 | octomap_msgs 23 | ) 24 | 25 | 26 | find_package(PCL REQUIRED QUIET COMPONENTS common sample_consensus io segmentation filters) 27 | 28 | find_package(octomap REQUIRED) 29 | 30 | find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) 31 | 32 | include_directories( 33 | include 34 | ${catkin_INCLUDE_DIRS} 35 | ${PCL_INCLUDE_DIRS} 36 | ${OCTOMAP_INCLUDE_DIRS} 37 | ) 38 | 39 | catkin_package( 40 | INCLUDE_DIRS include 41 | LIBRARIES ${PROJECT_NAME} 42 | CATKIN_DEPENDS ${PACKAGE_DEPENDENCIES} 43 | DEPENDS octomap PCL 44 | ) 45 | 46 | set(LINK_LIBS 47 | ${OCTOMAP_LIBRARIES} 48 | ${catkin_LIBRARIES} 49 | ${PCL_LIBRARIES} 50 | ) 51 | 52 | add_executable(octomap_generator 53 | src/octomap_generator/octomap_generator.cpp 54 | src/octomap_generator/octomap_generator_ros.cpp 55 | src/semantic_octree/semantics_bayesian.cpp 56 | src/semantic_octree/semantics_max.cpp 57 | ) 58 | 59 | target_link_libraries(octomap_generator ${LINK_LIBS}) 60 | -------------------------------------------------------------------------------- /octomap_generator/include/octomap_generator/octomap_generator_base.h: -------------------------------------------------------------------------------- 1 | #ifndef OCTOMAP_GENERATOR_BASE_H 2 | #define OCTOMAP_GENERATOR_BASE_H 3 | 4 | #include 5 | #include 6 | /** 7 | * Interface for octomap_generator for polymorphism 8 | * \author Xuan Zhang 9 | * \data Mai-July 2018 10 | */ 11 | class OctomapGeneratorBase 12 | { 13 | public: 14 | 15 | /// Desturctor 16 | virtual ~OctomapGeneratorBase(){}; 17 | 18 | /// Set max range for point cloud insertion 19 | virtual void setMaxRange(float max_range) = 0; 20 | 21 | /// Set max range to perform raycasting on inserted points 22 | virtual void setRayCastRange(float raycast_range) = 0; 23 | 24 | /// Set clamping_thres_min, parameter for octomap 25 | virtual void setClampingThresMin(float clamping_thres_min) = 0; 26 | 27 | /// Set clamping_thres_max, parameter for octomap 28 | virtual void setClampingThresMax(float clamping_thres_max) = 0; 29 | 30 | /// Set resolution, parameter for octomap 31 | virtual void setResolution(float resolution) = 0; 32 | 33 | /// Set occupancy_thres, parameter for octomap 34 | virtual void setOccupancyThres(float occupancy_thres) = 0; 35 | 36 | /// Set prob_hit, parameter for octomap 37 | virtual void setProbHit(float prob_hit) = 0; 38 | 39 | /// Set prob_miss, parameter for octomap 40 | virtual void setProbMiss(float prob_miss) = 0; 41 | 42 | /** 43 | * \brief Insert point cloud into octree 44 | * \param cloud converted ros cloud to be inserted 45 | * \param sensorToWorld transform from sensor frame to world frame 46 | */ 47 | virtual void insertPointCloud(const pcl::PCLPointCloud2::Ptr& cloud, const Eigen::Matrix4f& sensorToWorld) = 0; 48 | 49 | /// Set whether use semantic color for serialization 50 | virtual void setUseSemanticColor(bool use) = 0; 51 | 52 | /// Get whether use semantic color for serialization 53 | virtual bool isUseSemanticColor() = 0; 54 | 55 | /// Get octree 56 | virtual octomap::AbstractOcTree* getOctree() = 0; 57 | 58 | /// Save octomap to a file. NOTE: Not tested 59 | virtual bool save(const char* filename) const = 0; 60 | }; 61 | 62 | #endif//OCTOMAP_GENERATOR_BASE 63 | -------------------------------------------------------------------------------- /octomap_generator/include/semantics_octree/semantics_bayesian.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \author Xuan Zhang 3 | * \data Mai-July 2018 4 | */ 5 | #ifndef SEMANTICS_BAYESIAN_H 6 | #define SEMANTICS_BAYESIAN_H 7 | 8 | #include 9 | #define NUM_SEMANTICS 3 10 | 11 | 12 | namespace octomap 13 | { 14 | 15 | /// Structure of semantic color with confidence 16 | struct ColorWithConfidence 17 | { 18 | ColorWithConfidence() 19 | { 20 | color = ColorOcTreeNode::Color(255,255,255); 21 | confidence = 1.; 22 | } 23 | ColorWithConfidence(ColorOcTreeNode::Color col, float conf) 24 | { 25 | color = col; 26 | confidence = conf; 27 | } 28 | ColorOcTreeNode::Color color; 29 | float confidence; 30 | inline bool operator==(const ColorWithConfidence& rhs) const 31 | { 32 | return color == rhs.color && confidence == rhs.confidence; 33 | } 34 | inline bool operator!=(const ColorWithConfidence& rhs) const 35 | { 36 | return color != rhs.color || confidence != rhs.confidence; 37 | } 38 | inline bool operator<(const ColorWithConfidence& rhs) const 39 | { 40 | return confidence < rhs.confidence; 41 | } 42 | inline bool operator>(const ColorWithConfidence& rhs) const 43 | { 44 | return confidence > rhs.confidence; 45 | } 46 | }; 47 | 48 | std::ostream& operator<<(std::ostream& out, ColorWithConfidence const& c); 49 | /// Structure contains semantic colors and their confidences 50 | struct SemanticsBayesian 51 | { 52 | ColorWithConfidence data[NUM_SEMANTICS]; /// 9 | 10 | namespace octomap 11 | { 12 | 13 | /// Structure contains semantic colors and their confidences 14 | struct SemanticsMax 15 | { 16 | ColorOcTreeNode::Color semantic_color; /// s2.confidence ? s1 : s2; 58 | ret.confidence *= 0.9; 59 | } 60 | return ret; 61 | } 62 | }; 63 | 64 | std::ostream& operator<<(std::ostream& out, SemanticsMax const& s); 65 | } 66 | #endif //SEMANTICS_MAX_H 67 | -------------------------------------------------------------------------------- /octomap_generator/include/semantics_point_type/semantics_point_type.h: -------------------------------------------------------------------------------- 1 | #ifndef SEMANTICS_POINT_TYPE 2 | #define SEMANTICS_POINT_TYPE 3 | // Reference http://pointclouds.org/documentation/tutorials/adding_custom_ptype.php 4 | #define PCL_NO_PRECOMPILE 5 | #include 6 | /** 7 | * \brief Point type contains XYZ RGB, 3 most confident semantic colors and their confidences 8 | * \author Xuan Zhang 9 | * \data Mai-July 2018 10 | */ 11 | 12 | struct PointXYZRGBSemanticsMax 13 | { 14 | PCL_ADD_POINT4D; // Preferred way of adding a XYZ+padding 15 | PCL_ADD_RGB; 16 | union // Semantic color 17 | { 18 | float semantic_color; 19 | }; 20 | union // Confidences 21 | { 22 | float confidence; 23 | }; 24 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned 25 | } EIGEN_ALIGN16; // enforce SSE padding for correct memory alignment 26 | 27 | // here we assume a XYZ + RGB + "sementic_color" + "confidence" (as fields) 28 | POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZRGBSemanticsMax, 29 | (float, x, x) 30 | (float, y, y) 31 | (float, z, z) 32 | (float, rgb, rgb) 33 | (float, semantic_color, semantic_color) 34 | (float, confidence, confidence) 35 | ) 36 | 37 | struct PointXYZRGBSemanticsBayesian 38 | { 39 | PCL_ADD_POINT4D; // Preferred way of adding a XYZ+padding 40 | PCL_ADD_RGB; 41 | union // Semantic colors 42 | { 43 | float data_sem[4]; 44 | struct 45 | { 46 | float semantic_color1; 47 | float semantic_color2; 48 | float semantic_color3; 49 | }; 50 | }; 51 | union // Confidences 52 | { 53 | float data_conf[4]; 54 | struct 55 | { 56 | float confidence1; 57 | float confidence2; 58 | float confidence3; 59 | }; 60 | }; 61 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned 62 | } EIGEN_ALIGN16; // enforce SSE padding for correct memory alignment 63 | 64 | // here we assume a XYZ + RGB + "sementic_colors" + "confidences" (as fields) 65 | POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZRGBSemanticsBayesian, 66 | (float, x, x) 67 | (float, y, y) 68 | (float, z, z) 69 | (float, rgb, rgb) 70 | (float, semantic_color1, semantic_color1) 71 | (float, semantic_color2, semantic_color2) 72 | (float, semantic_color3, semantic_color3) 73 | (float, confidence1, confidence1) 74 | (float, confidence2, confidence2) 75 | (float, confidence3, confidence3) 76 | ) 77 | #endif 78 | -------------------------------------------------------------------------------- /octomap_generator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | octomap_generator 4 | 1.0.0 5 | Octomap generator 6 | 7 | Xuan Zhang 8 | 9 | 10 | MIT 11 | 12 | 13 | catkin 14 | 15 | roscpp 16 | sensor_msgs 17 | pcl_ros 18 | pcl_conversions 19 | std_srvs 20 | octomap 21 | 22 | octomap_msgs 23 | libpcl-all-dev 24 | 25 | roscpp 26 | sensor_msgs 27 | pcl_ros 28 | pcl_conversions 29 | std_srvs 30 | octomap 31 | octomap_msgs 32 | libpcl-all 33 | 34 | 35 | -------------------------------------------------------------------------------- /octomap_generator/src/semantic_octree/semantics_max.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | namespace octomap 3 | { 4 | std::ostream& operator<<(std::ostream& out, SemanticsMax const& s) 5 | { 6 | return out << '(' << s.semantic_color << ", " << s.confidence << ')'; 7 | } 8 | } 9 | -------------------------------------------------------------------------------- /semantic_cloud/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(semantic_cloud) 3 | 4 | set(PACKAGE_DEPENDENCIES 5 | roscpp 6 | rospy 7 | sensor_msgs 8 | ) 9 | 10 | find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) 11 | 12 | include_directories( 13 | ${catkin_INCLUDE_DIRS} 14 | ) 15 | 16 | # Make modules in include/ visible to work place 17 | catkin_python_setup() 18 | 19 | catkin_package( 20 | CATKIN_DEPENDS ${PACKAGE_DEPENDENCIES} 21 | ) 22 | -------------------------------------------------------------------------------- /semantic_cloud/include/color_pcl_generator/__init__.py: -------------------------------------------------------------------------------- 1 | from color_pcl_generator import PointType, ColorPclGenerator 2 | -------------------------------------------------------------------------------- /semantic_cloud/include/ptsemseg/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/floatlazer/semantic_slam/657814a1ba484de6b7f6f9d07c564566c8121f13/semantic_cloud/include/ptsemseg/__init__.py -------------------------------------------------------------------------------- /semantic_cloud/include/ptsemseg/loader/__init__.py: -------------------------------------------------------------------------------- 1 | import json 2 | 3 | from ptsemseg.loader.pascal_voc_loader import pascalVOCLoader 4 | from ptsemseg.loader.camvid_loader import camvidLoader 5 | from ptsemseg.loader.ade20k_loader import ADE20KLoader 6 | from ptsemseg.loader.mit_sceneparsing_benchmark_loader import MITSceneParsingBenchmarkLoader 7 | from ptsemseg.loader.cityscapes_loader import cityscapesLoader 8 | from ptsemseg.loader.nyuv2_loader import NYUv2Loader 9 | from ptsemseg.loader.sunrgbd_loader import SUNRGBDLoader 10 | 11 | 12 | def get_loader(name): 13 | """get_loader 14 | 15 | :param name: 16 | """ 17 | return { 18 | 'pascal': pascalVOCLoader, 19 | 'camvid': camvidLoader, 20 | 'ade20k': ADE20KLoader, 21 | 'mit_sceneparsing_benchmark': MITSceneParsingBenchmarkLoader, 22 | 'cityscapes': cityscapesLoader, 23 | 'nyuv2': NYUv2Loader, 24 | 'sunrgbd': SUNRGBDLoader, 25 | }[name] 26 | 27 | 28 | def get_data_path(name, config_file='config.json'): 29 | """get_data_path 30 | 31 | :param name: 32 | :param config_file: 33 | """ 34 | data = json.load(open(config_file)) 35 | return data[name]['data_path'] 36 | -------------------------------------------------------------------------------- /semantic_cloud/include/ptsemseg/loss.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import torch 3 | import numpy as np 4 | import torch.nn as nn 5 | import torch.nn.functional as F 6 | 7 | 8 | def cross_entropy2d(input, target, weight=None, size_average=True): 9 | n, c, h, w = input.size() 10 | nt, ht, wt = target.size() 11 | 12 | # Handle inconsistent size between input and target 13 | if h > ht and w > wt: # upsample labels 14 | target = target.unsequeeze(1) 15 | target = F.upsample(target, size=(h, w), mode='nearest') 16 | target = target.sequeeze(1) 17 | elif h < ht and w < wt: # upsample images 18 | input = F.upsample(input, size=(ht, wt), mode='bilinear', align_corners=True) 19 | elif h != ht and w != wt: 20 | raise Exception("Only support upsampling") 21 | 22 | log_p = F.log_softmax(input, dim=1) 23 | log_p = log_p.transpose(1, 2).transpose(2, 3).contiguous().view(-1, c) 24 | log_p = log_p[target.view(-1, 1).repeat(1, c) >= 0] 25 | log_p = log_p.view(-1, c) 26 | 27 | mask = target >= 0 28 | target = target[mask] 29 | loss = F.nll_loss(log_p, target, ignore_index=250, 30 | weight=weight, size_average=False) 31 | if size_average: 32 | loss = loss / float(mask.data.sum()) 33 | return loss 34 | 35 | def bootstrapped_cross_entropy2d(input, target, K, weight=None, size_average=True): 36 | 37 | batch_size = input.size()[0] 38 | 39 | def _bootstrap_xentropy_single(input, target, K, weight=None, size_average=True): 40 | n, c, h, w = input.size() 41 | log_p = F.log_softmax(input, dim=1) 42 | log_p = log_p.transpose(1, 2).transpose(2, 3).contiguous().view(-1, c) 43 | log_p = log_p[target.view(n * h * w, 1).repeat(1, c) >= 0] 44 | log_p = log_p.view(-1, c) 45 | 46 | mask = target >= 0 47 | target = target[mask] 48 | loss = F.nll_loss(log_p, target, weight=weight, ignore_index=250, 49 | reduce=False, size_average=False) 50 | topk_loss, _ = loss.topk(K) 51 | reduced_topk_loss = topk_loss.sum() / K 52 | 53 | return reduced_topk_loss 54 | 55 | loss = 0.0 56 | # Bootstrap from each image not entire batch 57 | for i in range(batch_size): 58 | loss += _bootstrap_xentropy_single(input=torch.unsqueeze(input[i], 0), 59 | target=torch.unsqueeze(target[i], 0), 60 | K=K, 61 | weight=weight, 62 | size_average=size_average) 63 | return loss / float(batch_size) 64 | 65 | 66 | def multi_scale_cross_entropy2d(input, target, weight=None, size_average=True, scale_weight=None, device = 'cpu'): 67 | # Auxiliary training for PSPNet [1.0, 0.4] and ICNet [1.0, 0.4, 0.16] 68 | if scale_weight == None: # scale_weight: torch tensor type 69 | n_inp = len(input) 70 | scale = 0.4 71 | scale_weight = torch.pow(scale * torch.ones(n_inp), torch.arange(n_inp)) 72 | scale_weight = scale_weight.to(device) 73 | loss = torch.Tensor([0.0]).to(device) 74 | for i, inp in enumerate(input): 75 | loss = loss + scale_weight[i] * cross_entropy2d(input=inp, target=target, weight=weight, size_average=size_average) 76 | 77 | return loss 78 | -------------------------------------------------------------------------------- /semantic_cloud/include/ptsemseg/metrics.py: -------------------------------------------------------------------------------- 1 | # Adapted from score written by wkentaro 2 | # https://github.com/wkentaro/pytorch-fcn/blob/master/torchfcn/utils.py 3 | from __future__ import division 4 | import numpy as np 5 | 6 | class runningScore(object): 7 | 8 | def __init__(self, n_classes): 9 | self.n_classes = n_classes 10 | self.confusion_matrix = np.zeros((n_classes, n_classes)) 11 | 12 | def _fast_hist(self, label_true, label_pred, n_class): 13 | mask = (label_true >= 0) & (label_true < n_class) 14 | hist = np.bincount( 15 | n_class * label_true[mask].astype(int) + 16 | label_pred[mask], minlength=n_class**2).reshape(n_class, n_class) 17 | return hist 18 | 19 | def update(self, label_trues, label_preds): 20 | for lt, lp in zip(label_trues, label_preds): 21 | self.confusion_matrix += self._fast_hist(lt.flatten(), lp.flatten(), self.n_classes) 22 | 23 | def get_scores(self): 24 | """Returns accuracy score evaluation result. 25 | - overall accuracy 26 | - mean accuracy 27 | - mean IU 28 | - fwavacc 29 | """ 30 | hist = self.confusion_matrix 31 | acc = np.diag(hist).sum() / hist.sum() 32 | acc_cls = np.diag(hist) / hist.sum(axis=1) 33 | acc_cls = np.nanmean(acc_cls) 34 | iu = np.diag(hist) / (hist.sum(axis=1) + hist.sum(axis=0) - np.diag(hist)) 35 | mean_iu = np.nanmean(iu) 36 | freq = hist.sum(axis=1) / hist.sum() 37 | fwavacc = (freq[freq > 0] * iu[freq > 0]).sum() 38 | cls_iu = dict(zip(range(self.n_classes), iu)) 39 | 40 | return {'Overall Acc': acc, 41 | 'Mean Acc': acc_cls, 42 | 'FreqW Acc': fwavacc, 43 | 'Mean IoU': mean_iu,}, cls_iu 44 | 45 | def reset(self): 46 | self.confusion_matrix = np.zeros((self.n_classes, self.n_classes)) 47 | -------------------------------------------------------------------------------- /semantic_cloud/include/ptsemseg/models/__init__.py: -------------------------------------------------------------------------------- 1 | import torchvision.models as models 2 | 3 | from ptsemseg.models.fcn import * 4 | from ptsemseg.models.segnet import * 5 | from ptsemseg.models.unet import * 6 | from ptsemseg.models.pspnet import * 7 | from ptsemseg.models.icnet import * 8 | from ptsemseg.models.linknet import * 9 | from ptsemseg.models.frrn import * 10 | 11 | 12 | def get_model(name, n_classes, version=None): 13 | model = _get_model_instance(name) 14 | 15 | if name in ['frrnA', 'frrnB']: 16 | model = model(n_classes, model_type=name[-1]) 17 | 18 | elif name in ['fcn32s', 'fcn16s', 'fcn8s']: 19 | model = model(n_classes=n_classes) 20 | vgg16 = models.vgg16(pretrained=True) 21 | model.init_vgg16_params(vgg16) 22 | 23 | elif name == 'segnet': 24 | model = model(n_classes=n_classes, 25 | is_unpooling=True) 26 | vgg16 = models.vgg16(pretrained=True) 27 | model.init_vgg16_params(vgg16) 28 | 29 | elif name == 'unet': 30 | model = model(n_classes=n_classes, 31 | is_batchnorm=True, 32 | in_channels=3, 33 | is_deconv=True) 34 | 35 | elif name == 'pspnet': 36 | model = model(n_classes=n_classes, version=version) 37 | 38 | elif name == 'icnet': 39 | model = model(n_classes=n_classes, with_bn=False, version=version) 40 | elif name == 'icnetBN': 41 | model = model(n_classes=n_classes, with_bn=True, version=version) 42 | 43 | else: 44 | model = model(n_classes=n_classes) 45 | 46 | return model 47 | 48 | def _get_model_instance(name): 49 | try: 50 | return { 51 | 'fcn32s': fcn32s, 52 | 'fcn8s': fcn8s, 53 | 'fcn16s': fcn16s, 54 | 'unet': unet, 55 | 'segnet': segnet, 56 | 'pspnet': pspnet, 57 | 'icnet': icnet, 58 | 'icnetBN': icnet, 59 | 'linknet': linknet, 60 | 'frrnA': frrn, 61 | 'frrnB': frrn, 62 | }[name] 63 | except: 64 | print('Model {} not available'.format(name)) 65 | -------------------------------------------------------------------------------- /semantic_cloud/include/ptsemseg/models/linknet.py: -------------------------------------------------------------------------------- 1 | import torch.nn as nn 2 | 3 | from ptsemseg.models.utils import * 4 | 5 | class linknet(nn.Module): 6 | 7 | def __init__(self, feature_scale=4, n_classes=21, is_deconv=True, in_channels=3, is_batchnorm=True): 8 | super(linknet, self).__init__() 9 | self.is_deconv = is_deconv 10 | self.in_channels = in_channels 11 | self.is_batchnorm = is_batchnorm 12 | self.feature_scale = feature_scale 13 | self.layers = [2, 2, 2, 2] # Currently hardcoded for ResNet-18 14 | 15 | filters = [64, 128, 256, 512] 16 | filters = [x / self.feature_scale for x in filters] 17 | 18 | self.inplanes = filters[0] 19 | 20 | 21 | # Encoder 22 | self.convbnrelu1 = conv2DBatchNormRelu(in_channels=3, k_size=7, n_filters=64, 23 | padding=3, stride=2, bias=False) 24 | self.maxpool = nn.MaxPool2d(kernel_size=3, stride=2, padding=1) 25 | 26 | block = residualBlock 27 | self.encoder1 = self._make_layer(block, filters[0], self.layers[0]) 28 | self.encoder2 = self._make_layer(block, filters[1], self.layers[1], stride=2) 29 | self.encoder3 = self._make_layer(block, filters[2], self.layers[2], stride=2) 30 | self.encoder4 = self._make_layer(block, filters[3], self.layers[3], stride=2) 31 | self.avgpool = nn.AvgPool2d(7) 32 | 33 | 34 | # Decoder 35 | self.decoder4 = linknetUp(filters[3], filters[2]) 36 | self.decoder4 = linknetUp(filters[2], filters[1]) 37 | self.decoder4 = linknetUp(filters[1], filters[0]) 38 | self.decoder4 = linknetUp(filters[0], filters[0]) 39 | 40 | # Final Classifier 41 | self.finaldeconvbnrelu1 = nn.Sequential(nn.ConvTranspose2d(filters[0], 32/feature_scale, 3, 2, 1), 42 | nn.BatchNorm2d(32/feature_scale), 43 | nn.ReLU(inplace=True),) 44 | self.finalconvbnrelu2 = conv2DBatchNormRelu(in_channels=32/feature_scale, k_size=3, n_filters=32/feature_scale, padding=1, stride=1) 45 | self.finalconv3 = nn.Conv2d(32/feature_scale, n_classes, 2, 2, 0) 46 | 47 | def _make_layer(self, block, planes, blocks, stride=1): 48 | downsample = None 49 | if stride != 1 or self.inplanes != planes * block.expansion: 50 | downsample = nn.Sequential(nn.Conv2d(self.inplanes, planes * block.expansion, 51 | kernel_size=1, stride=stride, bias=False), 52 | nn.BatchNorm2d(planes * block.expansion),) 53 | layers = [] 54 | layers.append(block(self.inplanes, planes, stride, downsample)) 55 | self.inplanes = planes * block.expansion 56 | for i in range(1, blocks): 57 | layers.append(block(self.inplanes, planes)) 58 | return nn.Sequential(*layers) 59 | 60 | 61 | def forward(self, x): 62 | # Encoder 63 | x = self.convbnrelu1(x) 64 | x = self.maxpool(x) 65 | 66 | e1 = self.encoder1(x) 67 | e2 = self.encoder2(e1) 68 | e3 = self.encoder3(e2) 69 | e4 = self.encoder4(e3) 70 | 71 | # Decoder with Skip Connections 72 | d4 = self.decoder4(e4) 73 | d4 += e3 74 | d3 = self.decoder3(d4) 75 | d3 += e2 76 | d2 = self.decoder2(d3) 77 | d2 += e1 78 | d1 = self.decoder1(d2) 79 | 80 | # Final Classification 81 | f1 = self.finaldeconvbnrelu1(d1) 82 | f2 = self.finalconvbnrelu2(f1) 83 | f3 = self.finalconv3(f2) 84 | 85 | return f3 86 | 87 | -------------------------------------------------------------------------------- /semantic_cloud/include/ptsemseg/models/models.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/floatlazer/semantic_slam/657814a1ba484de6b7f6f9d07c564566c8121f13/semantic_cloud/include/ptsemseg/models/models.py -------------------------------------------------------------------------------- /semantic_cloud/include/ptsemseg/models/refinenet.py: -------------------------------------------------------------------------------- 1 | import torch.nn as nn 2 | import torch.nn.functional as F 3 | 4 | from ptsemseg.models.utils import * 5 | 6 | class refinenet(nn.Module): 7 | """ 8 | RefineNet: Multi-Path Refinement Networks for High-Resolution Semantic Segmentation 9 | URL: https://arxiv.org/abs/1611.06612 10 | 11 | References: 12 | 1) Original Author's MATLAB code: https://github.com/guosheng/refinenet 13 | 2) TF implementation by @eragonruan: https://github.com/eragonruan/refinenet-image-segmentation 14 | """ 15 | 16 | def __init__(self, n_classes=21): 17 | super(refinenet, self).__init__() 18 | self.n_classes = n_classes 19 | 20 | pass 21 | 22 | 23 | 24 | def forward(self, x): 25 | pass 26 | 27 | -------------------------------------------------------------------------------- /semantic_cloud/include/ptsemseg/models/segnet.py: -------------------------------------------------------------------------------- 1 | import torch.nn as nn 2 | 3 | from ptsemseg.models.utils import * 4 | 5 | class segnet(nn.Module): 6 | 7 | def __init__(self, n_classes=21, in_channels=3, is_unpooling=True): 8 | super(segnet, self).__init__() 9 | 10 | self.in_channels = in_channels 11 | self.is_unpooling = is_unpooling 12 | 13 | self.down1 = segnetDown2(self.in_channels, 64) 14 | self.down2 = segnetDown2(64, 128) 15 | self.down3 = segnetDown3(128, 256) 16 | self.down4 = segnetDown3(256, 512) 17 | self.down5 = segnetDown3(512, 512) 18 | 19 | self.up5 = segnetUp3(512, 512) 20 | self.up4 = segnetUp3(512, 256) 21 | self.up3 = segnetUp3(256, 128) 22 | self.up2 = segnetUp2(128, 64) 23 | self.up1 = segnetUp2(64, n_classes) 24 | 25 | def forward(self, inputs): 26 | 27 | down1, indices_1, unpool_shape1 = self.down1(inputs) 28 | down2, indices_2, unpool_shape2 = self.down2(down1) 29 | down3, indices_3, unpool_shape3 = self.down3(down2) 30 | down4, indices_4, unpool_shape4 = self.down4(down3) 31 | down5, indices_5, unpool_shape5 = self.down5(down4) 32 | 33 | up5 = self.up5(down5, indices_5, unpool_shape5) 34 | up4 = self.up4(up5, indices_4, unpool_shape4) 35 | up3 = self.up3(up4, indices_3, unpool_shape3) 36 | up2 = self.up2(up3, indices_2, unpool_shape2) 37 | up1 = self.up1(up2, indices_1, unpool_shape1) 38 | 39 | return up1 40 | 41 | 42 | def init_vgg16_params(self, vgg16): 43 | blocks = [self.down1, 44 | self.down2, 45 | self.down3, 46 | self.down4, 47 | self.down5] 48 | 49 | ranges = [[0, 4], [5, 9], [10, 16], [17, 23], [24, 29]] 50 | features = list(vgg16.features.children()) 51 | 52 | vgg_layers = [] 53 | for _layer in features: 54 | if isinstance(_layer, nn.Conv2d): 55 | vgg_layers.append(_layer) 56 | 57 | merged_layers = [] 58 | for idx, conv_block in enumerate(blocks): 59 | if idx < 2: 60 | units = [conv_block.conv1.cbr_unit, 61 | conv_block.conv2.cbr_unit] 62 | else: 63 | units = [conv_block.conv1.cbr_unit, 64 | conv_block.conv2.cbr_unit, 65 | conv_block.conv3.cbr_unit] 66 | for _unit in units: 67 | for _layer in _unit: 68 | if isinstance(_layer, nn.Conv2d): 69 | merged_layers.append(_layer) 70 | 71 | assert len(vgg_layers) == len(merged_layers) 72 | 73 | for l1, l2 in zip(vgg_layers, merged_layers): 74 | if isinstance(l1, nn.Conv2d) and isinstance(l2, nn.Conv2d): 75 | assert l1.weight.size() == l2.weight.size() 76 | assert l1.bias.size() == l2.bias.size() 77 | l2.weight.data = l1.weight.data 78 | l2.bias.data = l1.bias.data 79 | -------------------------------------------------------------------------------- /semantic_cloud/include/ptsemseg/models/unet.py: -------------------------------------------------------------------------------- 1 | import torch.nn as nn 2 | 3 | from ptsemseg.models.utils import * 4 | 5 | class unet(nn.Module): 6 | 7 | def __init__(self, feature_scale=4, n_classes=21, is_deconv=True, in_channels=3, is_batchnorm=True): 8 | super(unet, self).__init__() 9 | self.is_deconv = is_deconv 10 | self.in_channels = in_channels 11 | self.is_batchnorm = is_batchnorm 12 | self.feature_scale = feature_scale 13 | 14 | filters = [64, 128, 256, 512, 1024] 15 | filters = [int(x / self.feature_scale) for x in filters] 16 | 17 | # downsampling 18 | self.conv1 = unetConv2(self.in_channels, filters[0], self.is_batchnorm) 19 | self.maxpool1 = nn.MaxPool2d(kernel_size=2) 20 | 21 | self.conv2 = unetConv2(filters[0], filters[1], self.is_batchnorm) 22 | self.maxpool2 = nn.MaxPool2d(kernel_size=2) 23 | 24 | self.conv3 = unetConv2(filters[1], filters[2], self.is_batchnorm) 25 | self.maxpool3 = nn.MaxPool2d(kernel_size=2) 26 | 27 | self.conv4 = unetConv2(filters[2], filters[3], self.is_batchnorm) 28 | self.maxpool4 = nn.MaxPool2d(kernel_size=2) 29 | 30 | self.center = unetConv2(filters[3], filters[4], self.is_batchnorm) 31 | 32 | # upsampling 33 | self.up_concat4 = unetUp(filters[4], filters[3], self.is_deconv) 34 | self.up_concat3 = unetUp(filters[3], filters[2], self.is_deconv) 35 | self.up_concat2 = unetUp(filters[2], filters[1], self.is_deconv) 36 | self.up_concat1 = unetUp(filters[1], filters[0], self.is_deconv) 37 | 38 | # final conv (without any concat) 39 | self.final = nn.Conv2d(filters[0], n_classes, 1) 40 | 41 | def forward(self, inputs): 42 | conv1 = self.conv1(inputs) 43 | maxpool1 = self.maxpool1(conv1) 44 | 45 | conv2 = self.conv2(maxpool1) 46 | maxpool2 = self.maxpool2(conv2) 47 | 48 | conv3 = self.conv3(maxpool2) 49 | maxpool3 = self.maxpool3(conv3) 50 | 51 | conv4 = self.conv4(maxpool3) 52 | maxpool4 = self.maxpool4(conv4) 53 | 54 | center = self.center(maxpool4) 55 | up4 = self.up_concat4(conv4, center) 56 | up3 = self.up_concat3(conv3, up4) 57 | up2 = self.up_concat2(conv2, up3) 58 | up1 = self.up_concat1(conv1, up2) 59 | 60 | final = self.final(up1) 61 | 62 | return final 63 | -------------------------------------------------------------------------------- /semantic_cloud/include/ptsemseg/utils.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Misc Utility functions 3 | ''' 4 | from __future__ import division 5 | from collections import OrderedDict 6 | import os 7 | import numpy as np 8 | 9 | def recursive_glob(rootdir='.', suffix=''): 10 | """Performs recursive glob with given suffix and rootdir 11 | :param rootdir is the root directory 12 | :param suffix is the suffix to be searched 13 | """ 14 | return [os.path.join(looproot, filename) 15 | for looproot, _, filenames in os.walk(rootdir) 16 | for filename in filenames if filename.endswith(suffix)] 17 | 18 | def poly_lr_scheduler(optimizer, init_lr, iter, lr_decay_iter=1, max_iter=30000, power=0.9,): 19 | """Polynomial decay of learning rate 20 | :param init_lr is base learning rate 21 | :param iter is a current iteration 22 | :param lr_decay_iter how frequently decay occurs, default is 1 23 | :param max_iter is number of maximum iterations 24 | :param power is a polymomial power 25 | 26 | """ 27 | if iter % lr_decay_iter or iter > max_iter: 28 | return optimizer 29 | 30 | for param_group in optimizer.param_groups: 31 | param_group['lr'] = init_lr*(1 - iter/max_iter)**power 32 | 33 | 34 | def adjust_learning_rate(optimizer, init_lr, epoch): 35 | """Sets the learning rate to the initial LR decayed by 10 every 30 epochs""" 36 | lr = init_lr * (0.1 ** (epoch // 30)) 37 | for param_group in optimizer.param_groups: 38 | param_group['lr'] = lr 39 | 40 | 41 | def alpha_blend(input_image, segmentation_mask, alpha=0.5): 42 | """Alpha Blending utility to overlay RGB masks on RBG images 43 | :param input_image is a np.ndarray with 3 channels 44 | :param segmentation_mask is a np.ndarray with 3 channels 45 | :param alpha is a float value 46 | 47 | """ 48 | blended = np.zeros(input_image.size, dtype=np.float32) 49 | blended = input_image * alpha + segmentation_mask * (1 - alpha) 50 | return blended 51 | 52 | def convert_state_dict(state_dict): 53 | """Converts a state dict saved from a dataParallel module to normal 54 | module state_dict inplace 55 | :param state_dict is the loaded DataParallel model_state 56 | 57 | """ 58 | new_state_dict = OrderedDict() 59 | for k, v in state_dict.items(): 60 | name = k[7:] # remove `module.` 61 | new_state_dict[name] = v 62 | return new_state_dict 63 | -------------------------------------------------------------------------------- /semantic_cloud/misc/ade20k_labels.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/floatlazer/semantic_slam/657814a1ba484de6b7f6f9d07c564566c8121f13/semantic_cloud/misc/ade20k_labels.png -------------------------------------------------------------------------------- /semantic_cloud/misc/eval_train.py: -------------------------------------------------------------------------------- 1 | import csv 2 | import matplotlib.pyplot as plt 3 | 4 | if __name__=='__main__': 5 | with open('../training/2018-06-19/training_loss.csv', 'r') as loss: 6 | loss_reader = csv.reader(loss, delimiter = ' ') 7 | num_epoch = 59 8 | train_loss = [0.0]*num_epoch 9 | count = [0]*num_epoch 10 | for row in loss_reader: 11 | epoch = int(row[0]) 12 | loss = float(row[1]) 13 | train_loss[epoch-1]+= loss 14 | count[epoch-1]+=1 15 | for i in range(num_epoch): 16 | train_loss[i]/=count[i] 17 | print(range(1, num_epoch+1), train_loss) 18 | plt.plot(train_loss) 19 | plt.xlabel('epoch') 20 | plt.ylabel('loss') 21 | plt.show() 22 | -------------------------------------------------------------------------------- /semantic_cloud/misc/pascalVOC_lables.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/floatlazer/semantic_slam/657814a1ba484de6b7f6f9d07c564566c8121f13/semantic_cloud/misc/pascalVOC_lables.png -------------------------------------------------------------------------------- /semantic_cloud/misc/split_train_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from shutil import copy 3 | import subprocess 4 | def split(): 5 | root = '/home/interns/xuan/datasets/SUNRGBD/' 6 | label_src = '/home/interns/xuan/datasets/sunrgbd_train_test_labels/' 7 | train_src = '/home/interns/xuan/datasets/SUNRGBD-train_images/' 8 | test_src = '/home/interns/xuan/datasets/SUNRGBD-test_images/' 9 | 10 | # clear folders 11 | print('Clearing folders...') 12 | subprocess.Popen('rm '+root + 'test/*', shell=True).wait() 13 | subprocess.Popen('rm '+root + 'train/*', shell=True).wait() 14 | subprocess.Popen('rm '+root + 'annotations/test/*', shell=True).wait() 15 | subprocess.Popen('rm '+root + 'annotations/train/*', shell=True).wait() 16 | 17 | #test 18 | size_train = 5285#5285 19 | size_test = 5050#5050 20 | for i in range(1, 1+size_test): 21 | src = test_src+ 'img-%06d.jpg' % i 22 | dst = root + 'test' 23 | copy(src, dst) 24 | print('copying ' + src + ' to '+ dst) 25 | #train 26 | for i in range(1, 1+size_train): 27 | src = train_src+ 'img-%06d.jpg' % i 28 | dst = root + 'train' 29 | copy(src, dst) 30 | print('copying ' + src + ' to '+ dst) 31 | 32 | # val test 33 | for i in range(1, 1+size_test): #range(1, 5051) 34 | src = label_src+ 'img-%06d.png' % i 35 | dst = root+'annotations/test' 36 | copy(src, dst) 37 | print('copying ' + src + ' to '+ dst) 38 | # val train 39 | for i in range(5051, 5051+size_train): #range(5051, 10336) 40 | src = label_src+ 'img-%06d.png' % i 41 | dst = root+'annotations/train' 42 | copy(src, dst) 43 | print('copying ' + src + ' to '+ dst) 44 | 45 | if __name__ == '__main__': 46 | split() 47 | -------------------------------------------------------------------------------- /semantic_cloud/misc/sunrgbd_labels.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/floatlazer/semantic_slam/657814a1ba484de6b7f6f9d07c564566c8121f13/semantic_cloud/misc/sunrgbd_labels.png -------------------------------------------------------------------------------- /semantic_cloud/misc/test_freeze.py: -------------------------------------------------------------------------------- 1 | from ptsemseg.models import get_model 2 | 3 | def freeze(m): 4 | if m.__class__.__name__ == 'BatchNorm2d': 5 | print m 6 | 7 | psp = get_model('pspnet', 38, version = 'sunrgbd') 8 | #print(psp.cbr_final.cbr_unit) 9 | psp.apply(freeze) 10 | -------------------------------------------------------------------------------- /semantic_cloud/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | semantic_cloud 4 | 1.0.0 5 | Semantic segmantation using convolutional neural network 6 | 7 | Xuan Zhang 8 | 9 | MIT 10 | 11 | catkin 12 | roscpp 13 | rospy 14 | sensor_msgs 15 | 16 | roscpp 17 | rospy 18 | sensor_msgs 19 | 20 | 21 | -------------------------------------------------------------------------------- /semantic_cloud/pcl_test/color_image.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/floatlazer/semantic_slam/657814a1ba484de6b7f6f9d07c564566c8121f13/semantic_cloud/pcl_test/color_image.png -------------------------------------------------------------------------------- /semantic_cloud/pcl_test/depth_image.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/floatlazer/semantic_slam/657814a1ba484de6b7f6f9d07c564566c8121f13/semantic_cloud/pcl_test/depth_image.png -------------------------------------------------------------------------------- /semantic_cloud/pcl_test/depth_image.tiff: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/floatlazer/semantic_slam/657814a1ba484de6b7f6f9d07c564566c8121f13/semantic_cloud/pcl_test/depth_image.tiff -------------------------------------------------------------------------------- /semantic_cloud/pcl_test/octomap_color_data.txt: -------------------------------------------------------------------------------- 1 | Semantics: (((255 0 0) 0.7) ((0 255 0) 0.2) ((0 0 255) 0.05 2 | Semantics: (((255 0 0) 0.915888) ((0 255 0) 0.0747663) ((0 0 255) 0.00467289 3 | Semantics: (((255 0 0) 0.976512) ((0 255 0) 0.0227758) ((0 0 255) 0.001 4 | Semantics: (((255 0 0) 0.993236) ((0 255 0) 0.0066188) ((0 0 255) 0.001 5 | Semantics: (((255 0 0) 0.997956) ((0 255 0) 0.00190007) ((0 0 255) 0.001 6 | Semantics: (((255 0 0) 0.999313) ((0 0 255) 0.001) ((0 255 0) 0.001 7 | Semantics: (((255 0 0) 0.999571) ((0 255 0) 0.001) ((0 0 255) 0.001 8 | Semantics: (((255 0 0) 0.999571) ((0 0 255) 0.001) ((0 255 0) 0.001 9 | Semantics: (((255 0 0) 0.999571) ((0 255 0) 0.001) ((0 0 255) 0.001 10 | Semantics: (((255 0 0) 0.999571) ((0 0 255) 0.001) ((0 255 0) 0.001 11 | 12 | Semantics: (((255 0 0) 0.996014) ((0 255 0) 0.00348755) ((0 0 255) 0.001 13 | Semantics: (((255 0 0) 0.987403) ((0 255 0) 0.0121009) ((0 0 255) 0.001 14 | Semantics: (((255 0 0) 0.958405) ((0 255 0) 0.0411093) ((0 0 255) 0.001 15 | Semantics: (((255 0 0) 0.869075) ((0 255 0) 0.130472) ((0 0 255) 0.001 16 | Semantics: (((255 0 0) 0.655299) ((0 255 0) 0.344324) ((0 0 255) 0.001 17 | Semantics: (((0 255 0) 0.647597) ((255 0 0) 0.352135) ((0 0 255) 0.001 18 | Semantics: (((0 255 0) 0.865367) ((255 0 0) 0.134442) ((0 0 255) 0.001 19 | Semantics: (((0 255 0) 0.957347) ((255 0 0) 0.0424949) ((0 0 255) 0.001 20 | Semantics: (((0 255 0) 0.987331) ((255 0 0) 0.0125217) ((0 0 255) 0.001 21 | Semantics: (((0 255 0) 0.996246) ((255 0 0) 0.00360992) ((0 0 255) 0.001 22 | Semantics: (((0 255 0) 0.998823) ((255 0 0) 0.00103407) ((0 0 255) 0.001 23 | 24 | Semantics: (((0 255 0) 0.981201) ((0 0 255) 0.013753) ((255 0 0) 0.00406332 25 | Semantics: (((0 255 0) 0.823849) ((0 0 255) 0.161665) ((255 0 0) 0.0136468 26 | Semantics: (((0 0 255) 0.72017) ((0 255 0) 0.262143) ((255 0 0) 0.0173692 27 | Semantics: (((0 0 255) 0.968063) ((0 255 0) 0.0251698) ((255 0 0) 0.00667085 28 | Semantics: (((0 0 255) 0.996115) ((255 0 0) 0.00196119) ((0 255 0) 0.00184993 29 | Semantics: (((0 0 255) 0.999234) ((0 255 0) 0.001) ((255 0 0) 0.001 30 | Semantics: (((0 0 255) 0.999571) ((255 0 0) 0.001) ((0 255 0) 0.001 31 | Semantics: (((0 0 255) 0.999571) ((0 255 0) 0.001) ((255 0 0) 0.001 32 | Semantics: (((0 0 255) 0.999571) ((255 0 0) 0.001) ((0 255 0) 0.001 33 | Semantics: (((0 0 255) 0.999571) ((0 255 0) 0.001) ((255 0 0) 0.001 34 | Semantics: (((0 0 255) 0.999571) ((255 0 0) 0.001) ((0 255 0) 0.001 35 | 36 | Semantics: (((0 0 255) 0.996749) ((255 255 254) 0.0027921) ((0 255 0) 0.001 37 | Semantics: (((0 0 255) 0.989888) ((255 255 254) 0.00970507) ((255 0 0) 0.001 38 | Semantics: (((0 0 255) 0.966352) ((255 255 254) 0.0331601) ((255 0 0) 0.001 39 | Semantics: (((0 0 255) 0.892364) ((255 255 254) 0.107174) ((255 0 0) 0.001 40 | Semantics: (((0 0 255) 0.703771) ((255 255 254) 0.295835) ((255 0 0) 0.001 41 | Semantics: (((255 255 254) 0.595175) ((0 0 255) 0.404538) ((255 0 0) 0.001 42 | Semantics: (((255 255 254) 0.837213) ((0 0 255) 0.162586) ((255 0 0) 0.001 43 | Semantics: (((255 255 254) 0.947278) ((0 0 255) 0.05256) ((255 0 0) 0.001 44 | Semantics: (((255 255 254) 0.984248) ((0 0 255) 0.0156032) ((255 0 0) 0.001 45 | Semantics: (((255 255 254) 0.995347) ((0 0 255) 0.00450833) ((255 0 0) 0.001 46 | Semantics: (((255 255 254) 0.998564) ((0 0 255) 0.00129226) ((255 0 0) 0.001 47 | -------------------------------------------------------------------------------- /semantic_cloud/pcl_test/ros_pcl_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from __future__ import print_function 3 | from __future__ import division 4 | import rospy 5 | import sys 6 | import cv2 7 | from sensor_msgs.msg import Image, PointCloud2 8 | from cv_bridge import CvBridge, CvBridgeError 9 | from color_pcl_generator import colorPclGenerator 10 | import message_filters 11 | import numpy as np 12 | from scipy import misc 13 | from skimage import io 14 | import time 15 | import matplotlib.pyplot as plt 16 | 17 | ''' 18 | Test generating color point cloud from image stream 19 | ''' 20 | class testPclRos: 21 | 22 | def __init__(self): 23 | fx = 544.771755 24 | fy = 546.966312 25 | cx = 322.376103 26 | cy = 245.357925 27 | self.intrinsic = np.matrix([[fx, 0, cx], [0, fy, cy], [0, 0, 1]], dtype = np.float32) 28 | self.pcl_pub = rospy.Publisher("pcl_test",PointCloud2, queue_size = 1, latch = False) 29 | self.bridge = CvBridge() 30 | self.color_sub = message_filters.Subscriber("/camera/rgb/image_raw",Image, queue_size = 1, buff_size = 24*640*500 ) 31 | self.depth_sub = message_filters.Subscriber("/camera/depth_registered/image_raw", Image, queue_size = 1, buff_size = 32*640*500 ) # increase buffer size to avoid delay (despite queue_size = 1) 32 | self.ats = message_filters.ApproximateTimeSynchronizer([self.color_sub, self.depth_sub], queue_size = 1, slop = 0.3) 33 | self.ats.registerCallback(self.callback) 34 | self.cloud_gen = colorPclGenerator(640, 480) 35 | 36 | def callback(self, color_img_ros, depth_img_ros): 37 | timer = time.time() 38 | try: 39 | color_img = self.bridge.imgmsg_to_cv2(color_img_ros, "bgr8") 40 | depth_img = self.bridge.imgmsg_to_cv2(depth_img_ros, "32FC1") 41 | except CvBridgeError as e: 42 | print(e) 43 | #io.imsave("color_image.png", color_img) 44 | #io.imsave("depth_image.tiff", depth_img) 45 | #print("depth image", depth_img) 46 | #print("image size", depth_img.shape) 47 | print("Prepare image took", time.time() - timer) 48 | #cv2.imshow('depth', depth_img) 49 | #cv2.waitKey(3) 50 | # Register depth to generate point cloud 51 | cloud_ros = self.cloud_gen.generate_cloud(color_img, depth_img, self.intrinsic) 52 | timer = time.time() 53 | self.pcl_pub.publish(cloud_ros) 54 | print("Publish cloud took", time.time() - timer) 55 | #cv2.waitKey() 56 | def main(args): 57 | rospy.init_node('test_pcl', anonymous=True) 58 | tpr = testPclRos() 59 | try: 60 | rospy.spin() 61 | except KeyboardInterrupt: 62 | print("Shutting down") 63 | 64 | if __name__ == '__main__': 65 | main(sys.argv) 66 | -------------------------------------------------------------------------------- /semantic_cloud/pcl_test/semantic_fusion_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ''' 3 | Send a simple 2d plan point cloud 4 | ''' 5 | from __future__ import division, print_function 6 | import rospy 7 | from color_pcl_generator import ColorPclGenerator 8 | import numpy as np 9 | from sensor_msgs.msg import PointCloud2 10 | 11 | def single_color_img(width, height, (b, g, r)): 12 | img = np.ones((height, width,3), dtype = np.uint8) 13 | img[:,:,0] = np.ones((height, width), dtype = np.uint8) * b 14 | img[:,:,1] = np.ones((height, width), dtype = np.uint8) * g 15 | img[:,:,2] = np.ones((height, width), dtype = np.uint8) * r 16 | return img 17 | 18 | class SemanticFusionTest: 19 | def __init__(self): 20 | # Camera intrinsic matrix 21 | fx = 544.771755 22 | fy = 546.966312 23 | cx = 322.376103 24 | cy = 245.357925 25 | width = 640 26 | height = 480 27 | RED = (0, 0, 255) 28 | BLUE = (255, 0, 0) 29 | GREEN = (0, 255, 0) 30 | WHITE = (255, 255, 255) 31 | bgr = WHITE 32 | confidence1 = 0.7#0.7 33 | confidence2 = 0.2#0.2 34 | confidence3 = 0.05#0.05 35 | self.intrinsic = np.matrix([[fx, 0, cx], [0, fy, cy], [0, 0, 1]], dtype = np.float32) 36 | self.pcl_pub = rospy.Publisher("semantic_pcl",PointCloud2, queue_size = 1) 37 | self.cloud_gen = ColorPclGenerator(width, height, frame_id = 'world') 38 | # Color image 39 | color_img = single_color_img(width, height, bgr) 40 | # Depth image 41 | depth_img = np.ones((height, width), dtype = np.float32) * 1. 42 | # Semantic colors 43 | semantic_colors = np.zeros((3, height, width, 3), dtype = np.uint8) 44 | # Confidences 45 | confidences = np.ones((3, height, width), dtype = np.float32) 46 | confidences[0] *= confidence1 47 | confidences[1] *= confidence2 48 | confidences[2] *= confidence3 49 | COLOR_SEQ = [] 50 | COLOR_SEQ.append([RED, GREEN, BLUE]) # Init color 51 | COLOR_SEQ.append([GREEN, RED, BLUE]) # Update with same color set 52 | COLOR_SEQ.append([BLUE, RED, GREEN]) # Update with same color set 53 | COLOR_SEQ.append([WHITE, BLUE, RED]) # Update with a new color 54 | # Publish 55 | r = rospy.Rate(2) 56 | i = 0 57 | while not rospy.is_shutdown(): 58 | # Change color 59 | semantic_colors[0] = single_color_img(width, height, COLOR_SEQ[i][0]) 60 | semantic_colors[1] = single_color_img(width, height, COLOR_SEQ[i][1]) 61 | semantic_colors[2] = single_color_img(width, height, COLOR_SEQ[i][2]) 62 | since = rospy.Time.now() 63 | while not rospy.is_shutdown() and (rospy.Time.now() - since).to_sec() < 5: 64 | # Produce point cloud with rgb colors, semantic colors and confidences 65 | cloud_ros = self.cloud_gen.generate_cloud(color_img, depth_img, semantic_colors, confidences, self.intrinsic, rospy.Time.now()) 66 | print('Publish point cloud. Color(bgr):', bgr, 'Semantic colors:', COLOR_SEQ[i][0], COLOR_SEQ[i][1], COLOR_SEQ[i][2], 'Confidences:', confidence1, confidence2, confidence3) 67 | self.pcl_pub.publish(cloud_ros) 68 | r.sleep() 69 | i += 1 70 | if i == len(COLOR_SEQ): 71 | #i = 0 72 | break 73 | 74 | if __name__ == '__main__': 75 | rospy.init_node('semantic_fusion_test') 76 | sft = SemanticFusionTest() 77 | -------------------------------------------------------------------------------- /semantic_cloud/setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | from distutils.core import setup 3 | from catkin_pkg.python_setup import generate_distutils_setup 4 | # fetch values from package.xml 5 | setup_args = generate_distutils_setup( 6 | packages=['color_pcl_generator', 'ptsemseg'], 7 | package_dir={'': 'include'}, 8 | ) 9 | setup(**setup_args) 10 | -------------------------------------------------------------------------------- /semantic_cloud/train_cnn/config.json: -------------------------------------------------------------------------------- 1 | { 2 | "pascal": 3 | { 4 | "data_path": "/home/interns/xuan/datasets/VOC/VOCdevkit/VOC2012" 5 | }, 6 | 7 | "sbd": 8 | { 9 | "data_path": "/home/interns/xuan/datasets/VOC/benchmark_RELEASE/" 10 | }, 11 | 12 | "camvid": 13 | { 14 | "data_path": "/Users/meet/data/CamVid/" 15 | }, 16 | 17 | "ade20k": 18 | { 19 | "data_path": "/home/meet/datasets/ADE20K/" 20 | }, 21 | 22 | "mit_sceneparsing_benchmark": 23 | { 24 | "data_path": "/home/ubuntu/DATA/ADEChallengeData2016/" 25 | }, 26 | 27 | "nyuv2": 28 | { 29 | "data_path": "/home/meet/datasets/NYUv2/" 30 | }, 31 | 32 | "sunrgbd": 33 | { 34 | "data_path": "/home/interns/xuan/datasets/SUNRGBD/" 35 | }, 36 | 37 | "cityscapes": 38 | { 39 | "data_path": "/home/meet/datasets/cityscapes/" 40 | } 41 | } 42 | -------------------------------------------------------------------------------- /semantic_cloud/train_cnn/test.py: -------------------------------------------------------------------------------- 1 | import sys, os 2 | import torch 3 | import visdom 4 | import argparse 5 | import timeit 6 | import numpy as np 7 | import scipy.misc as misc 8 | import torch.nn as nn 9 | import torchvision.models as models 10 | import torch 11 | 12 | from torch.autograd import Variable 13 | from torch.utils import data 14 | 15 | from ptsemseg.models import get_model 16 | from ptsemseg.utils import convert_state_dict 17 | import matplotlib.pyplot as plt 18 | 19 | def color_map(N=256, normalized=False): 20 | """ 21 | Return Color Map in PASCAL VOC format 22 | """ 23 | 24 | def bitget(byteval, idx): 25 | return ((byteval & (1 << idx)) != 0) 26 | 27 | dtype = 'float32' if normalized else 'uint8' 28 | cmap = np.zeros((N, 3), dtype=dtype) 29 | for i in range(N): 30 | r = g = b = 0 31 | c = i 32 | for j in range(8): 33 | r = r | (bitget(c, 0) << 7-j) 34 | g = g | (bitget(c, 1) << 7-j) 35 | b = b | (bitget(c, 2) << 7-j) 36 | c = c >> 3 37 | 38 | cmap[i] = np.array([r, g, b]) 39 | 40 | cmap = cmap/255.0 if normalized else cmap 41 | return cmap 42 | 43 | def decode_segmap(temp, n_classes, cmap): 44 | r = temp.copy() 45 | g = temp.copy() 46 | b = temp.copy() 47 | for l in range(0, n_classes): 48 | r[temp == l] = cmap[l,0] 49 | g[temp == l] = cmap[l,1] 50 | b[temp == l] = cmap[l,2] 51 | 52 | rgb = np.zeros((temp.shape[0], temp.shape[1], 3)) 53 | rgb[:, :, 0] = r / 255.0 54 | rgb[:, :, 1] = g / 255.0 55 | rgb[:, :, 2] = b / 255.0 56 | return rgb 57 | 58 | def test(): 59 | # model 60 | model_name ='segnet' 61 | checkpoint_path = '/home/interns/xuan/pre_catkin_ws/src/pre2018/seg_cnn/training/2018-06-19/segnet_sunrgbd_best_model.pkl' 62 | # dataset 63 | dataset = 'sunrgbd' 64 | n_classes = 38 65 | mean = np.array([104.00699, 116.66877, 122.67892]) 66 | 67 | # Setup Model 68 | model = get_model(model_name, n_classes) 69 | model = torch.nn.DataParallel(model, device_ids=range(torch.cuda.device_count())) 70 | model.cuda() 71 | checkpoint = torch.load(checkpoint_path) 72 | model.load_state_dict(checkpoint['model_state']) 73 | model.eval() 74 | 75 | # Setup image 76 | image_path = '/home/interns/xuan/datasets/SUNRGBD/test/img-000048.jpg' 77 | color_img = misc.imread(image_path) 78 | orig_size = color_img.shape[:-1] 79 | input_size = (240, 320) 80 | img = misc.imresize(color_img, input_size, interp='bicubic') 81 | img = img[:, :, ::-1] 82 | img = img.astype(float) 83 | img -= mean 84 | img = img / 255.0 85 | # NHWC -> NCHW 86 | img = img.transpose(2, 0, 1) 87 | img = np.expand_dims(img, 0) 88 | img = torch.from_numpy(img).float() 89 | images = Variable(img.cuda(0), volatile=True) 90 | 91 | # do prediction 92 | outputs = model(images) 93 | pred = np.squeeze(outputs.data.max(1)[1].cpu().numpy(), axis=0) 94 | pred = pred.astype(np.float32) 95 | pred = misc.imresize(pred, orig_size, 'nearest', mode='F') # float32 with F mode, resize back to orig_size 96 | 97 | cmap = color_map() 98 | decoded = decode_segmap(pred, n_classes, cmap) 99 | 100 | # show images 101 | plt.subplot(1,2,1), plt.imshow(color_img), plt.title('input') 102 | plt.subplot(1,2,2), plt.imshow(pred), plt.title('prediction') 103 | plt.show() 104 | 105 | 106 | if __name__ == '__main__': 107 | test() 108 | -------------------------------------------------------------------------------- /semantic_cloud/train_cnn/train.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | python train.py --dataset sunrgbd --img_rows 473 --img_cols 473 --n_epoch 50 --batch_size 2 --l_rate 0.001 --visdom 3 | -------------------------------------------------------------------------------- /semantic_slam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(semantic_slam) 3 | 4 | find_package(catkin REQUIRED) 5 | -------------------------------------------------------------------------------- /semantic_slam/launch/camera.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /semantic_slam/launch/octomap_generator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /semantic_slam/launch/semantic_cloud.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /semantic_slam/launch/semantic_mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /semantic_slam/launch/slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /semantic_slam/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | semantic_slam 4 | 1.0.0 5 | The semantic_slam package 6 | 7 | xuanzhang 8 | 9 | MIT 10 | 11 | catkin 12 | 13 | catkin 14 | 15 | roscpp 16 | sensor_msgs 17 | pcl_ros 18 | pcl_conversions 19 | std_srvs 20 | octomap 21 | 22 | octomap_msgs 23 | libpcl-all-dev 24 | 25 | roscpp 26 | sensor_msgs 27 | pcl_ros 28 | pcl_conversions 29 | std_srvs 30 | octomap 31 | octomap_msgs 32 | libpcl-all 33 | octomap_rviz_plugins 34 | 35 | 36 | -------------------------------------------------------------------------------- /semantic_slam/params/octomap_generator.yaml: -------------------------------------------------------------------------------- 1 | octomap: 2 | pointcloud_topic: "/semantic_pcl/semantic_pcl" 3 | tree_type: 1 4 | world_frame_id: "/world" 5 | resolution: 0.02 6 | max_range: 5.0 7 | raycast_range: 2.0 8 | clamping_thres_min: 0.12 9 | clamping_thres_max: 0.97 10 | occupancy_thres: 0.5 11 | prob_hit: 0.7 12 | prob_miss: 0.4 13 | save_path : "~/map.bt" 14 | -------------------------------------------------------------------------------- /semantic_slam/params/semantic_cloud.yaml: -------------------------------------------------------------------------------- 1 | # Camera intrinsic matrix parameters (OpenCV) 2 | camera: 3 | fx: 544.771755 4 | fy: 546.966312 5 | cx: 322.376103 6 | cy: 245.357925 7 | 8 | width: 640 9 | height: 480 10 | 11 | semantic_pcl: 12 | color_image_topic: "/camera/rgb/image_raw" 13 | depth_image_topic: "/camera/depth_registered/image_raw" 14 | point_type: 1 15 | frame_id: "/camera_rgb_optical_frame" 16 | dataset: "ade20k" # sunrgbd 17 | #dataset: "sunrgbd" # sunrgbd 18 | model_path: "/home/interns/xuan/models/pspnet_50_ade20k.pth" 19 | #model_path: "/home/interns/xuan/models/pspnet_sunrgbd_best_model180625_5k.pth" 20 | -------------------------------------------------------------------------------- /semantic_slam/params/xtion.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | # Parameters for orb_slam 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 544.771755 9 | Camera.fy: 546.966312 10 | Camera.cx: 322.376103 11 | Camera.cy: 245.357925 12 | 13 | Camera.k1: 0.055554 14 | Camera.k2: -0.127070 15 | Camera.p1: 0.002188 16 | Camera.p2: 0.002321 17 | Camera.k3: 0.000000 18 | 19 | Camera.width: 640 20 | Camera.height: 480 21 | 22 | # Camera frames per second 23 | Camera.fps: 30.0 24 | 25 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 26 | Camera.RGB: 1 27 | 28 | #-------------------------------------------------------------------------------------------- 29 | # ORB Parameters 30 | #-------------------------------------------------------------------------------------------- 31 | 32 | # ORB Extractor: Number of features per image 33 | ORBextractor.nFeatures: 1000 34 | 35 | # ORB Extractor: Scale factor between levels in the scale pyramid 36 | ORBextractor.scaleFactor: 1.2 37 | 38 | # ORB Extractor: Number of levels in the scale pyramid 39 | ORBextractor.nLevels: 8 40 | 41 | # ORB Extractor: Fast threshold 42 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 43 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 44 | # You can lower these values if your images have low contrast 45 | ORBextractor.iniThFAST: 20 46 | ORBextractor.minThFAST: 7 47 | 48 | #-------------------------------------------------------------------------------------------- 49 | # Viewer Parameters 50 | #-------------------------------------------------------------------------------------------- 51 | Viewer.KeyFrameSize: 0.05 52 | Viewer.KeyFrameLineWidth: 1 53 | Viewer.GraphLineWidth: 0.9 54 | Viewer.PointSize: 2 55 | Viewer.CameraSize: 0.08 56 | Viewer.CameraLineWidth: 3 57 | Viewer.ViewpointX: 0 58 | Viewer.ViewpointY: -0.7 59 | Viewer.ViewpointZ: -1.8 60 | Viewer.ViewpointF: 500 61 | --------------------------------------------------------------------------------