├── CMakeLists.txt ├── Dependencies.md ├── Examples ├── Monocular-Inertial │ ├── Config │ │ └── EuRoC.yaml │ ├── mono_inertial_euroc │ └── mono_inertial_euroc.cc ├── Monocular │ ├── Config │ │ ├── EuRoC.yaml │ │ ├── KITTI00-02.yaml │ │ ├── KITTI03.yaml │ │ ├── KITTI04-12.yaml │ │ ├── TUM1.yaml │ │ ├── TUM2.yaml │ │ └── TUM3.yaml │ ├── mono_euroc │ ├── mono_euroc.cc │ ├── mono_kitti │ ├── mono_kitti.cc │ ├── mono_tum │ └── mono_tum.cc ├── RGB-D │ ├── Config │ │ ├── TUM1.yaml │ │ ├── TUM2.yaml │ │ └── TUM3.yaml │ ├── associations │ │ ├── fr1_desk.txt │ │ ├── fr1_desk2.txt │ │ ├── fr1_room.txt │ │ ├── fr1_xyz.txt │ │ ├── fr2_desk.txt │ │ ├── fr2_xyz.txt │ │ ├── fr3_nstr_tex_near.txt │ │ ├── fr3_office.txt │ │ ├── fr3_office_val.txt │ │ ├── fr3_str_tex_far.txt │ │ └── fr3_str_tex_near.txt │ ├── rgbd_tum │ └── rgbd_tum.cc ├── ROS │ └── PLI_SLAM2 │ │ ├── Asus.yaml │ │ ├── CMakeLists.txt │ │ ├── manifest.xml │ │ └── src │ │ ├── AR │ │ ├── ViewerAR.cc │ │ ├── ViewerAR.h │ │ └── ros_mono_ar.cc │ │ ├── ros_mono.cc │ │ ├── ros_mono_inertial.cc │ │ ├── ros_rgbd.cc │ │ ├── ros_stereo.cc │ │ └── ros_stereo_inertial.cc ├── Stereo-Inertial │ ├── Config │ │ └── EuRoC.yaml │ ├── stereo_inertial_euroc │ └── stereo_inertial_euroc.cc └── Stereo │ ├── Config │ ├── EuRoC.yaml │ ├── KITTI00-02.yaml │ ├── KITTI03.yaml │ └── KITTI04-12.yaml │ ├── stereo_euroc │ ├── stereo_euroc.cc │ ├── stereo_kitti │ └── stereo_kitti.cc ├── FindEigen3.cmake ├── LICENSE ├── 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 │ └── README.txt ├── g2o │ ├── CMakeLists.txt │ ├── README.txt │ ├── cmake_modules │ │ ├── FindBLAS.cmake │ │ ├── FindEigen3.cmake │ │ └── FindLAPACK.cmake │ ├── config.h │ ├── 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 │ │ │ ├── se3mat.cpp │ │ │ ├── se3mat.h │ │ │ ├── se3quat.h │ │ │ ├── sim3.h │ │ │ ├── types_sba.cpp │ │ │ ├── types_sba.h │ │ │ ├── types_seven_dof_expmap.cpp │ │ │ ├── types_seven_dof_expmap.h │ │ │ ├── types_six_dof_expmap.cpp │ │ │ └── types_six_dof_expmap.h │ └── license-bsd.txt └── line_descriptor │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ ├── line_descriptor │ │ └── descriptor_custom.hpp │ └── line_descriptor_custom.hpp │ └── src │ ├── LSDDetector_custom.cpp │ ├── binary_descriptor_custom.cpp │ ├── binary_descriptor_matcher.cpp │ ├── bitarray_custom.hpp │ ├── bitops_custom.hpp │ ├── draw_custom.cpp │ ├── precomp_custom.hpp │ └── types_custom.hpp ├── Vocabulary └── README.md ├── build.sh ├── build_ros.sh ├── include ├── Atlas.h ├── Auxiliar.h ├── CameraModels │ ├── GeometricCamera.h │ ├── KannalaBrandt8.h │ └── Pinhole.h ├── Config.h ├── Converter.h ├── Frame.h ├── FrameDrawer.h ├── G2oTypes.h ├── ImuTypes.h ├── InitKeyFrame.h ├── Initializer.h ├── KDTree.h ├── KeyFrame.h ├── KeyFrameDatabase.h ├── LineExtractor.h ├── LineIterator.h ├── LineMatcher.h ├── LocalMapping.h ├── LoopClosing.h ├── MLPnPsolver.h ├── Map.h ├── MapDrawer.h ├── MapLine.h ├── MapPoint.h ├── ORBVocabulary.h ├── ORBextractor.h ├── ORBmatcher.h ├── OptimizableTypes.h ├── Optimizer.h ├── PnPsolver.h ├── Sim3Solver.h ├── System.h ├── SystemSetting.h ├── Tracking.h ├── TwoViewReconstruction.h ├── Viewer.h └── gridStructure.h └── src ├── Atlas.cc ├── Auxiliar.cc ├── CameraModels ├── KannalaBrandt8.cpp └── Pinhole.cpp ├── Config.cpp ├── Converter.cc ├── Frame.cc ├── FrameDrawer.cc ├── G2oTypes.cc ├── ImuTypes.cc ├── InitKeyFrame.cc ├── Initializer.cc ├── KeyFrame.cc ├── KeyFrameDatabase.cc ├── LineExtractor.cc ├── LineIterator.cpp ├── LineMatcher.cpp ├── LocalMapping.cc ├── LoopClosing.cc ├── MLPnPsolver.cpp ├── Map.cc ├── MapDrawer.cc ├── MapLine.cc ├── MapPoint.cc ├── ORBextractor.cc ├── ORBmatcher.cc ├── OptimizableTypes.cpp ├── Optimizer.cc ├── PnPsolver.cc ├── Sim3Solver.cc ├── System.cc ├── SystemSetting.cc ├── Tracking.cc ├── TwoViewReconstruction.cc ├── Viewer.cc └── gridStructure.cpp /Dependencies.md: -------------------------------------------------------------------------------- 1 | ##List of Known Dependencies 2 | ###ORB-SLAM3 version 0.3 3 | 4 | In this document we list all the pieces of code included by ORB-SLAM3 and linked libraries which are not property of the authors of ORB-SLAM3. 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 | * *MLPnPsolver.h, MLPnPsolver.cc*. 17 | This is a modified version of the MLPnP of Steffen Urban from [here](https://github.com/urbste/opengv). 18 | The original code is BSD licensed. 19 | 20 | * Function *ORBmatcher::DescriptorDistance* in *ORBmatcher.cc*. 21 | The code is from: http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel. 22 | The code is in the public domain. 23 | 24 | #####Code in Thirdparty folder 25 | 26 | * All code in **DBoW2** folder. 27 | 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. 28 | 29 | * All code in **g2o** folder. 30 | This is a modified version of [g2o](https://github.com/RainerKuemmerle/g2o). All files included are BSD licensed. 31 | 32 | #####Library dependencies 33 | 34 | * **Pangolin (visualization and user interface)**. 35 | [MIT license](https://en.wikipedia.org/wiki/MIT_License). 36 | 37 | * **OpenCV**. 38 | BSD license. 39 | 40 | * **Eigen3**. 41 | For versions greater than 3.1.1 is MPL2, earlier versions are LGPLv3. 42 | 43 | * **ROS (Optional, only if you build Examples/ROS)**. 44 | 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. 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /Examples/Monocular-Inertial/mono_inertial_euroc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VealFang/PLI-SLAM/28b84b9091183c277baf0f2f044617f41a572d09/Examples/Monocular-Inertial/mono_inertial_euroc -------------------------------------------------------------------------------- /Examples/Monocular/mono_euroc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VealFang/PLI-SLAM/28b84b9091183c277baf0f2f044617f41a572d09/Examples/Monocular/mono_euroc -------------------------------------------------------------------------------- /Examples/Monocular/mono_kitti: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VealFang/PLI-SLAM/28b84b9091183c277baf0f2f044617f41a572d09/Examples/Monocular/mono_kitti -------------------------------------------------------------------------------- /Examples/Monocular/mono_tum: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VealFang/PLI-SLAM/28b84b9091183c277baf0f2f044617f41a572d09/Examples/Monocular/mono_tum -------------------------------------------------------------------------------- /Examples/RGB-D/rgbd_tum: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VealFang/PLI-SLAM/28b84b9091183c277baf0f2f044617f41a572d09/Examples/RGB-D/rgbd_tum -------------------------------------------------------------------------------- /Examples/ROS/PLI_SLAM2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | rosbuild_init() 5 | 6 | IF(NOT ROS_BUILD_TYPE) 7 | SET(ROS_BUILD_TYPE Release) 8 | ENDIF() 9 | 10 | MESSAGE("Build type: " ${ROS_BUILD_TYPE}) 11 | 12 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 13 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 14 | 15 | # Check C++11 or C++0x support 16 | include(CheckCXXCompilerFlag) 17 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 18 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 19 | if(COMPILER_SUPPORTS_CXX11) 20 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 21 | add_definitions(-DCOMPILEDWITHC11) 22 | message(STATUS "Using flag -std=c++11.") 23 | elseif(COMPILER_SUPPORTS_CXX0X) 24 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 25 | add_definitions(-DCOMPILEDWITHC0X) 26 | message(STATUS "Using flag -std=c++0x.") 27 | else() 28 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 29 | endif() 30 | 31 | LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../../../cmake_modules) 32 | 33 | find_package(OpenCV 3.0 QUIET) 34 | if(NOT OpenCV_FOUND) 35 | find_package(OpenCV 2.4.3 QUIET) 36 | if(NOT OpenCV_FOUND) 37 | message(FATAL_ERROR "OpenCV > 2.4.3 not found.") 38 | endif() 39 | endif() 40 | 41 | find_package(Eigen3 3.1.0 REQUIRED) 42 | find_package(Pangolin REQUIRED) 43 | 44 | include_directories( 45 | ${PROJECT_SOURCE_DIR} 46 | ${PROJECT_SOURCE_DIR}/../../../ 47 | ${PROJECT_SOURCE_DIR}/../../../include 48 | ${PROJECT_SOURCE_DIR}/../../../include/CameraModels 49 | ${Pangolin_INCLUDE_DIRS} 50 | ) 51 | 52 | set(LIBS 53 | ${OpenCV_LIBS} 54 | ${EIGEN3_LIBS} 55 | ${Pangolin_LIBRARIES} 56 | ${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so 57 | ${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so 58 | ${PROJECT_SOURCE_DIR}/../../../lib/libPLI_SLAM.so 59 | -lboost_system 60 | ) 61 | 62 | # Node for monocular camera 63 | rosbuild_add_executable(Mono 64 | src/ros_mono.cc 65 | ) 66 | 67 | target_link_libraries(Mono 68 | ${LIBS} 69 | ) 70 | 71 | # Node for monocular camera (Augmented Reality Demo) 72 | rosbuild_add_executable(MonoAR 73 | src/AR/ros_mono_ar.cc 74 | src/AR/ViewerAR.h 75 | src/AR/ViewerAR.cc 76 | ) 77 | 78 | target_link_libraries(MonoAR 79 | ${LIBS} 80 | ) 81 | 82 | # Node for stereo camera 83 | rosbuild_add_executable(Stereo 84 | src/ros_stereo.cc 85 | ) 86 | 87 | target_link_libraries(Stereo 88 | ${LIBS} 89 | ) 90 | 91 | # Node for RGB-D camera 92 | rosbuild_add_executable(RGBD 93 | src/ros_rgbd.cc 94 | ) 95 | 96 | target_link_libraries(RGBD 97 | ${LIBS} 98 | ) 99 | 100 | # Node for monocular-inertial camera 101 | rosbuild_add_executable(Mono_Inertial 102 | src/ros_mono_inertial.cc 103 | ) 104 | 105 | target_link_libraries(Mono_Inertial 106 | ${LIBS} 107 | ) 108 | 109 | # Node for stereo-inertial camera 110 | rosbuild_add_executable(Stereo_Inertial 111 | src/ros_stereo_inertial.cc 112 | ) 113 | 114 | target_link_libraries(Stereo_Inertial 115 | ${LIBS} 116 | ) 117 | -------------------------------------------------------------------------------- /Examples/ROS/PLI_SLAM2/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ORB_SLAM3 4 | 5 | Carlos Campos, Richard Elvira, Juan J. Gomez, Jose M.M. Montiel, Juan D. Tardos 6 | GPLv3 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /Examples/ROS/PLI_SLAM2/src/AR/ViewerAR.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef VIEWERAR_H 21 | #define VIEWERAR_H 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include"../../../include/System.h" 28 | 29 | namespace ORB_SLAM3 30 | { 31 | 32 | class Plane 33 | { 34 | public: 35 | Plane(const std::vector &vMPs, const cv::Mat &Tcw); 36 | Plane(const float &nx, const float &ny, const float &nz, const float &ox, const float &oy, const float &oz); 37 | 38 | void Recompute(); 39 | 40 | //normal 41 | cv::Mat n; 42 | //origin 43 | cv::Mat o; 44 | //arbitrary orientation along normal 45 | float rang; 46 | //transformation from world to the plane 47 | cv::Mat Tpw; 48 | pangolin::OpenGlMatrix glTpw; 49 | //MapPoints that define the plane 50 | std::vector mvMPs; 51 | //camera pose when the plane was first observed (to compute normal direction) 52 | cv::Mat mTcw, XC; 53 | }; 54 | 55 | class ViewerAR 56 | { 57 | public: 58 | ViewerAR(); 59 | 60 | void SetFPS(const float fps){ 61 | mFPS = fps; 62 | mT=1e3/fps; 63 | } 64 | 65 | void SetSLAM(ORB_SLAM3::System* pSystem){ 66 | mpSystem = pSystem; 67 | } 68 | 69 | // Main thread function. 70 | void Run(); 71 | 72 | void SetCameraCalibration(const float &fx_, const float &fy_, const float &cx_, const float &cy_){ 73 | fx = fx_; fy = fy_; cx = cx_; cy = cy_; 74 | } 75 | 76 | void SetImagePose(const cv::Mat &im, const cv::Mat &Tcw, const int &status, 77 | const std::vector &vKeys, const std::vector &vMPs); 78 | 79 | void GetImagePose(cv::Mat &im, cv::Mat &Tcw, int &status, 80 | std::vector &vKeys, std::vector &vMPs); 81 | 82 | private: 83 | 84 | //SLAM 85 | ORB_SLAM3::System* mpSystem; 86 | 87 | void PrintStatus(const int &status, const bool &bLocMode, cv::Mat &im); 88 | void AddTextToImage(const std::string &s, cv::Mat &im, const int r=0, const int g=0, const int b=0); 89 | void LoadCameraPose(const cv::Mat &Tcw); 90 | void DrawImageTexture(pangolin::GlTexture &imageTexture, cv::Mat &im); 91 | void DrawCube(const float &size, const float x=0, const float y=0, const float z=0); 92 | void DrawPlane(int ndivs, float ndivsize); 93 | void DrawPlane(Plane* pPlane, int ndivs, float ndivsize); 94 | void DrawTrackedPoints(const std::vector &vKeys, const std::vector &vMPs, cv::Mat &im); 95 | 96 | Plane* DetectPlane(const cv::Mat Tcw, const std::vector &vMPs, const int iterations=50); 97 | 98 | // frame rate 99 | float mFPS, mT; 100 | float fx,fy,cx,cy; 101 | 102 | // Last processed image and computed pose by the SLAM 103 | std::mutex mMutexPoseImage; 104 | cv::Mat mTcw; 105 | cv::Mat mImage; 106 | int mStatus; 107 | std::vector mvKeys; 108 | std::vector mvMPs; 109 | 110 | }; 111 | 112 | 113 | } 114 | 115 | 116 | #endif // VIEWERAR_H 117 | 118 | 119 | -------------------------------------------------------------------------------- /Examples/ROS/PLI_SLAM2/src/ros_mono.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | 30 | #include"../../../include/System.h" 31 | 32 | using namespace std; 33 | 34 | class ImageGrabber 35 | { 36 | public: 37 | ImageGrabber(ORB_SLAM3::System* pSLAM):mpSLAM(pSLAM){} 38 | 39 | void GrabImage(const sensor_msgs::ImageConstPtr& msg); 40 | 41 | ORB_SLAM3::System* mpSLAM; 42 | }; 43 | 44 | int main(int argc, char **argv) 45 | { 46 | ros::init(argc, argv, "Mono"); 47 | ros::start(); 48 | 49 | if(argc != 3) 50 | { 51 | cerr << endl << "Usage: rosrun ORB_SLAM3 Mono path_to_vocabulary path_to_settings" << endl; 52 | ros::shutdown(); 53 | return 1; 54 | } 55 | 56 | // Create SLAM system. It initializes all system threads and gets ready to process frames. 57 | ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::MONOCULAR,true); 58 | 59 | ImageGrabber igb(&SLAM); 60 | 61 | ros::NodeHandle nodeHandler; 62 | ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb); 63 | 64 | ros::spin(); 65 | 66 | // Stop all threads 67 | SLAM.Shutdown(); 68 | 69 | // Save camera trajectory 70 | SLAM.SaveTrajectoryTUM("CamTraj_TUM.txt"); 71 | SLAM.SaveKeyFrameTrajectoryTUM("KFTraj_TUM.txt"); 72 | 73 | ros::shutdown(); 74 | 75 | return 0; 76 | } 77 | 78 | void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg) 79 | { 80 | // Copy the ros image message to cv::Mat. 81 | cv_bridge::CvImageConstPtr cv_ptr; 82 | try 83 | { 84 | cv_ptr = cv_bridge::toCvShare(msg); 85 | } 86 | catch (cv_bridge::Exception& e) 87 | { 88 | ROS_ERROR("cv_bridge exception: %s", e.what()); 89 | return; 90 | } 91 | 92 | mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec()); 93 | } 94 | 95 | 96 | -------------------------------------------------------------------------------- /Examples/ROS/PLI_SLAM2/src/ros_rgbd.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | 33 | #include"../../../include/System.h" 34 | 35 | using namespace std; 36 | 37 | class ImageGrabber 38 | { 39 | public: 40 | ImageGrabber(ORB_SLAM3::System* pSLAM):mpSLAM(pSLAM){} 41 | 42 | void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD); 43 | 44 | ORB_SLAM3::System* mpSLAM; 45 | }; 46 | 47 | int main(int argc, char **argv) 48 | { 49 | ros::init(argc, argv, "RGBD"); 50 | ros::start(); 51 | 52 | if(argc != 3) 53 | { 54 | cerr << endl << "Usage: rosrun ORB_SLAM3 RGBD path_to_vocabulary path_to_settings" << endl; 55 | ros::shutdown(); 56 | return 1; 57 | } 58 | 59 | // Create SLAM system. It initializes all system threads and gets ready to process frames. 60 | ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::RGBD,true); 61 | 62 | ImageGrabber igb(&SLAM); 63 | 64 | ros::NodeHandle nh; 65 | 66 | message_filters::Subscriber rgb_sub(nh, "/camera/rgb/image_raw", 100); 67 | message_filters::Subscriber depth_sub(nh, "camera/depth_registered/image_raw", 100); 68 | typedef message_filters::sync_policies::ApproximateTime sync_pol; 69 | message_filters::Synchronizer sync(sync_pol(10), rgb_sub,depth_sub); 70 | sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2)); 71 | 72 | ros::spin(); 73 | 74 | // Stop all threads 75 | SLAM.Shutdown(); 76 | 77 | // Save camera trajectory 78 | SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); 79 | 80 | ros::shutdown(); 81 | 82 | return 0; 83 | } 84 | 85 | void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD) 86 | { 87 | // Copy the ros image message to cv::Mat. 88 | cv_bridge::CvImageConstPtr cv_ptrRGB; 89 | try 90 | { 91 | cv_ptrRGB = cv_bridge::toCvShare(msgRGB); 92 | } 93 | catch (cv_bridge::Exception& e) 94 | { 95 | ROS_ERROR("cv_bridge exception: %s", e.what()); 96 | return; 97 | } 98 | 99 | cv_bridge::CvImageConstPtr cv_ptrD; 100 | try 101 | { 102 | cv_ptrD = cv_bridge::toCvShare(msgD); 103 | } 104 | catch (cv_bridge::Exception& e) 105 | { 106 | ROS_ERROR("cv_bridge exception: %s", e.what()); 107 | return; 108 | } 109 | mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec()); 110 | } 111 | 112 | 113 | -------------------------------------------------------------------------------- /Examples/Stereo-Inertial/stereo_inertial_euroc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VealFang/PLI-SLAM/28b84b9091183c277baf0f2f044617f41a572d09/Examples/Stereo-Inertial/stereo_inertial_euroc -------------------------------------------------------------------------------- /Examples/Stereo/stereo_euroc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VealFang/PLI-SLAM/28b84b9091183c277baf0f2f044617f41a572d09/Examples/Stereo/stereo_euroc -------------------------------------------------------------------------------- /Examples/Stereo/stereo_kitti: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VealFang/PLI-SLAM/28b84b9091183c277baf0f2f044617f41a572d09/Examples/Stereo/stereo_kitti -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PLI-SLAM: A Stereo Visual-Inertial SLAM based on Point-Line Features 2 | PLI-SLAM is developed on the foundation of [PL-SLAM](http://github.com/rubengooj/pl-slam) and [ORB_SLAM3](http://github.com/UZ-SLAMLab/ORB_SLAM3), line features are fully engaged in the whole process of the system including tracking, map building and loop detection. 3 | Higher accuracy has been shown in PLI-SLAM on EuRoC Dataset compared to other VIO or point-line based SLAM systems. PLI-SLAM also performed stronger tracking robustness in various real scenario experiments conduted with a self-developed robot. 4 | 5 | **Video demo**: [youtube](https://youtu.be/UuJuEZoYsso) 6 | 7 | ## 1. License: 8 | The provided code is published under the General Public License Version 3 (GPL v3). 9 | ## 2. Prerequisites: 10 | ### Ubuntu 11 | Ubuntu 16.04 or higher version is required. 12 | ### ROS 13 | We use ROS Melodic for real scenario experiments. 14 | ### C++11 or C++0x Compiler 15 | We use the new thread and chrono functionalities of C++11. 16 | ### Pangolin 17 | We use Pangolin for visualization and user interface. It can be found at: https://github.com/stevenlovegrove/Pangolin. 18 | ### OpenCV 19 | We use OpenCV 3.3.1 to manipulate images and features. 20 | Noted that [line_descripter](https://github.com/opencv/opencv_contrib/tree/master/modules/line_descriptor) should be impoerted from the [OpenCV/contrib](https://github.com/opencv/opencv_contrib) library independently. 21 | ### Eigen3 22 | We use Eigen 3.3.4. It can be found at: http://eigen.tuxfamily.org. 23 | ### Ceres Solver 24 | We use ceres-solver-1.14.0 for nonlinear optimization. It can be found at: https://ceres-solver.googlesource.com/ceres-solver. 25 | ### DBoW2 and g2o 26 | We use modified versions of the [DBoW2](https://github.com/dorian3d/DBoW2) library to perform place recognition and [g2o](https://github.com/RainerKuemmerle/g2o) library to perform non-linear optimizations. 27 | ## 3. Building PLI-SLAM library and examples on ROS 28 | 1. Clone the repository and add the path to the ROS_PACKAGE_PATH environment variable: 29 | ```Bash 30 | git clone https://github.com/VealFang/PLI-SLAM.git 31 | gedit ~/.bashrc #Open .bashrc file 32 | export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM3/Examples/ROS #Add at the end of the file 33 | source ~/.bashrc 34 | ``` 35 | 2. Execute ``build_ros.sh`` script: 36 | ```Bash 37 | cd PLI_SLAM 38 | chmod +x build_ros.sh 39 | ./build_ros.sh 40 | ``` 41 | ## 4. Run on EuRoC dataset 42 | Download a [rosbag](http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) from the EuRoC dataset. 43 | Open 3 tabs on the terminal and run the following command at each tab: 44 | ```Bash 45 | roscore 46 | ``` 47 | ```Bash 48 | roslaunch PLI_SLAM Stereo_Inertial Vocabulary Examples/Stereo-Inertial/EuRoC.yaml true 49 | ``` 50 | ```Bash 51 | rosbag play ROSBAG_PATH 52 | ``` 53 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(DBoW2) 3 | 4 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 6 | 7 | set(HDRS_DBOW2 8 | DBoW2/BowVector.h 9 | DBoW2/FORB.h 10 | DBoW2/FClass.h 11 | DBoW2/FeatureVector.h 12 | DBoW2/ScoringObject.h 13 | DBoW2/TemplatedVocabulary.h) 14 | set(SRCS_DBOW2 15 | DBoW2/BowVector.cpp 16 | DBoW2/FORB.cpp 17 | DBoW2/FeatureVector.cpp 18 | DBoW2/ScoringObject.cpp) 19 | 20 | set(HDRS_DUTILS 21 | DUtils/Random.h 22 | DUtils/Timestamp.h) 23 | set(SRCS_DUTILS 24 | DUtils/Random.cpp 25 | DUtils/Timestamp.cpp) 26 | 27 | find_package(OpenCV 3.0 QUIET) 28 | if(NOT OpenCV_FOUND) 29 | find_package(OpenCV 2.4.3 QUIET) 30 | if(NOT OpenCV_FOUND) 31 | message(FATAL_ERROR "OpenCV > 2.4.3 not found.") 32 | endif() 33 | endif() 34 | 35 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 36 | 37 | include_directories(${OpenCV_INCLUDE_DIRS}) 38 | add_library(DBoW2 SHARED ${SRCS_DBOW2} ${SRCS_DUTILS}) 39 | target_link_libraries(DBoW2 ${OpenCV_LIBS}) 40 | 41 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/BowVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.cpp 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "BowVector.h" 17 | 18 | namespace DBoW2 { 19 | 20 | // -------------------------------------------------------------------------- 21 | 22 | BowVector::BowVector(void) 23 | { 24 | } 25 | 26 | // -------------------------------------------------------------------------- 27 | 28 | BowVector::~BowVector(void) 29 | { 30 | } 31 | 32 | // -------------------------------------------------------------------------- 33 | 34 | void BowVector::addWeight(WordId id, WordValue v) 35 | { 36 | BowVector::iterator vit = this->lower_bound(id); 37 | 38 | if(vit != this->end() && !(this->key_comp()(id, vit->first))) 39 | { 40 | vit->second += v; 41 | } 42 | else 43 | { 44 | this->insert(vit, BowVector::value_type(id, v)); 45 | } 46 | } 47 | 48 | // -------------------------------------------------------------------------- 49 | 50 | void BowVector::addIfNotExist(WordId id, WordValue v) 51 | { 52 | BowVector::iterator vit = this->lower_bound(id); 53 | 54 | if(vit == this->end() || (this->key_comp()(id, vit->first))) 55 | { 56 | this->insert(vit, BowVector::value_type(id, v)); 57 | } 58 | } 59 | 60 | // -------------------------------------------------------------------------- 61 | 62 | void BowVector::normalize(LNorm norm_type) 63 | { 64 | double norm = 0.0; 65 | BowVector::iterator it; 66 | 67 | if(norm_type == DBoW2::L1) 68 | { 69 | for(it = begin(); it != end(); ++it) 70 | norm += fabs(it->second); 71 | } 72 | else 73 | { 74 | for(it = begin(); it != end(); ++it) 75 | norm += it->second * it->second; 76 | norm = sqrt(norm); 77 | } 78 | 79 | if(norm > 0.0) 80 | { 81 | for(it = begin(); it != end(); ++it) 82 | it->second /= norm; 83 | } 84 | } 85 | 86 | // -------------------------------------------------------------------------- 87 | 88 | std::ostream& operator<< (std::ostream &out, const BowVector &v) 89 | { 90 | BowVector::const_iterator vit; 91 | std::vector::const_iterator iit; 92 | unsigned int i = 0; 93 | const unsigned int N = v.size(); 94 | for(vit = v.begin(); vit != v.end(); ++vit, ++i) 95 | { 96 | out << "<" << vit->first << ", " << vit->second << ">"; 97 | 98 | if(i < N-1) out << ", "; 99 | } 100 | return out; 101 | } 102 | 103 | // -------------------------------------------------------------------------- 104 | 105 | void BowVector::saveM(const std::string &filename, size_t W) const 106 | { 107 | std::fstream f(filename.c_str(), std::ios::out); 108 | 109 | WordId last = 0; 110 | BowVector::const_iterator bit; 111 | for(bit = this->begin(); bit != this->end(); ++bit) 112 | { 113 | for(; last < bit->first; ++last) 114 | { 115 | f << "0 "; 116 | } 117 | f << bit->second << " "; 118 | 119 | last = bit->first + 1; 120 | } 121 | for(; last < (WordId)W; ++last) 122 | f << "0 "; 123 | 124 | f.close(); 125 | } 126 | 127 | // -------------------------------------------------------------------------- 128 | 129 | } // namespace DBoW2 130 | 131 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/BowVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.h 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_BOW_VECTOR__ 11 | #define __D_T_BOW_VECTOR__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | namespace DBoW2 { 21 | 22 | /// Id of words 23 | typedef unsigned int WordId; 24 | 25 | /// Value of a word 26 | typedef double WordValue; 27 | 28 | /// Id of nodes in the vocabulary treee 29 | typedef unsigned int NodeId; 30 | 31 | /// L-norms for normalization 32 | enum LNorm 33 | { 34 | L1, 35 | L2 36 | }; 37 | 38 | /// Weighting type 39 | enum WeightingType 40 | { 41 | TF_IDF, 42 | TF, 43 | IDF, 44 | BINARY 45 | }; 46 | 47 | /// Scoring type 48 | enum ScoringType 49 | { 50 | L1_NORM, 51 | L2_NORM, 52 | CHI_SQUARE, 53 | KL, 54 | BHATTACHARYYA, 55 | DOT_PRODUCT, 56 | }; 57 | 58 | /// Vector of words to represent images 59 | class BowVector: 60 | public std::map 61 | { 62 | friend class boost::serialization::access; 63 | template 64 | void serialize(Archive& ar, const int version) 65 | { 66 | ar & boost::serialization::base_object >(*this); 67 | } 68 | 69 | public: 70 | 71 | /** 72 | * Constructor 73 | */ 74 | BowVector(void); 75 | 76 | /** 77 | * Destructor 78 | */ 79 | ~BowVector(void); 80 | 81 | /** 82 | * Adds a value to a word value existing in the vector, or creates a new 83 | * word with the given value 84 | * @param id word id to look for 85 | * @param v value to create the word with, or to add to existing word 86 | */ 87 | void addWeight(WordId id, WordValue v); 88 | 89 | /** 90 | * Adds a word with a value to the vector only if this does not exist yet 91 | * @param id word id to look for 92 | * @param v value to give to the word if this does not exist 93 | */ 94 | void addIfNotExist(WordId id, WordValue v); 95 | 96 | /** 97 | * L1-Normalizes the values in the vector 98 | * @param norm_type norm used 99 | */ 100 | void normalize(LNorm norm_type); 101 | 102 | /** 103 | * Prints the content of the bow vector 104 | * @param out stream 105 | * @param v 106 | */ 107 | friend std::ostream& operator<<(std::ostream &out, const BowVector &v); 108 | 109 | /** 110 | * Saves the bow vector as a vector in a matlab file 111 | * @param filename 112 | * @param W number of words in the vocabulary 113 | */ 114 | void saveM(const std::string &filename, size_t W) const; 115 | }; 116 | 117 | } // namespace DBoW2 118 | 119 | #endif 120 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/FClass.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FClass.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: generic FClass to instantiate templated classes 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FCLASS__ 11 | #define __D_T_FCLASS__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Generic class to encapsulate functions to manage descriptors. 20 | /** 21 | * This class must be inherited. Derived classes can be used as the 22 | * parameter F when creating Templated structures 23 | * (TemplatedVocabulary, TemplatedDatabase, ...) 24 | */ 25 | class FClass 26 | { 27 | class TDescriptor; 28 | typedef const TDescriptor *pDescriptor; 29 | 30 | /** 31 | * Calculates the mean value of a set of descriptors 32 | * @param descriptors 33 | * @param mean mean descriptor 34 | */ 35 | virtual void meanValue(const std::vector &descriptors, 36 | TDescriptor &mean) = 0; 37 | 38 | /** 39 | * Calculates the distance between two descriptors 40 | * @param a 41 | * @param b 42 | * @return distance 43 | */ 44 | static double distance(const TDescriptor &a, const TDescriptor &b); 45 | 46 | /** 47 | * Returns a string version of the descriptor 48 | * @param a descriptor 49 | * @return string version 50 | */ 51 | static std::string toString(const TDescriptor &a); 52 | 53 | /** 54 | * Returns a descriptor from a string 55 | * @param a descriptor 56 | * @param s string version 57 | */ 58 | static void fromString(TDescriptor &a, const std::string &s); 59 | 60 | /** 61 | * Returns a mat with the descriptors in float format 62 | * @param descriptors 63 | * @param mat (out) NxL 32F matrix 64 | */ 65 | static void toMat32F(const std::vector &descriptors, 66 | cv::Mat &mat); 67 | }; 68 | 69 | } // namespace DBoW2 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/FORB.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FORB.h 3 | * Date: June 2012 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for ORB descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_F_ORB__ 11 | #define __D_T_F_ORB__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "FClass.h" 18 | 19 | namespace DBoW2 { 20 | 21 | /// Functions to manipulate ORB descriptors 22 | class FORB: protected FClass 23 | { 24 | public: 25 | 26 | /// Descriptor type 27 | typedef cv::Mat TDescriptor; // CV_8U 28 | /// Pointer to a single descriptor 29 | typedef const TDescriptor *pDescriptor; 30 | /// Descriptor length (in bytes) 31 | static const int L; 32 | 33 | /** 34 | * Calculates the mean value of a set of descriptors 35 | * @param descriptors 36 | * @param mean mean descriptor 37 | */ 38 | static void meanValue(const std::vector &descriptors, 39 | TDescriptor &mean); 40 | 41 | /** 42 | * Calculates the distance between two descriptors 43 | * @param a 44 | * @param b 45 | * @return distance 46 | */ 47 | static int distance(const TDescriptor &a, const TDescriptor &b); 48 | 49 | /** 50 | * Returns a string version of the descriptor 51 | * @param a descriptor 52 | * @return string version 53 | */ 54 | static std::string toString(const TDescriptor &a); 55 | 56 | /** 57 | * Returns a descriptor from a string 58 | * @param a descriptor 59 | * @param s string version 60 | */ 61 | static void fromString(TDescriptor &a, const std::string &s); 62 | 63 | /** 64 | * Returns a mat with the descriptors in float format 65 | * @param descriptors 66 | * @param mat (out) NxL 32F matrix 67 | */ 68 | static void toMat32F(const std::vector &descriptors, 69 | cv::Mat &mat); 70 | 71 | static void toMat8U(const std::vector &descriptors, 72 | cv::Mat &mat); 73 | 74 | }; 75 | 76 | } // namespace DBoW2 77 | 78 | #endif 79 | 80 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/FeatureVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.cpp 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include "FeatureVector.h" 11 | #include 12 | #include 13 | #include 14 | 15 | namespace DBoW2 { 16 | 17 | // --------------------------------------------------------------------------- 18 | 19 | FeatureVector::FeatureVector(void) 20 | { 21 | } 22 | 23 | // --------------------------------------------------------------------------- 24 | 25 | FeatureVector::~FeatureVector(void) 26 | { 27 | } 28 | 29 | // --------------------------------------------------------------------------- 30 | 31 | void FeatureVector::addFeature(NodeId id, unsigned int i_feature) 32 | { 33 | FeatureVector::iterator vit = this->lower_bound(id); 34 | 35 | if(vit != this->end() && vit->first == id) 36 | { 37 | vit->second.push_back(i_feature); 38 | } 39 | else 40 | { 41 | vit = this->insert(vit, FeatureVector::value_type(id, 42 | std::vector() )); 43 | vit->second.push_back(i_feature); 44 | } 45 | } 46 | 47 | // --------------------------------------------------------------------------- 48 | 49 | std::ostream& operator<<(std::ostream &out, 50 | const FeatureVector &v) 51 | { 52 | if(!v.empty()) 53 | { 54 | FeatureVector::const_iterator vit = v.begin(); 55 | 56 | const std::vector* f = &vit->second; 57 | 58 | out << "<" << vit->first << ": ["; 59 | if(!f->empty()) out << (*f)[0]; 60 | for(unsigned int i = 1; i < f->size(); ++i) 61 | { 62 | out << ", " << (*f)[i]; 63 | } 64 | out << "]>"; 65 | 66 | for(++vit; vit != v.end(); ++vit) 67 | { 68 | f = &vit->second; 69 | 70 | out << ", <" << vit->first << ": ["; 71 | if(!f->empty()) out << (*f)[0]; 72 | for(unsigned int i = 1; i < f->size(); ++i) 73 | { 74 | out << ", " << (*f)[i]; 75 | } 76 | out << "]>"; 77 | } 78 | } 79 | 80 | return out; 81 | } 82 | 83 | // --------------------------------------------------------------------------- 84 | 85 | } // namespace DBoW2 86 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/FeatureVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FEATURE_VECTOR__ 11 | #define __D_T_FEATURE_VECTOR__ 12 | 13 | #include "BowVector.h" 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | namespace DBoW2 { 22 | 23 | /// Vector of nodes with indexes of local features 24 | class FeatureVector: 25 | public std::map > 26 | { 27 | friend class boost::serialization::access; 28 | template 29 | void serialize(Archive& ar, const int version) 30 | { 31 | ar & boost::serialization::base_object > >(*this); 32 | } 33 | 34 | public: 35 | 36 | /** 37 | * Constructor 38 | */ 39 | FeatureVector(void); 40 | 41 | /** 42 | * Destructor 43 | */ 44 | ~FeatureVector(void); 45 | 46 | /** 47 | * Adds a feature to an existing node, or adds a new node with an initial 48 | * feature 49 | * @param id node id to add or to modify 50 | * @param i_feature index of feature to add to the given node 51 | */ 52 | void addFeature(NodeId id, unsigned int i_feature); 53 | 54 | /** 55 | * Sends a string versions of the feature vector through the stream 56 | * @param out stream 57 | * @param v feature vector 58 | */ 59 | friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v); 60 | 61 | }; 62 | 63 | } // namespace DBoW2 64 | 65 | #endif 66 | 67 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/ScoringObject.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: ScoringObject.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions to compute bow scores 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_SCORING_OBJECT__ 11 | #define __D_T_SCORING_OBJECT__ 12 | 13 | #include "BowVector.h" 14 | 15 | namespace DBoW2 { 16 | 17 | /// Base class of scoring functions 18 | class GeneralScoring 19 | { 20 | public: 21 | /** 22 | * Computes the score between two vectors. Vectors must be sorted and 23 | * normalized if necessary 24 | * @param v (in/out) 25 | * @param w (in/out) 26 | * @return score 27 | */ 28 | virtual double score(const BowVector &v, const BowVector &w) const = 0; 29 | 30 | /** 31 | * Returns whether a vector must be normalized before scoring according 32 | * to the scoring scheme 33 | * @param norm norm to use 34 | * @return true iff must normalize 35 | */ 36 | virtual bool mustNormalize(LNorm &norm) const = 0; 37 | 38 | /// Log of epsilon 39 | static const double LOG_EPS; 40 | // If you change the type of WordValue, make sure you change also the 41 | // epsilon value (this is needed by the KL method) 42 | 43 | virtual ~GeneralScoring() {} //!< Required for virtual base classes 44 | 45 | }; 46 | 47 | /** 48 | * Macro for defining Scoring classes 49 | * @param NAME name of class 50 | * @param MUSTNORMALIZE if vectors must be normalized to compute the score 51 | * @param NORM type of norm to use when MUSTNORMALIZE 52 | */ 53 | #define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ 54 | NAME: public GeneralScoring \ 55 | { public: \ 56 | /** \ 57 | * Computes score between two vectors \ 58 | * @param v \ 59 | * @param w \ 60 | * @return score between v and w \ 61 | */ \ 62 | virtual double score(const BowVector &v, const BowVector &w) const; \ 63 | \ 64 | /** \ 65 | * Says if a vector must be normalized according to the scoring function \ 66 | * @param norm (out) if true, norm to use 67 | * @return true iff vectors must be normalized \ 68 | */ \ 69 | virtual inline bool mustNormalize(LNorm &norm) const \ 70 | { norm = NORM; return MUSTNORMALIZE; } \ 71 | } 72 | 73 | /// L1 Scoring object 74 | class __SCORING_CLASS(L1Scoring, true, L1); 75 | 76 | /// L2 Scoring object 77 | class __SCORING_CLASS(L2Scoring, true, L2); 78 | 79 | /// Chi square Scoring object 80 | class __SCORING_CLASS(ChiSquareScoring, true, L1); 81 | 82 | /// KL divergence Scoring object 83 | class __SCORING_CLASS(KLScoring, true, L1); 84 | 85 | /// Bhattacharyya Scoring object 86 | class __SCORING_CLASS(BhattacharyyaScoring, true, L1); 87 | 88 | /// Dot product Scoring object 89 | class __SCORING_CLASS(DotProductScoring, false, L1); 90 | 91 | #undef __SCORING_CLASS 92 | 93 | } // namespace DBoW2 94 | 95 | #endif 96 | 97 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DUtils/Random.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Random.cpp 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: April 2010 6 | * Description: manages pseudo-random numbers 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #include "Random.h" 12 | #include "Timestamp.h" 13 | #include 14 | using namespace std; 15 | 16 | bool DUtils::Random::m_already_seeded = false; 17 | 18 | void DUtils::Random::SeedRand(){ 19 | Timestamp time; 20 | time.setToCurrentTime(); 21 | srand((unsigned)time.getFloatTime()); 22 | } 23 | 24 | void DUtils::Random::SeedRandOnce() 25 | { 26 | if(!m_already_seeded) 27 | { 28 | DUtils::Random::SeedRand(); 29 | m_already_seeded = true; 30 | } 31 | } 32 | 33 | void DUtils::Random::SeedRand(int seed) 34 | { 35 | srand(seed); 36 | } 37 | 38 | void DUtils::Random::SeedRandOnce(int seed) 39 | { 40 | if(!m_already_seeded) 41 | { 42 | DUtils::Random::SeedRand(seed); 43 | m_already_seeded = true; 44 | } 45 | } 46 | 47 | int DUtils::Random::RandomInt(int min, int max){ 48 | int d = max - min + 1; 49 | return int(((double)rand()/((double)RAND_MAX + 1.0)) * d) + min; 50 | } 51 | 52 | // --------------------------------------------------------------------------- 53 | // --------------------------------------------------------------------------- 54 | 55 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer(int min, int max) 56 | { 57 | if(min <= max) 58 | { 59 | m_min = min; 60 | m_max = max; 61 | } 62 | else 63 | { 64 | m_min = max; 65 | m_max = min; 66 | } 67 | 68 | createValues(); 69 | } 70 | 71 | // --------------------------------------------------------------------------- 72 | 73 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer 74 | (const DUtils::Random::UnrepeatedRandomizer& rnd) 75 | { 76 | *this = rnd; 77 | } 78 | 79 | // --------------------------------------------------------------------------- 80 | 81 | int DUtils::Random::UnrepeatedRandomizer::get() 82 | { 83 | if(empty()) createValues(); 84 | 85 | DUtils::Random::SeedRandOnce(); 86 | 87 | int k = DUtils::Random::RandomInt(0, m_values.size()-1); 88 | int ret = m_values[k]; 89 | m_values[k] = m_values.back(); 90 | m_values.pop_back(); 91 | 92 | return ret; 93 | } 94 | 95 | // --------------------------------------------------------------------------- 96 | 97 | void DUtils::Random::UnrepeatedRandomizer::createValues() 98 | { 99 | int n = m_max - m_min + 1; 100 | 101 | m_values.resize(n); 102 | for(int i = 0; i < n; ++i) m_values[i] = m_min + i; 103 | } 104 | 105 | // --------------------------------------------------------------------------- 106 | 107 | void DUtils::Random::UnrepeatedRandomizer::reset() 108 | { 109 | if((int)m_values.size() != m_max - m_min + 1) createValues(); 110 | } 111 | 112 | // --------------------------------------------------------------------------- 113 | 114 | DUtils::Random::UnrepeatedRandomizer& 115 | DUtils::Random::UnrepeatedRandomizer::operator= 116 | (const DUtils::Random::UnrepeatedRandomizer& rnd) 117 | { 118 | if(this != &rnd) 119 | { 120 | this->m_min = rnd.m_min; 121 | this->m_max = rnd.m_max; 122 | this->m_values = rnd.m_values; 123 | } 124 | return *this; 125 | } 126 | 127 | // --------------------------------------------------------------------------- 128 | 129 | 130 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/README.txt: -------------------------------------------------------------------------------- 1 | You should have received this DBoW2 version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2). 2 | See the original DBoW2 library at: https://github.com/dorian3d/DBoW2 3 | All files included in this version are BSD, see LICENSE.txt 4 | 5 | We also use Random.h, Random.cpp, Timestamp.pp and Timestamp.h from DLib/DUtils. 6 | See the original DLib library at: https://github.com/dorian3d/DLib 7 | All files included in this version are BSD, see LICENSE.txt 8 | -------------------------------------------------------------------------------- /Thirdparty/g2o/README.txt: -------------------------------------------------------------------------------- 1 | You should have received this g2o version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2). 2 | See the original g2o library at: https://github.com/RainerKuemmerle/g2o 3 | All files included in this g2o version are BSD, see license-bsd.txt 4 | -------------------------------------------------------------------------------- /Thirdparty/g2o/cmake_modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | # specific additional paths for some OS 65 | if (WIN32) 66 | set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include") 67 | endif(WIN32) 68 | 69 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 70 | PATHS 71 | ${CMAKE_INSTALL_PREFIX}/include 72 | ${EIGEN_ADDITIONAL_SEARCH_PATHS} 73 | ${KDE4_INCLUDE_DIR} 74 | PATH_SUFFIXES eigen3 eigen 75 | ) 76 | 77 | if(EIGEN3_INCLUDE_DIR) 78 | _eigen3_check_version() 79 | endif(EIGEN3_INCLUDE_DIR) 80 | 81 | include(FindPackageHandleStandardArgs) 82 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 83 | 84 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 85 | 86 | endif(EIGEN3_INCLUDE_DIR) 87 | 88 | -------------------------------------------------------------------------------- /Thirdparty/g2o/config.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_CONFIG_H 2 | #define G2O_CONFIG_H 3 | 4 | /* #undef G2O_OPENMP */ 5 | /* #undef G2O_SHARED_LIBS */ 6 | 7 | // give a warning if Eigen defaults to row-major matrices. 8 | // We internally assume column-major matrices throughout the code. 9 | #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR 10 | # error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)" 11 | #endif 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /Thirdparty/g2o/config.h.in: -------------------------------------------------------------------------------- 1 | #ifndef G2O_CONFIG_H 2 | #define G2O_CONFIG_H 3 | 4 | #cmakedefine G2O_OPENMP 1 5 | #cmakedefine G2O_SHARED_LIBS 1 6 | 7 | // give a warning if Eigen defaults to row-major matrices. 8 | // We internally assume column-major matrices throughout the code. 9 | #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR 10 | # error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)" 11 | #endif 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/base_unary_edge.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, 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_BASE_UNARY_EDGE_H 28 | #define G2O_BASE_UNARY_EDGE_H 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "base_edge.h" 35 | #include "robust_kernel.h" 36 | #include "../../config.h" 37 | 38 | namespace g2o { 39 | 40 | using namespace Eigen; 41 | 42 | template 43 | class BaseUnaryEdge : public BaseEdge 44 | { 45 | public: 46 | static const int Dimension = BaseEdge::Dimension; 47 | typedef typename BaseEdge::Measurement Measurement; 48 | typedef VertexXi VertexXiType; 49 | typedef typename Matrix::AlignedMapType JacobianXiOplusType; 50 | typedef typename BaseEdge::ErrorVector ErrorVector; 51 | typedef typename BaseEdge::InformationType InformationType; 52 | 53 | BaseUnaryEdge() : BaseEdge(), 54 | _jacobianOplusXi(0, D, VertexXiType::Dimension) 55 | { 56 | _vertices.resize(1); 57 | } 58 | 59 | virtual void resize(size_t size); 60 | 61 | virtual bool allVerticesFixed() const; 62 | 63 | virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace); 64 | 65 | /** 66 | * Linearizes the oplus operator in the vertex, and stores 67 | * the result in temporary variables _jacobianOplusXi and _jacobianOplusXj 68 | */ 69 | virtual void linearizeOplus(); 70 | 71 | //! returns the result of the linearization in the manifold space for the node xi 72 | const JacobianXiOplusType& jacobianOplusXi() const { return _jacobianOplusXi;} 73 | 74 | virtual void constructQuadraticForm(); 75 | 76 | virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to); 77 | 78 | virtual void mapHessianMemory(double*, int, int, bool) {assert(0 && "BaseUnaryEdge does not map memory of the Hessian");} 79 | 80 | using BaseEdge::resize; 81 | using BaseEdge::computeError; 82 | 83 | protected: 84 | using BaseEdge::_measurement; 85 | using BaseEdge::_information; 86 | using BaseEdge::_error; 87 | using BaseEdge::_vertices; 88 | using BaseEdge::_dimension; 89 | 90 | JacobianXiOplusType _jacobianOplusXi; 91 | 92 | public: 93 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 94 | }; 95 | 96 | #include "base_unary_edge.hpp" 97 | 98 | } // end namespace g2o 99 | 100 | #endif 101 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/base_vertex.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | template 28 | BaseVertex::BaseVertex() : 29 | OptimizableGraph::Vertex(), 30 | _hessian(0, D, D) 31 | { 32 | _dimension = D; 33 | } 34 | 35 | template 36 | double BaseVertex::solveDirect(double lambda) { 37 | Matrix tempA=_hessian + Matrix ::Identity()*lambda; 38 | double det=tempA.determinant(); 39 | if (g2o_isnan(det) || det < std::numeric_limits::epsilon()) 40 | return det; 41 | Matrix dx=tempA.llt().solve(_b); 42 | oplus(&dx[0]); 43 | return det; 44 | } 45 | 46 | template 47 | void BaseVertex::clearQuadraticForm() { 48 | _b.setZero(); 49 | } 50 | 51 | template 52 | void BaseVertex::mapHessianMemory(double* d) 53 | { 54 | new (&_hessian) HessianBlockType(d, D, D); 55 | } 56 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/batch_stats.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "batch_stats.h" 28 | #include 29 | 30 | namespace g2o { 31 | using namespace std; 32 | 33 | G2OBatchStatistics* G2OBatchStatistics::_globalStats=0; 34 | 35 | #ifndef PTHING 36 | #define PTHING(s) \ 37 | #s << "= " << (st.s) << "\t " 38 | #endif 39 | 40 | G2OBatchStatistics::G2OBatchStatistics(){ 41 | // zero all. 42 | memset (this, 0, sizeof(G2OBatchStatistics)); 43 | 44 | // set the iteration to -1 to show that it isn't valid 45 | iteration = -1; 46 | } 47 | 48 | std::ostream& operator << (std::ostream& os , const G2OBatchStatistics& st) 49 | { 50 | os << PTHING(iteration); 51 | 52 | os << PTHING( numVertices ); // how many vertices are involved 53 | os << PTHING( numEdges ); // hoe many edges 54 | os << PTHING( chi2 ); // total chi2 55 | 56 | /** timings **/ 57 | // nonlinear part 58 | os << PTHING( timeResiduals ); 59 | os << PTHING( timeLinearize ); // jacobians 60 | os << PTHING( timeQuadraticForm ); // construct the quadratic form in the graph 61 | 62 | // block_solver (constructs Ax=b, plus maybe schur); 63 | os << PTHING( timeSchurComplement ); // compute schur complement (0 if not done); 64 | 65 | // linear solver (computes Ax=b); ); 66 | os << PTHING( timeSymbolicDecomposition ); // symbolic decomposition (0 if not done); 67 | os << PTHING( timeNumericDecomposition ); // numeric decomposition (0 if not done); 68 | os << PTHING( timeLinearSolution ); // total time for solving Ax=b 69 | os << PTHING( iterationsLinearSolver ); // iterations of PCG 70 | os << PTHING( timeUpdate ); // oplus 71 | os << PTHING( timeIteration ); // total time ); 72 | 73 | os << PTHING( levenbergIterations ); 74 | os << PTHING( timeLinearSolver); 75 | 76 | os << PTHING(hessianDimension); 77 | os << PTHING(hessianPoseDimension); 78 | os << PTHING(hessianLandmarkDimension); 79 | os << PTHING(choleskyNNZ); 80 | os << PTHING(timeMarginals); 81 | 82 | return os; 83 | }; 84 | 85 | void G2OBatchStatistics::setGlobalStats(G2OBatchStatistics* b) 86 | { 87 | _globalStats = b; 88 | } 89 | 90 | } // end namespace 91 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/batch_stats.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_BATCH_STATS_H_ 28 | #define G2O_BATCH_STATS_H_ 29 | 30 | #include 31 | #include 32 | 33 | 34 | namespace g2o { 35 | 36 | /** 37 | * \brief statistics about the optimization 38 | */ 39 | struct G2OBatchStatistics { 40 | G2OBatchStatistics(); 41 | int iteration; ///< which iteration 42 | int numVertices; ///< how many vertices are involved 43 | int numEdges; ///< how many edges 44 | double chi2; ///< total chi2 45 | 46 | /** timings **/ 47 | // nonlinear part 48 | double timeResiduals; ///< residuals 49 | double timeLinearize; ///< jacobians 50 | double timeQuadraticForm; ///< construct the quadratic form in the graph 51 | int levenbergIterations; ///< number of iterations performed by LM 52 | // block_solver (constructs Ax=b, plus maybe schur) 53 | double timeSchurComplement; ///< compute schur complement (0 if not done) 54 | 55 | // linear solver (computes Ax=b); 56 | double timeSymbolicDecomposition; ///< symbolic decomposition (0 if not done) 57 | double timeNumericDecomposition; ///< numeric decomposition (0 if not done) 58 | double timeLinearSolution; ///< total time for solving Ax=b (including detup for schur) 59 | double timeLinearSolver; ///< time for solving, excluding Schur setup 60 | int iterationsLinearSolver; ///< iterations of PCG, (0 if not used, i.e., Cholesky) 61 | double timeUpdate; ///< time to apply the update 62 | double timeIteration; ///< total time; 63 | 64 | double timeMarginals; ///< computing the inverse elements (solve blocks) and thus the marginal covariances 65 | 66 | // information about the Hessian matrix 67 | size_t hessianDimension; ///< rows / cols of the Hessian 68 | size_t hessianPoseDimension; ///< dimension of the pose matrix in Schur 69 | size_t hessianLandmarkDimension; ///< dimension of the landmark matrix in Schur 70 | size_t choleskyNNZ; ///< number of non-zeros in the cholesky factor 71 | 72 | static G2OBatchStatistics* globalStats() {return _globalStats;} 73 | static void setGlobalStats(G2OBatchStatistics* b); 74 | protected: 75 | static G2OBatchStatistics* _globalStats; 76 | }; 77 | 78 | std::ostream& operator<<(std::ostream&, const G2OBatchStatistics&); 79 | 80 | typedef std::vector BatchStatisticsContainer; 81 | } 82 | 83 | #endif 84 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/creators.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_CREATORS_H 28 | #define G2O_CREATORS_H 29 | 30 | #include "hyper_graph.h" 31 | 32 | #include 33 | #include 34 | 35 | namespace g2o 36 | { 37 | 38 | /** 39 | * \brief Abstract interface for allocating HyperGraphElement 40 | */ 41 | class AbstractHyperGraphElementCreator 42 | { 43 | public: 44 | /** 45 | * create a hyper graph element. Has to implemented in derived class. 46 | */ 47 | virtual HyperGraph::HyperGraphElement* construct() = 0; 48 | /** 49 | * name of the class to be created. Has to implemented in derived class. 50 | */ 51 | virtual const std::string& name() const = 0; 52 | 53 | virtual ~AbstractHyperGraphElementCreator() { } 54 | }; 55 | 56 | /** 57 | * \brief templatized creator class which creates graph elements 58 | */ 59 | template 60 | class HyperGraphElementCreator : public AbstractHyperGraphElementCreator 61 | { 62 | public: 63 | HyperGraphElementCreator() : _name(typeid(T).name()) {} 64 | #if defined (WINDOWS) && defined(__GNUC__) // force stack alignment on Windows with GCC 65 | __attribute__((force_align_arg_pointer)) 66 | #endif 67 | HyperGraph::HyperGraphElement* construct() { return new T;} 68 | virtual const std::string& name() const { return _name;} 69 | protected: 70 | std::string _name; 71 | }; 72 | 73 | } // end namespace 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/jacobian_workspace.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "jacobian_workspace.h" 28 | 29 | #include 30 | 31 | #include "optimizable_graph.h" 32 | 33 | using namespace std; 34 | 35 | namespace g2o { 36 | 37 | JacobianWorkspace::JacobianWorkspace() : 38 | _maxNumVertices(-1), _maxDimension(-1) 39 | { 40 | } 41 | 42 | JacobianWorkspace::~JacobianWorkspace() 43 | { 44 | } 45 | 46 | bool JacobianWorkspace::allocate() 47 | { 48 | //cerr << __PRETTY_FUNCTION__ << " " << PVAR(this) << " " << PVAR(_maxNumVertices) << " " << PVAR(_maxDimension) << endl; 49 | if (_maxNumVertices <=0 || _maxDimension <= 0) 50 | return false; 51 | _workspace.resize(_maxNumVertices); 52 | for (WorkspaceVector::iterator it = _workspace.begin(); it != _workspace.end(); ++it) { 53 | it->resize(_maxDimension); 54 | it->setZero(); 55 | } 56 | return true; 57 | } 58 | 59 | void JacobianWorkspace::updateSize(const HyperGraph::Edge* e_) 60 | { 61 | const OptimizableGraph::Edge* e = static_cast(e_); 62 | int errorDimension = e->dimension(); 63 | int numVertices = e->vertices().size(); 64 | int maxDimensionForEdge = -1; 65 | for (int i = 0; i < numVertices; ++i) { 66 | const OptimizableGraph::Vertex* v = static_cast(e->vertex(i)); 67 | assert(v && "Edge has no vertex assigned"); 68 | maxDimensionForEdge = max(v->dimension() * errorDimension, maxDimensionForEdge); 69 | } 70 | _maxNumVertices = max(numVertices, _maxNumVertices); 71 | _maxDimension = max(maxDimensionForEdge, _maxDimension); 72 | //cerr << __PRETTY_FUNCTION__ << " " << PVAR(this) << " " << PVAR(_maxNumVertices) << " " << PVAR(_maxDimension) << endl; 73 | } 74 | 75 | void JacobianWorkspace::updateSize(const OptimizableGraph& graph) 76 | { 77 | for (OptimizableGraph::EdgeSet::const_iterator it = graph.edges().begin(); it != graph.edges().end(); ++it) { 78 | const OptimizableGraph::Edge* e = static_cast(*it); 79 | updateSize(e); 80 | } 81 | } 82 | 83 | void JacobianWorkspace::updateSize(int numVertices, int dimension) 84 | { 85 | _maxNumVertices = max(numVertices, _maxNumVertices); 86 | _maxDimension = max(dimension, _maxDimension); 87 | } 88 | 89 | } // end namespace 90 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/jacobian_workspace.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef JACOBIAN_WORKSPACE_H 28 | #define JACOBIAN_WORKSPACE_H 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | #include "hyper_graph.h" 37 | 38 | namespace g2o { 39 | 40 | struct OptimizableGraph; 41 | 42 | /** 43 | * \brief provide memory workspace for computing the Jacobians 44 | * 45 | * The workspace is used by an OptimizableGraph to provide temporary memory 46 | * for computing the Jacobian of the error functions. 47 | * Before calling linearizeOplus on an edge, the workspace needs to be allocated 48 | * by calling allocate(). 49 | */ 50 | class JacobianWorkspace 51 | { 52 | public: 53 | typedef std::vector > WorkspaceVector; 54 | 55 | public: 56 | JacobianWorkspace(); 57 | ~JacobianWorkspace(); 58 | 59 | /** 60 | * allocate the workspace 61 | */ 62 | bool allocate(); 63 | 64 | /** 65 | * update the maximum required workspace needed by taking into account this edge 66 | */ 67 | void updateSize(const HyperGraph::Edge* e); 68 | 69 | /** 70 | * update the required workspace by looking at a full graph 71 | */ 72 | void updateSize(const OptimizableGraph& graph); 73 | 74 | /** 75 | * manually update with the given parameters 76 | */ 77 | void updateSize(int numVertices, int dimension); 78 | 79 | /** 80 | * return the workspace for a vertex in an edge 81 | */ 82 | double* workspaceForVertex(int vertexIndex) 83 | { 84 | assert(vertexIndex >= 0 && (size_t)vertexIndex < _workspace.size() && "Index out of bounds"); 85 | return _workspace[vertexIndex].data(); 86 | } 87 | 88 | protected: 89 | WorkspaceVector _workspace; ///< the memory pre-allocated for computing the Jacobians 90 | int _maxNumVertices; ///< the maximum number of vertices connected by a hyper-edge 91 | int _maxDimension; ///< the maximum dimension (number of elements) for a Jacobian 92 | }; 93 | 94 | } // end namespace 95 | 96 | #endif 97 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/matrix_operations.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_CORE_MATRIX_OPERATIONS_H 28 | #define G2O_CORE_MATRIX_OPERATIONS_H 29 | 30 | #include 31 | 32 | namespace g2o { 33 | namespace internal { 34 | 35 | template 36 | inline void axpy(const MatrixType& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 37 | { 38 | y.segment(yoff) += A * x.segment(xoff); 39 | } 40 | 41 | template 42 | inline void axpy(const Eigen::Matrix& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 43 | { 44 | y.segment(yoff, A.rows()) += A * x.segment::ColsAtCompileTime>(xoff); 45 | } 46 | 47 | template<> 48 | inline void axpy(const Eigen::MatrixXd& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 49 | { 50 | y.segment(yoff, A.rows()) += A * x.segment(xoff, A.cols()); 51 | } 52 | 53 | template 54 | inline void atxpy(const MatrixType& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 55 | { 56 | y.segment(yoff) += A.transpose() * x.segment(xoff); 57 | } 58 | 59 | template 60 | inline void atxpy(const Eigen::Matrix& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 61 | { 62 | y.segment::ColsAtCompileTime>(yoff) += A.transpose() * x.segment(xoff, A.rows()); 63 | } 64 | 65 | template<> 66 | inline void atxpy(const Eigen::MatrixXd& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 67 | { 68 | y.segment(yoff, A.cols()) += A.transpose() * x.segment(xoff, A.rows()); 69 | } 70 | 71 | } // end namespace internal 72 | } // end namespace g2o 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/matrix_structure.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_MATRIX_STRUCTURE_H 28 | #define G2O_MATRIX_STRUCTURE_H 29 | 30 | 31 | namespace g2o { 32 | 33 | /** 34 | * \brief representing the structure of a matrix in column compressed structure (only the upper triangular part of the matrix) 35 | */ 36 | class MatrixStructure 37 | { 38 | public: 39 | MatrixStructure(); 40 | ~MatrixStructure(); 41 | /** 42 | * allocate space for the Matrix Structure. You may call this on an already allocated struct, it will 43 | * then reallocate the memory + additional space (double the required space). 44 | */ 45 | void alloc(int n_, int nz); 46 | 47 | void free(); 48 | 49 | /** 50 | * Write the matrix pattern to a file. File is also loadable by octave, e.g., then use spy(matrix) 51 | */ 52 | bool write(const char* filename) const; 53 | 54 | int n; ///< A is m-by-n. n must be >= 0. 55 | int m; ///< A is m-by-n. m must be >= 0. 56 | int* Ap; ///< column pointers for A, of size n+1 57 | int* Aii; ///< row indices of A, of size nz = Ap [n] 58 | 59 | //! max number of non-zeros blocks 60 | int nzMax() const { return maxNz;} 61 | 62 | protected: 63 | int maxN; ///< size of the allocated memory 64 | int maxNz; ///< size of the allocated memory 65 | }; 66 | 67 | } // end namespace 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/openmp_mutex.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPENMP_MUTEX 28 | #define G2O_OPENMP_MUTEX 29 | 30 | #include "../../config.h" 31 | 32 | #ifdef G2O_OPENMP 33 | #include 34 | #else 35 | #include 36 | #endif 37 | 38 | namespace g2o { 39 | 40 | #ifdef G2O_OPENMP 41 | 42 | /** 43 | * \brief Mutex realized via OpenMP 44 | */ 45 | class OpenMPMutex 46 | { 47 | public: 48 | OpenMPMutex() { omp_init_lock(&_lock); } 49 | ~OpenMPMutex() { omp_destroy_lock(&_lock); } 50 | void lock() { omp_set_lock(&_lock); } 51 | void unlock() { omp_unset_lock(&_lock); } 52 | protected: 53 | omp_lock_t _lock; 54 | }; 55 | 56 | #else 57 | 58 | /* 59 | * dummy which does nothing in case we don't have OpenMP support. 60 | * In debug mode, the mutex allows to verify the correct lock and unlock behavior 61 | */ 62 | class OpenMPMutex 63 | { 64 | public: 65 | #ifdef NDEBUG 66 | OpenMPMutex() {} 67 | #else 68 | OpenMPMutex() : _cnt(0) {} 69 | #endif 70 | ~OpenMPMutex() { assert(_cnt == 0 && "Freeing locked mutex");} 71 | void lock() { assert(++_cnt == 1 && "Locking already locked mutex");} 72 | void unlock() { assert(--_cnt == 0 && "Trying to unlock a mutex which is not locked");} 73 | protected: 74 | #ifndef NDEBUG 75 | char _cnt; 76 | #endif 77 | }; 78 | 79 | #endif 80 | 81 | /** 82 | * \brief lock a mutex within a scope 83 | */ 84 | class ScopedOpenMPMutex 85 | { 86 | public: 87 | explicit ScopedOpenMPMutex(OpenMPMutex* mutex) : _mutex(mutex) { _mutex->lock(); } 88 | ~ScopedOpenMPMutex() { _mutex->unlock(); } 89 | private: 90 | OpenMPMutex* const _mutex; 91 | ScopedOpenMPMutex(const ScopedOpenMPMutex&); 92 | void operator=(const ScopedOpenMPMutex&); 93 | }; 94 | 95 | } 96 | 97 | #endif 98 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/optimization_algorithm.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "optimization_algorithm.h" 28 | 29 | using namespace std; 30 | 31 | namespace g2o { 32 | 33 | OptimizationAlgorithm::OptimizationAlgorithm() : 34 | _optimizer(0) 35 | { 36 | } 37 | 38 | OptimizationAlgorithm::~OptimizationAlgorithm() 39 | { 40 | } 41 | 42 | void OptimizationAlgorithm::printProperties(std::ostream& os) const 43 | { 44 | os << "------------- Algorithm Properties -------------" << endl; 45 | for (PropertyMap::const_iterator it = _properties.begin(); it != _properties.end(); ++it) { 46 | BaseProperty* p = it->second; 47 | os << it->first << "\t" << p->toString() << endl; 48 | } 49 | os << "------------------------------------------------" << endl; 50 | } 51 | 52 | bool OptimizationAlgorithm::updatePropertiesFromString(const std::string& propString) 53 | { 54 | return _properties.updateMapFromString(propString); 55 | } 56 | 57 | void OptimizationAlgorithm::setOptimizer(SparseOptimizer* optimizer) 58 | { 59 | _optimizer = optimizer; 60 | } 61 | 62 | } // end namespace 63 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H 29 | 30 | #include "optimization_algorithm_with_hessian.h" 31 | 32 | namespace g2o { 33 | 34 | class BlockSolverBase; 35 | 36 | /** 37 | * \brief Implementation of Powell's Dogleg Algorithm 38 | */ 39 | class OptimizationAlgorithmDogleg : public OptimizationAlgorithmWithHessian 40 | { 41 | public: 42 | /** \brief type of the step to take */ 43 | enum { 44 | STEP_UNDEFINED, 45 | STEP_SD, STEP_GN, STEP_DL 46 | }; 47 | 48 | public: 49 | /** 50 | * construct the Dogleg algorithm, which will use the given Solver for solving the 51 | * linearized system. 52 | */ 53 | explicit OptimizationAlgorithmDogleg(BlockSolverBase* solver); 54 | virtual ~OptimizationAlgorithmDogleg(); 55 | 56 | virtual SolverResult solve(int iteration, bool online = false); 57 | 58 | virtual void printVerbose(std::ostream& os) const; 59 | 60 | //! return the type of the last step taken by the algorithm 61 | int lastStep() const { return _lastStep;} 62 | //! return the diameter of the trust region 63 | double trustRegion() const { return _delta;} 64 | 65 | //! convert the type into an integer 66 | static const char* stepType2Str(int stepType); 67 | 68 | protected: 69 | // parameters 70 | Property* _maxTrialsAfterFailure; 71 | Property* _userDeltaInit; 72 | // damping to enforce positive definite matrix 73 | Property* _initialLambda; 74 | Property* _lamdbaFactor; 75 | 76 | Eigen::VectorXd _hsd; ///< steepest decent step 77 | Eigen::VectorXd _hdl; ///< final dogleg step 78 | Eigen::VectorXd _auxVector; ///< auxilary vector used to perform multiplications or other stuff 79 | 80 | double _currentLambda; ///< the damping factor to force positive definite matrix 81 | double _delta; ///< trust region 82 | int _lastStep; ///< type of the step taken by the algorithm 83 | bool _wasPDInAllIterations; ///< the matrix we solve was positive definite in all iterations -> if not apply damping 84 | int _lastNumTries; 85 | }; 86 | 87 | } // end namespace 88 | 89 | #endif 90 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.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_gauss_newton.h" 28 | 29 | #include 30 | 31 | #include "../stuff/timeutil.h" 32 | #include "../stuff/macros.h" 33 | 34 | #include "solver.h" 35 | #include "batch_stats.h" 36 | #include "sparse_optimizer.h" 37 | using namespace std; 38 | 39 | namespace g2o { 40 | 41 | OptimizationAlgorithmGaussNewton::OptimizationAlgorithmGaussNewton(Solver* solver) : 42 | OptimizationAlgorithmWithHessian(solver) 43 | { 44 | } 45 | 46 | OptimizationAlgorithmGaussNewton::~OptimizationAlgorithmGaussNewton() 47 | { 48 | } 49 | 50 | OptimizationAlgorithm::SolverResult OptimizationAlgorithmGaussNewton::solve(int iteration, bool online) 51 | { 52 | assert(_optimizer && "_optimizer not set"); 53 | assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph"); 54 | bool ok = true; 55 | 56 | //here so that correct component for max-mixtures can be computed before the build structure 57 | double t=get_monotonic_time(); 58 | _optimizer->computeActiveErrors(); 59 | G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats(); 60 | if (globalStats) { 61 | globalStats->timeResiduals = get_monotonic_time()-t; 62 | } 63 | 64 | if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure 65 | ok = _solver->buildStructure(); 66 | if (! ok) { 67 | cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl; 68 | return OptimizationAlgorithm::Fail; 69 | } 70 | } 71 | 72 | t=get_monotonic_time(); 73 | _solver->buildSystem(); 74 | if (globalStats) { 75 | globalStats->timeQuadraticForm = get_monotonic_time()-t; 76 | t=get_monotonic_time(); 77 | } 78 | 79 | ok = _solver->solve(); 80 | if (globalStats) { 81 | globalStats->timeLinearSolution = get_monotonic_time()-t; 82 | t=get_monotonic_time(); 83 | } 84 | 85 | _optimizer->update(_solver->x()); 86 | if (globalStats) { 87 | globalStats->timeUpdate = get_monotonic_time()-t; 88 | } 89 | if (ok) 90 | return OK; 91 | else 92 | return Fail; 93 | } 94 | 95 | void OptimizationAlgorithmGaussNewton::printVerbose(std::ostream& os) const 96 | { 97 | os 98 | << "\t schur= " << _solver->schur(); 99 | } 100 | 101 | } // end namespace 102 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H 29 | 30 | #include "optimization_algorithm_with_hessian.h" 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief Implementation of the Gauss Newton Algorithm 36 | */ 37 | class OptimizationAlgorithmGaussNewton : public OptimizationAlgorithmWithHessian 38 | { 39 | public: 40 | /** 41 | * construct the Gauss Newton algorithm, which use the given Solver for solving the 42 | * linearized system. 43 | */ 44 | explicit OptimizationAlgorithmGaussNewton(Solver* solver); 45 | virtual ~OptimizationAlgorithmGaussNewton(); 46 | 47 | virtual SolverResult solve(int iteration, bool online = false); 48 | 49 | virtual void printVerbose(std::ostream& os) const; 50 | }; 51 | 52 | } // end namespace 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.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_SOLVER_LEVENBERG_H 28 | #define G2O_SOLVER_LEVENBERG_H 29 | 30 | #include "optimization_algorithm_with_hessian.h" 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief Implementation of the Levenberg Algorithm 36 | */ 37 | class OptimizationAlgorithmLevenberg : public OptimizationAlgorithmWithHessian 38 | { 39 | public: 40 | /** 41 | * construct the Levenberg algorithm, which will use the given Solver for solving the 42 | * linearized system. 43 | */ 44 | explicit OptimizationAlgorithmLevenberg(Solver* solver); 45 | virtual ~OptimizationAlgorithmLevenberg(); 46 | 47 | virtual SolverResult solve(int iteration, bool online = false); 48 | 49 | virtual void printVerbose(std::ostream& os) const; 50 | 51 | //! return the currently used damping factor 52 | double currentLambda() const { return _currentLambda;} 53 | 54 | //! the number of internal iteration if an update step increases chi^2 within Levenberg-Marquardt 55 | void setMaxTrialsAfterFailure(int max_trials); 56 | 57 | //! get the number of inner iterations for Levenberg-Marquardt 58 | int maxTrialsAfterFailure() const { return _maxTrialsAfterFailure->value();} 59 | 60 | //! return the lambda set by the user, if < 0 the SparseOptimizer will compute the initial lambda 61 | double userLambdaInit() {return _userLambdaInit->value();} 62 | //! specify the initial lambda used for the first iteraion, if not given the SparseOptimizer tries to compute a suitable value 63 | void setUserLambdaInit(double lambda); 64 | 65 | //! return the number of levenberg iterations performed in the last round 66 | int levenbergIteration() { return _levenbergIterations;} 67 | 68 | protected: 69 | // Levenberg parameters 70 | Property* _maxTrialsAfterFailure; 71 | Property* _userLambdaInit; 72 | double _currentLambda; 73 | double _tau; 74 | double _goodStepLowerScale; ///< lower bound for lambda decrease if a good LM step 75 | double _goodStepUpperScale; ///< upper bound for lambda decrease if a good LM step 76 | double _ni; 77 | int _levenbergIterations; ///< the numer of levenberg iterations performed to accept the last step 78 | //RAUL 79 | int _nBad; 80 | 81 | /** 82 | * helper for Levenberg, this function computes the initial damping factor, if the user did not 83 | * specify an own value, see setUserLambdaInit() 84 | */ 85 | double computeLambdaInit() const; 86 | double computeScale() const; 87 | 88 | }; 89 | 90 | } // end namespace 91 | 92 | #endif 93 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/optimization_algorithm_property.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H 29 | 30 | #include 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief describe the properties of a solver 36 | */ 37 | struct OptimizationAlgorithmProperty 38 | { 39 | std::string name; ///< name of the solver, e.g., var 40 | std::string desc; ///< short description of the solver 41 | std::string type; ///< type of solver, e.g., "CSparse Cholesky", "PCG" 42 | bool requiresMarginalize; ///< whether the solver requires marginalization of landmarks 43 | int poseDim; ///< dimension of the pose vertices (-1 if variable) 44 | int landmarkDim; ///< dimension of the landmar vertices (-1 if variable) 45 | OptimizationAlgorithmProperty() : 46 | name(), desc(), type(), requiresMarginalize(false), poseDim(-1), landmarkDim(-1) 47 | { 48 | } 49 | OptimizationAlgorithmProperty(const std::string& name_, const std::string& desc_, const std::string& type_, bool requiresMarginalize_, int poseDim_, int landmarkDim_) : 50 | name(name_), desc(desc_), type(type_), requiresMarginalize(requiresMarginalize_), poseDim(poseDim_), landmarkDim(landmarkDim_) 51 | { 52 | } 53 | }; 54 | 55 | } // end namespace 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.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_with_hessian.h" 28 | 29 | #include "solver.h" 30 | #include "optimizable_graph.h" 31 | #include "sparse_optimizer.h" 32 | 33 | #include 34 | using namespace std; 35 | 36 | namespace g2o { 37 | 38 | OptimizationAlgorithmWithHessian::OptimizationAlgorithmWithHessian(Solver* solver) : 39 | OptimizationAlgorithm(), 40 | _solver(solver) 41 | { 42 | _writeDebug = _properties.makeProperty >("writeDebug", true); 43 | } 44 | 45 | OptimizationAlgorithmWithHessian::~OptimizationAlgorithmWithHessian() 46 | { 47 | delete _solver; 48 | } 49 | 50 | bool OptimizationAlgorithmWithHessian::init(bool online) 51 | { 52 | assert(_optimizer && "_optimizer not set"); 53 | assert(_solver && "Solver not set"); 54 | _solver->setWriteDebug(_writeDebug->value()); 55 | bool useSchur=false; 56 | for (OptimizableGraph::VertexContainer::const_iterator it=_optimizer->activeVertices().begin(); it!=_optimizer->activeVertices().end(); ++it) { 57 | OptimizableGraph::Vertex* v= *it; 58 | if (v->marginalized()){ 59 | useSchur=true; 60 | break; 61 | } 62 | } 63 | if (useSchur){ 64 | if (_solver->supportsSchur()) 65 | _solver->setSchur(true); 66 | } else { 67 | if (_solver->supportsSchur()) 68 | _solver->setSchur(false); 69 | } 70 | 71 | bool initState = _solver->init(_optimizer, online); 72 | return initState; 73 | } 74 | 75 | bool OptimizationAlgorithmWithHessian::computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices) 76 | { 77 | return _solver ? _solver->computeMarginals(spinv, blockIndices) : false; 78 | } 79 | 80 | bool OptimizationAlgorithmWithHessian::buildLinearStructure() 81 | { 82 | return _solver ? _solver->buildStructure() : false; 83 | } 84 | 85 | void OptimizationAlgorithmWithHessian::updateLinearSystem() 86 | { 87 | if (_solver) 88 | _solver->buildSystem(); 89 | } 90 | 91 | bool OptimizationAlgorithmWithHessian::updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges) 92 | { 93 | return _solver ? _solver->updateStructure(vset, edges) : false; 94 | } 95 | 96 | void OptimizationAlgorithmWithHessian::setWriteDebug(bool writeDebug) 97 | { 98 | _writeDebug->setValue(writeDebug); 99 | } 100 | 101 | } // end namespace 102 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H 29 | 30 | #include "optimization_algorithm.h" 31 | 32 | namespace g2o { 33 | 34 | class Solver; 35 | 36 | /** 37 | * \brief Base for solvers operating on the approximated Hessian, e.g., Gauss-Newton, Levenberg 38 | */ 39 | class OptimizationAlgorithmWithHessian : public OptimizationAlgorithm 40 | { 41 | public: 42 | explicit OptimizationAlgorithmWithHessian(Solver* solver); 43 | virtual ~OptimizationAlgorithmWithHessian(); 44 | 45 | virtual bool init(bool online = false); 46 | 47 | virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices); 48 | 49 | virtual bool buildLinearStructure(); 50 | 51 | virtual void updateLinearSystem(); 52 | 53 | virtual bool updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges); 54 | 55 | //! return the underlying solver used to solve the linear system 56 | Solver* solver() { return _solver;} 57 | 58 | /** 59 | * write debug output of the Hessian if system is not positive definite 60 | */ 61 | virtual void setWriteDebug(bool writeDebug); 62 | virtual bool writeDebug() const { return _writeDebug->value();} 63 | 64 | protected: 65 | Solver* _solver; 66 | Property* _writeDebug; 67 | 68 | }; 69 | 70 | }// end namespace 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/parameter.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "parameter.h" 28 | 29 | namespace g2o { 30 | 31 | Parameter::Parameter() : _id(-1) 32 | { 33 | } 34 | 35 | void Parameter::setId(int id_) 36 | { 37 | _id = id_; 38 | } 39 | 40 | } // end namespace 41 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/parameter.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_GRAPH_PARAMETER_HH_ 28 | #define G2O_GRAPH_PARAMETER_HH_ 29 | 30 | #include 31 | 32 | #include "hyper_graph.h" 33 | 34 | namespace g2o { 35 | 36 | class Parameter : public HyperGraph::HyperGraphElement 37 | { 38 | public: 39 | Parameter(); 40 | virtual ~Parameter() {}; 41 | //! read the data from a stream 42 | virtual bool read(std::istream& is) = 0; 43 | //! write the data to a stream 44 | virtual bool write(std::ostream& os) const = 0; 45 | int id() const {return _id;} 46 | void setId(int id_); 47 | virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_PARAMETER;} 48 | protected: 49 | int _id; 50 | }; 51 | 52 | typedef std::vector ParameterVector; 53 | 54 | } // end namespace 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/parameter_container.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_GRAPH_PARAMETER_CONTAINER_HH_ 28 | #define G2O_GRAPH_PARAMETER_CONTAINER_HH_ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace g2o { 35 | 36 | class Parameter; 37 | 38 | /** 39 | * \brief map id to parameters 40 | */ 41 | class ParameterContainer : protected std::map 42 | { 43 | public: 44 | typedef std::map BaseClass; 45 | 46 | /** 47 | * create a container for the parameters. 48 | * @param isMainStorage_ pointers to the parameters are owned by this container, i.e., freed in its constructor 49 | */ 50 | ParameterContainer(bool isMainStorage_=true); 51 | virtual ~ParameterContainer(); 52 | //! add parameter to the container 53 | bool addParameter(Parameter* p); 54 | //! return a parameter based on its ID 55 | Parameter* getParameter(int id); 56 | //! remove a parameter from the container, i.e., the user now owns the pointer 57 | Parameter* detachParameter(int id); 58 | //! read parameters from a stream 59 | virtual bool read(std::istream& is, const std::map* renamedMap =0); 60 | //! write the data to a stream 61 | virtual bool write(std::ostream& os) const; 62 | bool isMainStorage() const {return _isMainStorage;} 63 | void clear(); 64 | 65 | // stuff of the base class that should re-appear 66 | using BaseClass::size; 67 | 68 | protected: 69 | bool _isMainStorage; 70 | }; 71 | 72 | } // end namespace 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/robust_kernel.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "robust_kernel.h" 28 | 29 | namespace g2o { 30 | 31 | RobustKernel::RobustKernel() : 32 | _delta(1.) 33 | { 34 | } 35 | 36 | RobustKernel::RobustKernel(double delta) : 37 | _delta(delta) 38 | { 39 | } 40 | 41 | void RobustKernel::setDelta(double delta) 42 | { 43 | _delta = delta; 44 | } 45 | 46 | } // end namespace g2o 47 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/robust_kernel.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_ROBUST_KERNEL_H 28 | #define G2O_ROBUST_KERNEL_H 29 | 30 | #ifdef _MSC_VER 31 | #include 32 | #else 33 | #include 34 | #endif 35 | #include 36 | 37 | 38 | namespace g2o { 39 | 40 | /** 41 | * \brief base for all robust cost functions 42 | * 43 | * Note in all the implementations for the other cost functions the e in the 44 | * funtions corresponds to the sqaured errors, i.e., the robustification 45 | * functions gets passed the squared error. 46 | * 47 | * e.g. the robustified least squares function is 48 | * 49 | * chi^2 = sum_{e} rho( e^T Omega e ) 50 | */ 51 | class RobustKernel 52 | { 53 | public: 54 | RobustKernel(); 55 | explicit RobustKernel(double delta); 56 | virtual ~RobustKernel() {} 57 | /** 58 | * compute the scaling factor for a error: 59 | * The error is e^T Omega e 60 | * The output rho is 61 | * rho[0]: The actual scaled error value 62 | * rho[1]: First derivative of the scaling function 63 | * rho[2]: Second derivative of the scaling function 64 | */ 65 | virtual void robustify(double squaredError, Eigen::Vector3d& rho) const = 0; 66 | 67 | /** 68 | * set the window size of the error. A squared error above delta^2 is considered 69 | * as outlier in the data. 70 | */ 71 | virtual void setDelta(double delta); 72 | double delta() const { return _delta;} 73 | 74 | protected: 75 | double _delta; 76 | }; 77 | typedef std::tr1::shared_ptr RobustKernelPtr; 78 | 79 | } // end namespace g2o 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/robust_kernel_factory.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "robust_kernel_factory.h" 28 | #include "robust_kernel.h" 29 | 30 | #include 31 | 32 | using namespace std; 33 | 34 | namespace g2o { 35 | 36 | RobustKernelFactory* RobustKernelFactory::factoryInstance = 0; 37 | 38 | RobustKernelFactory::RobustKernelFactory() 39 | { 40 | } 41 | 42 | RobustKernelFactory::~RobustKernelFactory() 43 | { 44 | for (CreatorMap::iterator it = _creator.begin(); it != _creator.end(); ++it) { 45 | delete it->second; 46 | } 47 | _creator.clear(); 48 | } 49 | 50 | RobustKernelFactory* RobustKernelFactory::instance() 51 | { 52 | if (factoryInstance == 0) { 53 | factoryInstance = new RobustKernelFactory; 54 | } 55 | 56 | return factoryInstance; 57 | } 58 | 59 | void RobustKernelFactory::registerRobustKernel(const std::string& tag, AbstractRobustKernelCreator* c) 60 | { 61 | CreatorMap::const_iterator foundIt = _creator.find(tag); 62 | if (foundIt != _creator.end()) { 63 | cerr << "RobustKernelFactory WARNING: Overwriting robust kernel tag " << tag << endl; 64 | assert(0); 65 | } 66 | 67 | _creator[tag] = c; 68 | } 69 | 70 | void RobustKernelFactory::unregisterType(const std::string& tag) 71 | { 72 | CreatorMap::iterator tagPosition = _creator.find(tag); 73 | if (tagPosition != _creator.end()) { 74 | AbstractRobustKernelCreator* c = tagPosition->second; 75 | delete c; 76 | _creator.erase(tagPosition); 77 | } 78 | } 79 | 80 | RobustKernel* RobustKernelFactory::construct(const std::string& tag) const 81 | { 82 | CreatorMap::const_iterator foundIt = _creator.find(tag); 83 | if (foundIt != _creator.end()) { 84 | return foundIt->second->construct(); 85 | } 86 | return 0; 87 | } 88 | 89 | AbstractRobustKernelCreator* RobustKernelFactory::creator(const std::string& tag) const 90 | { 91 | CreatorMap::const_iterator foundIt = _creator.find(tag); 92 | if (foundIt != _creator.end()) { 93 | return foundIt->second; 94 | } 95 | return 0; 96 | } 97 | 98 | void RobustKernelFactory::fillKnownKernels(std::vector& types) const 99 | { 100 | types.clear(); 101 | for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it) 102 | types.push_back(it->first); 103 | } 104 | 105 | void RobustKernelFactory::destroy() 106 | { 107 | delete factoryInstance; 108 | factoryInstance = 0; 109 | } 110 | 111 | } // end namespace 112 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/solver.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "solver.h" 28 | 29 | #include 30 | #include 31 | 32 | namespace g2o { 33 | 34 | Solver::Solver() : 35 | _optimizer(0), _x(0), _b(0), _xSize(0), _maxXSize(0), 36 | _isLevenberg(false), _additionalVectorSpace(0) 37 | { 38 | } 39 | 40 | Solver::~Solver() 41 | { 42 | delete[] _x; 43 | delete[] _b; 44 | } 45 | 46 | void Solver::resizeVector(size_t sx) 47 | { 48 | size_t oldSize = _xSize; 49 | _xSize = sx; 50 | sx += _additionalVectorSpace; // allocate some additional space if requested 51 | if (_maxXSize < sx) { 52 | _maxXSize = 2*sx; 53 | delete[] _x; 54 | _x = new double[_maxXSize]; 55 | #ifndef NDEBUG 56 | memset(_x, 0, _maxXSize * sizeof(double)); 57 | #endif 58 | if (_b) { // backup the former b, might still be needed for online processing 59 | memcpy(_x, _b, oldSize * sizeof(double)); 60 | delete[] _b; 61 | _b = new double[_maxXSize]; 62 | std::swap(_b, _x); 63 | } else { 64 | _b = new double[_maxXSize]; 65 | #ifndef NDEBUG 66 | memset(_b, 0, _maxXSize * sizeof(double)); 67 | #endif 68 | } 69 | } 70 | } 71 | 72 | void Solver::setOptimizer(SparseOptimizer* optimizer) 73 | { 74 | _optimizer = optimizer; 75 | } 76 | 77 | void Solver::setLevenberg(bool levenberg) 78 | { 79 | _isLevenberg = levenberg; 80 | } 81 | 82 | void Solver::setAdditionalVectorSpace(size_t s) 83 | { 84 | _additionalVectorSpace = s; 85 | } 86 | 87 | } // end namespace 88 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/sparse_block_matrix_test.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "sparse_block_matrix.h" 28 | #include 29 | 30 | using namespace std; 31 | using namespace g2o; 32 | using namespace Eigen; 33 | 34 | typedef SparseBlockMatrix< MatrixXd > 35 | SparseBlockMatrixX; 36 | 37 | std::ostream& operator << (std::ostream& os, const SparseBlockMatrixX::SparseMatrixBlock& m) { 38 | for (int i=0; iblock(0,0, true); 55 | cerr << b->rows() << " " << b->cols() << endl; 56 | for (int i=0; irows(); ++i) 57 | for (int j=0; jcols(); ++j){ 58 | (*b)(i,j)=i*b->cols()+j; 59 | } 60 | 61 | 62 | cerr << "block access 2" << endl; 63 | b=M->block(0,2, true); 64 | cerr << b->rows() << " " << b->cols() << endl; 65 | for (int i=0; irows(); ++i) 66 | for (int j=0; jcols(); ++j){ 67 | (*b)(i,j)=i*b->cols()+j; 68 | } 69 | 70 | b=M->block(3,2, true); 71 | cerr << b->rows() << " " << b->cols() << endl; 72 | for (int i=0; irows(); ++i) 73 | for (int j=0; jcols(); ++j){ 74 | (*b)(i,j)=i*b->cols()+j; 75 | } 76 | 77 | cerr << *M << endl; 78 | 79 | cerr << "SUM" << endl; 80 | 81 | SparseBlockMatrixX* Ms=0; 82 | M->add(Ms); 83 | M->add(Ms); 84 | cerr << *Ms; 85 | 86 | SparseBlockMatrixX* Mt=0; 87 | M->transpose(Mt); 88 | cerr << *Mt << endl; 89 | 90 | SparseBlockMatrixX* Mp=0; 91 | M->multiply(Mp, Mt); 92 | cerr << *Mp << endl; 93 | 94 | int iperm[]={3,2,1,0}; 95 | SparseBlockMatrixX* PMp=0; 96 | 97 | Mp->symmPermutation(PMp,iperm, false); 98 | cerr << *PMp << endl; 99 | 100 | PMp->clear(true); 101 | Mp->block(3,0)->fill(0.); 102 | Mp->symmPermutation(PMp,iperm, true); 103 | cerr << *PMp << endl; 104 | 105 | 106 | 107 | } 108 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/stuff/color_macros.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_COLOR_MACROS_H 28 | #define G2O_COLOR_MACROS_H 29 | 30 | // font attributes 31 | #define FT_BOLD "\033[1m" 32 | #define FT_UNDERLINE "\033[4m" 33 | 34 | //background color 35 | #define BG_BLACK "\033[40m" 36 | #define BG_RED "\033[41m" 37 | #define BG_GREEN "\033[42m" 38 | #define BG_YELLOW "\033[43m" 39 | #define BG_LIGHTBLUE "\033[44m" 40 | #define BG_MAGENTA "\033[45m" 41 | #define BG_BLUE "\033[46m" 42 | #define BG_WHITE "\033[47m" 43 | 44 | // font color 45 | #define CL_BLACK(s) "\033[30m" << s << "\033[0m" 46 | #define CL_RED(s) "\033[31m" << s << "\033[0m" 47 | #define CL_GREEN(s) "\033[32m" << s << "\033[0m" 48 | #define CL_YELLOW(s) "\033[33m" << s << "\033[0m" 49 | #define CL_LIGHTBLUE(s) "\033[34m" << s << "\033[0m" 50 | #define CL_MAGENTA(s) "\033[35m" << s << "\033[0m" 51 | #define CL_BLUE(s) "\033[36m" << s << "\033[0m" 52 | #define CL_WHITE(s) "\033[37m" << s << "\033[0m" 53 | 54 | #define FG_BLACK "\033[30m" 55 | #define FG_RED "\033[31m" 56 | #define FG_GREEN "\033[32m" 57 | #define FG_YELLOW "\033[33m" 58 | #define FG_LIGHTBLUE "\033[34m" 59 | #define FG_MAGENTA "\033[35m" 60 | #define FG_BLUE "\033[36m" 61 | #define FG_WHITE "\033[37m" 62 | 63 | #define FG_NORM "\033[0m" 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/stuff/os_specific.c: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "os_specific.h" 28 | 29 | #ifdef WINDOWS 30 | 31 | int vasprintf(char** strp, const char* fmt, va_list ap) 32 | { 33 | int n; 34 | int size = 100; 35 | char* p; 36 | char* np; 37 | 38 | if ((p = (char*)malloc(size * sizeof(char))) == NULL) 39 | return -1; 40 | 41 | while (1) { 42 | #ifdef _MSC_VER 43 | n = vsnprintf_s(p, size, size - 1, fmt, ap); 44 | #else 45 | n = vsnprintf(p, size, fmt, ap); 46 | #endif 47 | if (n > -1 && n < size) { 48 | *strp = p; 49 | return n; 50 | } 51 | if (n > -1) 52 | size = n+1; 53 | else 54 | size *= 2; 55 | if ((np = (char*)realloc (p, size * sizeof(char))) == NULL) { 56 | free(p); 57 | return -1; 58 | } else 59 | p = np; 60 | } 61 | } 62 | 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/stuff/os_specific.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OS_SPECIFIC_HH_ 28 | #define G2O_OS_SPECIFIC_HH_ 29 | 30 | #ifdef WINDOWS 31 | #include 32 | #include 33 | #include 34 | #ifndef _WINDOWS 35 | #include 36 | #endif 37 | #define drand48() ((double) rand()/(double)RAND_MAX) 38 | 39 | #ifdef __cplusplus 40 | extern "C" { 41 | #endif 42 | 43 | int vasprintf(char** strp, const char* fmt, va_list ap); 44 | 45 | #ifdef __cplusplus 46 | } 47 | #endif 48 | 49 | #endif 50 | 51 | #ifdef UNIX 52 | #include 53 | // nothing to do on real operating systems 54 | #endif 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/stuff/property.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "property.h" 28 | 29 | #include 30 | #include 31 | 32 | #include "macros.h" 33 | 34 | #include "string_tools.h" 35 | using namespace std; 36 | 37 | namespace g2o { 38 | 39 | BaseProperty::BaseProperty(const std::string name_) :_name(name_){ 40 | } 41 | 42 | BaseProperty::~BaseProperty(){} 43 | 44 | bool PropertyMap::addProperty(BaseProperty* p) { 45 | std::pair result = insert(make_pair(p->name(), p)); 46 | return result.second; 47 | } 48 | 49 | bool PropertyMap::eraseProperty(const std::string& name) { 50 | PropertyMapIterator it=find(name); 51 | if (it==end()) 52 | return false; 53 | delete it->second; 54 | erase(it); 55 | return true; 56 | } 57 | 58 | PropertyMap::~PropertyMap() { 59 | for (PropertyMapIterator it=begin(); it!=end(); it++){ 60 | if (it->second) 61 | delete it->second; 62 | } 63 | } 64 | 65 | bool PropertyMap::updatePropertyFromString(const std::string& name, const std::string& value) 66 | { 67 | PropertyMapIterator it = find(name); 68 | if (it == end()) 69 | return false; 70 | it->second->fromString(value); 71 | return true; 72 | } 73 | 74 | void PropertyMap::writeToCSV(std::ostream& os) const 75 | { 76 | for (PropertyMapConstIterator it=begin(); it!=end(); it++){ 77 | BaseProperty* p =it->second; 78 | os << p->name() << ", "; 79 | } 80 | os << std::endl; 81 | for (PropertyMapConstIterator it=begin(); it!=end(); it++){ 82 | BaseProperty* p =it->second; 83 | os << p->toString() << ", "; 84 | } 85 | os << std::endl; 86 | } 87 | 88 | bool PropertyMap::updateMapFromString(const std::string& values) 89 | { 90 | bool status = true; 91 | vector valuesMap = strSplit(values, ","); 92 | for (size_t i = 0; i < valuesMap.size(); ++i) { 93 | vector m = strSplit(valuesMap[i], "="); 94 | if (m.size() != 2) { 95 | cerr << __PRETTY_FUNCTION__ << ": unable to extract name=value pair from " << valuesMap[i] << endl; 96 | continue; 97 | } 98 | string name = trim(m[0]); 99 | string value = trim(m[1]); 100 | status = status && updatePropertyFromString(name, value); 101 | } 102 | return status; 103 | } 104 | 105 | } // end namespace 106 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/se3_ops.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 H. Strasdat 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_MATH_STUFF 28 | #define G2O_MATH_STUFF 29 | 30 | #include 31 | #include 32 | 33 | namespace g2o { 34 | using namespace Eigen; 35 | 36 | inline Matrix3d skew(const Vector3d&v); 37 | inline Vector3d deltaR(const Matrix3d& R); 38 | inline Vector2d project(const Vector3d&); 39 | inline Vector3d project(const Vector4d&); 40 | inline Vector3d unproject(const Vector2d&); 41 | inline Vector4d unproject(const Vector3d&); 42 | 43 | #include "se3_ops.hpp" 44 | 45 | } 46 | 47 | #endif //MATH_STUFF 48 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/se3_ops.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 H. Strasdat 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | Matrix3d skew(const Vector3d&v) 28 | { 29 | Matrix3d m; 30 | m.fill(0.); 31 | m(0,1) = -v(2); 32 | m(0,2) = v(1); 33 | m(1,2) = -v(0); 34 | m(1,0) = v(2); 35 | m(2,0) = -v(1); 36 | m(2,1) = v(0); 37 | return m; 38 | } 39 | 40 | Vector3d deltaR(const Matrix3d& R) 41 | { 42 | Vector3d v; 43 | v(0)=R(2,1)-R(1,2); 44 | v(1)=R(0,2)-R(2,0); 45 | v(2)=R(1,0)-R(0,1); 46 | return v; 47 | } 48 | 49 | Vector2d project(const Vector3d& v) 50 | { 51 | Vector2d res; 52 | res(0) = v(0)/v(2); 53 | res(1) = v(1)/v(2); 54 | return res; 55 | } 56 | 57 | Vector3d project(const Vector4d& v) 58 | { 59 | Vector3d res; 60 | res(0) = v(0)/v(3); 61 | res(1) = v(1)/v(3); 62 | res(2) = v(2)/v(3); 63 | return res; 64 | } 65 | 66 | Vector3d unproject(const Vector2d& v) 67 | { 68 | Vector3d res; 69 | res(0) = v(0); 70 | res(1) = v(1); 71 | res(2) = 1; 72 | return res; 73 | } 74 | 75 | Vector4d unproject(const Vector3d& v) 76 | { 77 | Vector4d res; 78 | res(0) = v(0); 79 | res(1) = v(1); 80 | res(2) = v(2); 81 | res(3) = 1; 82 | return res; 83 | } 84 | 85 | 86 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/se3mat.cpp: -------------------------------------------------------------------------------- 1 | #include "se3mat.h" 2 | 3 | namespace g2o { 4 | 5 | 6 | void SE3mat::Retract(const Eigen::Vector3d dr, const Eigen::Vector3d &dt) 7 | { 8 | t += R*dt; 9 | R = R*ExpSO3(dr); 10 | } 11 | 12 | Eigen::Matrix3d SE3mat::ExpSO3(const Eigen::Vector3d r) 13 | { 14 | Eigen::Matrix3d W; 15 | W << 0, -r[2], r[1], 16 | r[2], 0, -r[0], 17 | -r[1], r[0], 0; 18 | 19 | const double theta = r.norm(); 20 | 21 | if(theta<1e-6) 22 | return Eigen::Matrix3d::Identity() + W + 0.5l*W*W; 23 | else 24 | return Eigen::Matrix3d::Identity() + W*sin(theta)/theta + W*W*(1-cos(theta))/(theta*theta); 25 | } 26 | 27 | Eigen::Vector3d SE3mat::LogSO3(const Eigen::Matrix3d R) 28 | { 29 | const double tr = R(0,0)+R(1,1)+R(2,2); 30 | const double theta = acos((tr-1.0l)*0.5l); 31 | Eigen::Vector3d w; 32 | w << R(2,1), R(0,2), R(1,0); 33 | if(theta<1e-6) 34 | return w; 35 | else 36 | return theta*w/sin(theta); 37 | } 38 | 39 | } //namespace g2o 40 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/se3mat.h: -------------------------------------------------------------------------------- 1 | #ifndef SE3mat_H 2 | #define SE3mat_H 3 | 4 | #include 5 | #include 6 | 7 | namespace g2o { 8 | 9 | class SE3mat 10 | { 11 | public: 12 | SE3mat(){ 13 | R = Eigen::Matrix3d::Identity(); 14 | t.setZero(); 15 | } 16 | 17 | SE3mat(const Eigen::Matrix3d &R_, const Eigen::Vector3d &t_):R(R_),t(t_){} 18 | 19 | void Retract(const Eigen::Vector3d dr, const Eigen::Vector3d &dt); 20 | 21 | inline Eigen::Vector3d operator* (const Eigen::Vector3d& v) const { 22 | return R*v + t; 23 | } 24 | 25 | inline SE3mat& operator*= (const SE3mat& T2){ 26 | t+=R*T2.t; 27 | R*=T2.R; 28 | return *this; 29 | } 30 | 31 | inline SE3mat inverse() const{ 32 | Eigen::Matrix3d Rt = R.transpose(); 33 | return SE3mat(Rt,-Rt*t); 34 | } 35 | 36 | protected: 37 | Eigen::Vector3d t; 38 | Eigen::Matrix3d R; 39 | 40 | public: 41 | static Eigen::Matrix3d ExpSO3(const Eigen::Vector3d r); 42 | static Eigen::Vector3d LogSO3(const Eigen::Matrix3d R); 43 | }; 44 | 45 | }//namespace g2o 46 | 47 | #endif // SE3mat_H 48 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/types_sba.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 Kurt Konolige 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "types_sba.h" 28 | #include 29 | 30 | namespace g2o { 31 | 32 | using namespace std; 33 | 34 | 35 | VertexSBAPointXYZ::VertexSBAPointXYZ() : BaseVertex<3, Vector3d>() 36 | { 37 | } 38 | 39 | bool VertexSBAPointXYZ::read(std::istream& is) 40 | { 41 | Vector3d lv; 42 | for (int i=0; i<3; i++) 43 | is >> _estimate[i]; 44 | return true; 45 | } 46 | 47 | bool VertexSBAPointXYZ::write(std::ostream& os) const 48 | { 49 | Vector3d lv=estimate(); 50 | for (int i=0; i<3; i++){ 51 | os << lv[i] << " "; 52 | } 53 | return os.good(); 54 | } 55 | 56 | } // end namespace 57 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/types_sba.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 Kurt Konolige 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_SBA_TYPES 28 | #define G2O_SBA_TYPES 29 | 30 | #include "../core/base_vertex.h" 31 | 32 | #include 33 | #include 34 | 35 | namespace g2o { 36 | 37 | /** 38 | * \brief Point vertex, XYZ 39 | */ 40 | class VertexSBAPointXYZ : public BaseVertex<3, Vector3d> 41 | { 42 | public: 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 44 | VertexSBAPointXYZ(); 45 | virtual bool read(std::istream& is); 46 | virtual bool write(std::ostream& os) const; 47 | 48 | virtual void setToOriginImpl() { 49 | _estimate.fill(0.); 50 | } 51 | 52 | virtual void oplusImpl(const double* update) 53 | { 54 | Eigen::Map v(update); 55 | _estimate += v; 56 | } 57 | }; 58 | 59 | } // end namespace 60 | 61 | #endif // SBA_TYPES 62 | -------------------------------------------------------------------------------- /Thirdparty/g2o/license-bsd.txt: -------------------------------------------------------------------------------- 1 | g2o - General Graph Optimization 2 | Copyright (C) 2011 Rainer Kuemmerle, Giorgio Grisetti, Hauke Strasdat, 3 | Kurt Konolige, and Wolfram Burgard 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are 8 | met: 9 | 10 | * Redistributions of source code must retain the above copyright notice, 11 | this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 17 | IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 18 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 19 | PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 20 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 22 | TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 23 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 24 | LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 25 | NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | 28 | -------------------------------------------------------------------------------- /Thirdparty/line_descriptor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project( line-descriptor ) 2 | 3 | cmake_minimum_required(VERSION 2.7) 4 | find_package( OpenCV 3 REQUIRED) 5 | 6 | if(COMMAND cmake_policy) 7 | cmake_policy(SET CMP0003 NEW) 8 | endif(COMMAND cmake_policy) 9 | link_directories(${OpenCV_LIBS_DIR}) 10 | include_directories(${OpenCV2_INCLUDE_DIRS}) 11 | 12 | SET(BUILD_SHARED_LIBS ON) 13 | SET(CMAKE_MODULE_PATH $ENV{CMAKE_MODULE_PATH}) 14 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -O3 -mtune=native -march=native") 15 | 16 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/build) 17 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | include_directories( include ${OpenCV_INCLUDE_DIRS} ) 20 | list(APPEND LINK_LIBS ${OpenCV_LIBS} ) 21 | file(GLOB_RECURSE all_include_files RELATIVE "${CMAKE_SOURCE_DIR}" *.h *.hpp) 22 | 23 | link_directories(${CMAKE_SOURCE_DIR}/src/) 24 | file(GLOB_RECURSE all_source_files RELATIVE "${CMAKE_SOURCE_DIR}src/" *.cpp ) 25 | 26 | add_custom_target( linedesc_includes DEPENDS ${all_include_files} SOURCES ${all_source_files} ) 27 | 28 | add_library( linedesc ${all_source_files} ) 29 | target_link_libraries( linedesc ${LINK_LIBS} ) 30 | 31 | -------------------------------------------------------------------------------- /Thirdparty/line_descriptor/README.md: -------------------------------------------------------------------------------- 1 | Binary descriptors for lines extracted from an image 2 | ==================================================== -------------------------------------------------------------------------------- /Thirdparty/line_descriptor/src/bitarray_custom.hpp: -------------------------------------------------------------------------------- 1 | /*M/////////////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this license. 6 | // If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // 10 | // License Agreement 11 | // For Open Source Computer Vision Library 12 | // 13 | // Copyright (C) 2014, Mohammad Norouzi, Ali Punjani, David J. Fleet, 14 | // all rights reserved. 15 | // Third party copyrights are property of their respective owners. 16 | // 17 | // Redistribution and use in source and binary forms, with or without modification, 18 | // are permitted provided that the following conditions are met: 19 | // 20 | // * Redistribution's of source code must retain the above copyright notice, 21 | // this list of conditions and the following disclaimer. 22 | // 23 | // * Redistribution's in binary form must reproduce the above copyright notice, 24 | // this list of conditions and the following disclaimer in the documentation 25 | // and/or other materials provided with the distribution. 26 | // 27 | // * The name of the copyright holders may not be used to endorse or promote products 28 | // derived from this software without specific prior written permission. 29 | // 30 | // This software is provided by the copyright holders and contributors "as is" and 31 | // any express or implied warranties, including, but not limited to, the implied 32 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 33 | // In no event shall the Intel Corporation or contributors be liable for any direct, 34 | // indirect, incidental, special, exemplary, or consequential damages 35 | // (including, but not limited to, procurement of substitute goods or services; 36 | // loss of use, data, or profits; or business interruption) however caused 37 | // and on any theory of liability, whether in contract, strict liability, 38 | // or tort (including negligence or otherwise) arising in any way out of 39 | // the use of this software, even if advised of the possibility of such damage. 40 | // 41 | //M*/ 42 | 43 | #ifndef __OPENCV_BITARRAY_HPP 44 | #define __OPENCV_BITARRAY_HPP 45 | 46 | #ifdef _MSC_VER 47 | #pragma warning( disable : 4267 ) 48 | #endif 49 | 50 | #include "types_custom.hpp" 51 | #include 52 | #include 53 | #include 54 | 55 | /* class defining a sequence of bits */ 56 | class bitarray 57 | { 58 | 59 | public: 60 | /* pointer to bits sequence and sequence's length */ 61 | UINT32 *arr; 62 | UINT32 length; 63 | 64 | /* constructor setting default values */ 65 | bitarray() 66 | { 67 | arr = NULL; 68 | length = 0; 69 | } 70 | 71 | /* constructor setting sequence's length */ 72 | bitarray( UINT64 _bits ) 73 | { 74 | init( _bits ); 75 | } 76 | 77 | /* initializer of private fields */ 78 | void init( UINT64 _bits ) 79 | { 80 | length = (UINT32) ceil( _bits / 32.00 ); 81 | arr = new UINT32[length]; 82 | erase(); 83 | } 84 | 85 | /* destructor */ 86 | ~bitarray() 87 | { 88 | if( arr ) 89 | delete[] arr; 90 | } 91 | 92 | inline void flip( UINT64 index ) 93 | { 94 | arr[index >> 5] ^= ( (UINT32) 0x01 ) << ( index % 32 ); 95 | } 96 | 97 | inline void set( UINT64 index ) 98 | { 99 | arr[index >> 5] |= ( (UINT32) 0x01 ) << ( index % 32 ); 100 | } 101 | 102 | inline UINT8 get( UINT64 index ) 103 | { 104 | return ( arr[index >> 5] & ( ( (UINT32) 0x01 ) << ( index % 32 ) ) ) != 0; 105 | } 106 | 107 | /* reserve menory for an UINT32 */ 108 | inline void erase() 109 | { 110 | memset( arr, 0, sizeof(UINT32) * length ); 111 | } 112 | 113 | }; 114 | 115 | #endif 116 | -------------------------------------------------------------------------------- /Thirdparty/line_descriptor/src/precomp_custom.hpp: -------------------------------------------------------------------------------- 1 | /*M/////////////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this license. 6 | // If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // 10 | // License Agreement 11 | // For Open Source Computer Vision Library 12 | // 13 | // Copyright (C) 2014, Biagio Montesano, all rights reserved. 14 | // Third party copyrights are property of their respective owners. 15 | // 16 | // Redistribution and use in source and binary forms, with or without modification, 17 | // are permitted provided that the following conditions are met: 18 | // 19 | // * Redistribution's of source code must retain the above copyright notice, 20 | // this list of conditions and the following disclaimer. 21 | // 22 | // * Redistribution's in binary form must reproduce the above copyright notice, 23 | // this list of conditions and the following disclaimer in the documentation 24 | // and/or other materials provided with the distribution. 25 | // 26 | // * The name of the copyright holders may not be used to endorse or promote products 27 | // derived from this software without specific prior written permission. 28 | // 29 | // This software is provided by the copyright holders and contributors "as is" and 30 | // any express or implied warranties, including, but not limited to, the implied 31 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 32 | // In no event shall the Intel Corporation or contributors be liable for any direct, 33 | // indirect, incidental, special, exemplary, or consequential damages 34 | // (including, but not limited to, procurement of substitute goods or services; 35 | // loss of use, data, or profits; or business interruption) however caused 36 | // and on any theory of liability, whether in contract, strict liability, 37 | // or tort (including negligence or otherwise) arising in any way out of 38 | // the use of this software, even if advised of the possibility of such damage. 39 | // 40 | //M*/ 41 | 42 | #ifndef __OPENCV_PRECOMP_H__ 43 | #define __OPENCV_PRECOMP_H__ 44 | 45 | #ifdef _MSC_VER 46 | #pragma warning( disable : 4267 ) 47 | #endif 48 | 49 | #define _USE_MATH_DEFINES 50 | 51 | #include 52 | #include "opencv2/core/utility.hpp" 53 | #include 54 | #include 55 | #include 56 | #include "opencv2/core.hpp" 57 | 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | #include 67 | #include 68 | #include 69 | 70 | #include "bitarray_custom.hpp" 71 | #include "bitops_custom.hpp" 72 | #include "types_custom.hpp" 73 | 74 | #include "line_descriptor_custom.hpp" 75 | 76 | #endif 77 | -------------------------------------------------------------------------------- /Thirdparty/line_descriptor/src/types_custom.hpp: -------------------------------------------------------------------------------- 1 | /*M/////////////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this license. 6 | // If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // 10 | // License Agreement 11 | // For Open Source Computer Vision Library 12 | // 13 | // Copyright (C) 2014, Mohammad Norouzi, Ali Punjani, David J. Fleet, 14 | // all rights reserved. 15 | // Third party copyrights are property of their respective owners. 16 | // 17 | // Redistribution and use in source and binary forms, with or without modification, 18 | // are permitted provided that the following conditions are met: 19 | // 20 | // * Redistribution's of source code must retain the above copyright notice, 21 | // this list of conditions and the following disclaimer. 22 | // 23 | // * Redistribution's in binary form must reproduce the above copyright notice, 24 | // this list of conditions and the following disclaimer in the documentation 25 | // and/or other materials provided with the distribution. 26 | // 27 | // * The name of the copyright holders may not be used to endorse or promote products 28 | // derived from this software without specific prior written permission. 29 | // 30 | // This software is provided by the copyright holders and contributors "as is" and 31 | // any express or implied warranties, including, but not limited to, the implied 32 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 33 | // In no event shall the Intel Corporation or contributors be liable for any direct, 34 | // indirect, incidental, special, exemplary, or consequential damages 35 | // (including, but not limited to, procurement of substitute goods or services; 36 | // loss of use, data, or profits; or business interruption) however caused 37 | // and on any theory of liability, whether in contract, strict liability, 38 | // or tort (including negligence or otherwise) arising in any way out of 39 | // the use of this software, even if advised of the possibility of such damage. 40 | // 41 | //M*/ 42 | 43 | #if defined _MSC_VER && _MSC_VER <= 1700 44 | #include 45 | #else 46 | #include 47 | #endif 48 | 49 | #ifndef __OPENCV_TYPES_HPP 50 | #define __OPENCV_TYPES_HPP 51 | 52 | #ifdef _MSC_VER 53 | #pragma warning( disable : 4267 ) 54 | #endif 55 | 56 | /* define data types */ 57 | typedef uint64_t UINT64; 58 | typedef uint32_t UINT32; 59 | typedef uint16_t UINT16; 60 | typedef uint8_t UINT8; 61 | 62 | /* define constants */ 63 | #define UINT64_1 ((UINT64)0x01) 64 | #define UINT32_1 ((UINT32)0x01) 65 | 66 | #endif 67 | -------------------------------------------------------------------------------- /Vocabulary/README.md: -------------------------------------------------------------------------------- 1 | Download the ORB vocabulary file from https://github.com/raulmur/ORB_SLAM2 ORB_SLAM2/Vocabulary/ORBvoc.txt.tar.gz 2 | 3 | Download the line vocabulary file from https://github.com/rubengooj/pl-slam pl-slam/vocabulary/voc.tar.gz 4 | -------------------------------------------------------------------------------- /build.sh: -------------------------------------------------------------------------------- 1 | echo "Configuring and building Thirdparty/line_descriptor ..." 2 | 3 | cd Thirdparty/line_descriptor 4 | mkdir build 5 | cd build 6 | cmake .. -DCMAKE_BUILD_TYPE=Release 7 | make -j 8 | 9 | cd ../../DBoW2 10 | 11 | echo "Configuring and building Thirdparty/DBoW2 ..." 12 | 13 | mkdir build 14 | cd build 15 | cmake .. -DCMAKE_BUILD_TYPE=Release 16 | make -j8 17 | 18 | cd ../../g2o 19 | 20 | echo "Configuring and building Thirdparty/g2o ..." 21 | 22 | mkdir build 23 | cd build 24 | cmake .. -DCMAKE_BUILD_TYPE=Release 25 | make -j8 26 | 27 | cd ../../../ 28 | 29 | #echo "Uncompress vocabulary ..." 30 | 31 | #cd Vocabulary 32 | #tar -xf ORBvoc.txt.tar.gz 33 | #tar -xf LSvoc.txt.tar.gz 34 | #cd .. 35 | 36 | echo "Configuring and building PLI_SLAM ..." 37 | 38 | mkdir build 39 | cd build 40 | cmake .. -DCMAKE_BUILD_TYPE=Release 41 | make -j8 42 | -------------------------------------------------------------------------------- /build_ros.sh: -------------------------------------------------------------------------------- 1 | echo "Building ROS nodes" 2 | 3 | cd Examples/ROS/PLI_SLAM2 4 | mkdir build 5 | cd build 6 | cmake .. -DROS_BUILD_TYPE=Release 7 | make -j8 8 | -------------------------------------------------------------------------------- /include/Converter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef CONVERTER_H 21 | #define CONVERTER_H 22 | 23 | #include 24 | 25 | #include 26 | #include"Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" 27 | #include"Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" 28 | 29 | namespace ORB_SLAM3 30 | { 31 | 32 | class Converter 33 | { 34 | public: 35 | static std::vector toDescriptorVector(const cv::Mat &Descriptors); 36 | 37 | static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT); 38 | static g2o::SE3Quat toSE3Quat(const g2o::Sim3 &gSim3); 39 | 40 | static cv::Mat toCvMat(const g2o::SE3Quat &SE3); 41 | static cv::Mat toCvMat(const g2o::Sim3 &Sim3); 42 | static cv::Mat toCvMat(const Eigen::Matrix &m); 43 | static cv::Mat toCvMat(const Eigen::Matrix3d &m); 44 | static cv::Mat toCvMat(const Eigen::Matrix &m); 45 | static cv::Mat toCvMat(const Eigen::MatrixXd &m); 46 | static cv::Mat toInvCvMat(const cv::Mat &T_cvMat); 47 | static cv::Mat toInvCvMat(const Eigen::Matrix &T_eigenMtrx); 48 | 49 | static cv::Mat toCvSE3(const Eigen::Matrix &R, const Eigen::Matrix &t); 50 | static cv::Mat tocvSkewMatrix(const cv::Mat &v); 51 | 52 | static Eigen::Matrix toVector3d(const cv::Mat &cvVector); 53 | static Eigen::Matrix toVector3d(const cv::Point3f &cvPoint); 54 | static Eigen::Matrix toMatrix3d(const cv::Mat &cvMat3); 55 | static Eigen::Matrix toMatrix4d(const cv::Mat &cvMat4); 56 | static Eigen::Matrix toInvMatrix4d(const cv::Mat &T_cvMat); 57 | static Eigen::Matrix toInvMatrix4d(const Eigen::Matrix &T_eigenMtrx); 58 | 59 | static std::vector toQuaternion(const cv::Mat &M); 60 | static void RmatOfQuat(cv::Mat &M, const cv::Mat &q); 61 | 62 | static bool isRotationMatrix(const cv::Mat &R); 63 | static std::vector toEuler(const cv::Mat &R); 64 | 65 | }; 66 | 67 | }// namespace ORB_SLAM 68 | 69 | #endif // CONVERTER_H 70 | -------------------------------------------------------------------------------- /include/FrameDrawer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef FRAMEDRAWER_H 21 | #define FRAMEDRAWER_H 22 | 23 | #include "Tracking.h" 24 | #include "MapPoint.h" 25 | #include "MapLine.h" 26 | #include "Atlas.h" 27 | #include "opencv2/imgproc/imgproc.hpp" 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | 39 | namespace ORB_SLAM3 40 | { 41 | 42 | class Tracking; 43 | class Viewer; 44 | 45 | class FrameDrawer 46 | { 47 | public: 48 | FrameDrawer(Atlas* pAtlas); 49 | 50 | // Update info from the last processed frame. 51 | void Update(Tracking *pTracker); 52 | 53 | // Draw last processed frame. 54 | cv::Mat DrawFrame(bool bOldFeatures=true); 55 | cv::Mat DrawRightFrame(); 56 | 57 | bool both; 58 | 59 | protected: 60 | 61 | void DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText); 62 | 63 | // Info of the frame to be drawn 64 | // KeyPoint Info 65 | cv::Mat mIm, mImRight; 66 | int N, N_l; 67 | vector mvCurrentKeys,mvCurrentKeysRight; 68 | vector mvCurrentKeys_l, mvCurrentKeysRight_l; 69 | vector mvbMap, mvbVO, mvbMap_l, mvbVO_l; 70 | int mnTracked, mnTrackedVO; 71 | int mnTracked_l, mnTrackedVO_l; 72 | vector mvIniKeys; 73 | vector mvIniMatches; 74 | 75 | bool mbOnlyTracking; 76 | int mState; 77 | 78 | // mapping 79 | Atlas* mpAtlas; 80 | 81 | std::mutex mMutex; 82 | vector > mvTracks; 83 | 84 | Frame mCurrentFrame; 85 | 86 | vector mvpLocalMap; 87 | vector mvMatchedKeys; 88 | vector mvpMatchedMPs; 89 | vector mvOutlierKeys; 90 | vector mvpOutlierMPs; 91 | 92 | map mmProjectPoints; 93 | map mmMatchedInImage; 94 | 95 | }; 96 | 97 | } //namespace ORB_SLAM 98 | 99 | #endif // FRAMEDRAWER_H 100 | -------------------------------------------------------------------------------- /include/InitKeyFrame.h: -------------------------------------------------------------------------------- 1 | #ifndef INITKF_H 2 | #define INITKF_H 3 | 4 | #include "Thirdparty/DBoW2/DBoW2/BowVector.h" 5 | #include "Thirdparty/DBoW2/DBoW2/FeatureVector.h" 6 | #include "SystemSetting.h" 7 | #include 8 | #include "ORBVocabulary.h" 9 | #include "KeyFrameDatabase.h" 10 | #include "Frame.h" 11 | #include 12 | #include 13 | #include "ORBextractor.h" 14 | #include "LineExtractor.h" 15 | 16 | namespace ORB_SLAM3 17 | { 18 | #define FRAME_GRID_ROWS 48 19 | #define FRAME_GRID_COLS 64 20 | 21 | class SystemSetting; 22 | class KeyFrameDatabase; 23 | 24 | class InitKeyFrame 25 | { 26 | public: 27 | 28 | // InitKeyFrame(const Frame &frame); 29 | InitKeyFrame(SystemSetting &SS); 30 | 31 | void UndistortKeyPoints(); 32 | void UndistortKeyLines(); 33 | bool PosInGrid(const cv::KeyPoint& kp, int &posX, int &posY); 34 | void AssignFeaturesToGrid(); 35 | 36 | public: 37 | ORBVocabulary* pORBvocabulary; 38 | LineVocabulary* pLinevocabulary; 39 | 40 | long unsigned int nId; 41 | long unsigned int mnFrameId; 42 | double TimeStamp; 43 | 44 | float fGridElementWidthInv; 45 | float fGridElementHeightInv; 46 | std::vector vGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS]; 47 | 48 | float fx; 49 | float fy; 50 | float cx; 51 | float cy; 52 | float invfx; 53 | float invfy; 54 | float bf; 55 | float b; 56 | float ThDepth; 57 | int N; 58 | int N_l; 59 | std::vector vKps; 60 | std::vector vKpsUn; 61 | cv::Mat Descriptors; 62 | 63 | std::vector vRight; 64 | std::vector vDepth; 65 | 66 | DBoW2::BowVector BowVec; 67 | DBoW2::FeatureVector FeatVec; 68 | 69 | std::vector vKeys_Line; 70 | std::vector vKeysUn_Line; 71 | std::vector> vDisparity_l; 72 | std::vector vle_l; 73 | cv::Mat Descriptors_Line; 74 | 75 | //DBoW2::BowVector BowVec_l; 76 | //DBoW2::FeatureVector FeatVec_l; 77 | 78 | int nScaleLevels; 79 | float fScaleFactor; 80 | float fLogScaleFactor; 81 | std::vector vScaleFactors; 82 | std::vector vLevelSigma2; 83 | std::vector vInvLevelSigma2; 84 | std::vector vInvScaleFactors; 85 | 86 | int nMinX; 87 | int nMinY; 88 | int nMaxX; 89 | int nMaxY; 90 | cv::Mat K; 91 | cv::Mat DistCoef; 92 | }; 93 | } 94 | 95 | #endif 96 | -------------------------------------------------------------------------------- /include/LineExtractor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef LINEEXTRACTOR_H 22 | #define LINEEXTRACTOR_H 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include "Auxiliar.h" 29 | #include "Config.h" 30 | 31 | // #include 32 | // #include 33 | 34 | #include 35 | #include 36 | 37 | namespace ORB_SLAM3 38 | { 39 | 40 | class Lineextractor 41 | { 42 | public: 43 | Lineextractor(int _lsd_nfeatures, double _llength_th, bool _bFLD = false); 44 | Lineextractor(int _lsd_nfeatures, double _llength_th, int _lsd_refine, double _lsd_scale, double _lsd_sigma_scale, 45 | double _lsd_quant, double _lsd_ang_th, double _lsd_log_eps, double _lsd_density_th, int _lsd_n_bins, bool _bFLD = false); 46 | 47 | ~Lineextractor(){} 48 | 49 | void operator()( const cv::Mat& image, const cv::Mat& mask, 50 | std::vector& keylines, 51 | cv::Mat& descriptors_line); 52 | 53 | protected: 54 | int lsd_nfeatures; 55 | double min_line_length; 56 | 57 | // lines detection and matching 58 | int lsd_refine; 59 | double lsd_scale; 60 | double lsd_sigma_scale; 61 | double lsd_quant; 62 | double lsd_ang_th; 63 | double lsd_log_eps; 64 | double lsd_density_th; 65 | int lsd_n_bins; 66 | // double line_horiz_th; 67 | 68 | bool bFLD; 69 | 70 | double min_ratio_12_l; 71 | double ls_min_disp_ratio; 72 | }; 73 | 74 | } //namespace ORB_SLAM 75 | 76 | #endif 77 | 78 | -------------------------------------------------------------------------------- /include/LineIterator.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #pragma once 24 | 25 | //STL 26 | #include 27 | 28 | namespace ORB_SLAM3 { 29 | 30 | class LineIterator { 31 | public: 32 | 33 | LineIterator(const double x1_, const double y1_, const double x2_, const double y2_); 34 | 35 | bool getNext(std::pair &pixel); 36 | 37 | private: 38 | 39 | const bool steep; 40 | double x1, y1, x2, y2; 41 | 42 | double dx, dy, error; 43 | 44 | int maxX, ystep; 45 | int y, x; 46 | }; 47 | 48 | } // namespace ORB_SLAM2 49 | -------------------------------------------------------------------------------- /include/LineMatcher.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #pragma once 24 | 25 | //STL 26 | #include 27 | #include 28 | 29 | //OpenCV 30 | #include 31 | 32 | #include "gridStructure.h" 33 | #include "Frame.h" 34 | #include "MapPoint.h" 35 | #include "MapLine.h" 36 | 37 | using namespace line_descriptor; 38 | 39 | namespace ORB_SLAM3 { 40 | 41 | class Frame; 42 | class MapPoint; 43 | class MapLine; 44 | 45 | typedef std::pair point_2d; 46 | typedef std::pair line_2d; 47 | 48 | inline double dot(const std::pair &a, const std::pair &b) { 49 | return (a.first*b.first + a.second*b.second); 50 | } 51 | 52 | inline void normalize(std::pair &v) { 53 | double magnitude = std::sqrt(dot(v, v)); 54 | 55 | v.first /= magnitude; 56 | v.second /= magnitude; 57 | } 58 | 59 | int matchNNR(const cv::Mat &desc1, const cv::Mat &desc2, float nnr, std::vector &matches_12); 60 | 61 | int match(const std::vector &mvpLocalMapLines, Frame &CurrentFrame, float nnr, std::vector &matches_12); 62 | 63 | int match(const cv::Mat &desc1, const cv::Mat &desc2, float nnr, std::vector &matches_12); 64 | 65 | int distance(const cv::Mat &a, const cv::Mat &b); 66 | 67 | //Points 68 | int matchGrid(const std::vector &points1, const cv::Mat &desc1, const GridStructure &grid, const cv::Mat &desc2, const GridWindow &w, std::vector &matches_12); 69 | 70 | //Lines 71 | int matchGrid(const std::vector &lines1, const cv::Mat &desc1, const GridStructure &grid, const cv::Mat &desc2, const std::vector> &directions2, const GridWindow &w, std::vector &matches_12); 72 | 73 | class Linematcher 74 | { 75 | public: 76 | Linematcher(float nnratio=0.7, bool checkOri=true); 77 | int SearchDouble(Frame &InitialFrame, Frame &CurrentFrame, vector &LineMatches); 78 | 79 | public: 80 | 81 | static const int TH_LOW; 82 | static const int TH_HIGH; 83 | static const int HISTO_LENGTH; 84 | 85 | protected: 86 | void FrameBFMatch(cv::Mat ldesc1, cv::Mat ldesc2, vector& LineMatches, float TH); 87 | void lineDescriptorMAD(vector> line_matches, double &nn_mad, double &nn12_mad) const; 88 | 89 | float mfNNratio; 90 | bool mbCheckOrientation; 91 | }; 92 | } // namesapce ORB_SLAM2 93 | -------------------------------------------------------------------------------- /include/MapDrawer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef MAPDRAWER_H 21 | #define MAPDRAWER_H 22 | 23 | #include"Atlas.h" 24 | #include"Map.h" 25 | #include"MapPoint.h" 26 | #include"MapLine.h" 27 | #include"KeyFrame.h" 28 | #include 29 | 30 | #include 31 | 32 | namespace ORB_SLAM3 33 | { 34 | 35 | class MapDrawer 36 | { 37 | public: 38 | MapDrawer(Atlas* pAtlas, const string &strSettingPath); 39 | 40 | Atlas* mpAtlas; 41 | 42 | void DrawMapPoints(); 43 | void DrawMapLines(); 44 | void DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph, const bool bDrawInertialGraph); 45 | void DrawCurrentCamera(pangolin::OpenGlMatrix &Twc); 46 | void SetCurrentCameraPose(const cv::Mat &Tcw); 47 | void SetReferenceKeyFrame(KeyFrame *pKF); 48 | void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw); 49 | void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw, pangolin::OpenGlMatrix &MTwwp); 50 | 51 | private: 52 | 53 | bool ParseViewerParamFile(cv::FileStorage &fSettings); 54 | 55 | float mKeyFrameSize; 56 | float mKeyFrameLineWidth; 57 | float mGraphLineWidth; 58 | float mPointSize; 59 | float mCameraSize; 60 | float mCameraLineWidth; 61 | 62 | cv::Mat mCameraPose; 63 | 64 | std::mutex mMutexCamera; 65 | 66 | float mfFrameColors[6][3] = {{0.0f, 0.0f, 1.0f}, 67 | {0.8f, 0.4f, 1.0f}, 68 | {1.0f, 0.2f, 0.4f}, 69 | {0.6f, 0.0f, 1.0f}, 70 | {1.0f, 1.0f, 0.0f}, 71 | {0.0f, 1.0f, 1.0f}}; 72 | }; 73 | 74 | } //namespace ORB_SLAM 75 | 76 | #endif // MAPDRAWER_H 77 | -------------------------------------------------------------------------------- /include/MapLine.h: -------------------------------------------------------------------------------- 1 | #ifndef MAPLINE_H 2 | #define MAPLINE_H 3 | 4 | #include"KeyFrame.h" 5 | #include"Frame.h" 6 | #include"Map.h" 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | using namespace Eigen; 14 | 15 | typedef Eigen::Matrix Matrix6d; 16 | typedef Eigen::Matrix Vector6d; 17 | 18 | namespace ORB_SLAM3 19 | { 20 | 21 | class KeyFrame; 22 | class Map; 23 | class Frame; 24 | 25 | class MapLine 26 | { 27 | public: 28 | MapLine(); 29 | 30 | MapLine(Vector6d &Pos, KeyFrame *pRefKF, Map *pMap); 31 | MapLine(const Eigen::Vector3d &sP, const Eigen::Vector3d &eP, Map* pMap); 32 | MapLine(const Eigen::Vector3d &sP, const Eigen::Vector3d &eP, KeyFrame* pRefKF, Map* pMap); 33 | MapLine(const Eigen::Vector3d &sP, const Eigen::Vector3d &eP, Map* pMap, Frame* pFrame, const int &idxF); 34 | 35 | void SetWorldPos(const Eigen::Vector3d &sP, const Eigen::Vector3d &eP); 36 | void SetWorldPos(const Vector6d &Pos); 37 | Vector6d GetWorldPos(); 38 | 39 | KeyFrame* GetReferenceKeyFrame(); 40 | 41 | std::map GetObservations(); 42 | int Observations(); 43 | 44 | void AddObservation(KeyFrame* pKF,int idx); 45 | void EraseObservation(KeyFrame* pKF); 46 | 47 | int GetIndexInKeyFrame(KeyFrame* pKF); 48 | bool IsInKeyFrame(KeyFrame* pKF); 49 | 50 | void SetBadFlag(); 51 | bool isBad(); 52 | 53 | void Replace(MapLine* pML); 54 | MapLine* GetReplaced(); 55 | 56 | void IncreaseVisible(int n=1); 57 | void IncreaseFound(int n=1); 58 | float GetFoundRatio(); 59 | inline int GetFound(){ 60 | return mnFound; 61 | } 62 | void ComputeDistinctiveDescriptors(); 63 | 64 | cv::Mat GetDescriptor(); 65 | 66 | Map* GetMap(); 67 | 68 | void UpdateAverageDir(); 69 | void UpdateMap(Map* pMap); 70 | 71 | //void UpdateNormalAndDepth(); 72 | 73 | KeyFrame* SetReferenceKeyFrame(KeyFrame* RFKF); 74 | 75 | public: 76 | long unsigned int mnId; 77 | static long unsigned int nNextId; 78 | long int mnFirstKFid; 79 | long int mnFirstFrame; 80 | int nObs; 81 | 82 | static std::mutex mGlobalMutex; 83 | 84 | // Position in absolute coordinates 85 | Eigen::Vector3d mWorldPos_sP; 86 | Eigen::Vector3d mWorldPos_eP; 87 | 88 | // Tracking 89 | bool mbTrackInView; 90 | float mTrackProjsX; 91 | float mTrackProjsY; 92 | float mTrackProjeX; 93 | float mTrackProjeY; 94 | double mnTrackangle; 95 | long unsigned int mnTrackReferenceForFrame; 96 | long unsigned int mnLastFrameSeen; 97 | 98 | bool mbTrackInViewLine, mbTrackInViewRLine; //new 99 | 100 | // Variables used by local mapping 101 | long unsigned int mnBALocalForKF; 102 | long unsigned int mnFuseCandidateForKF; 103 | 104 | // Variables used by loop closing 105 | long unsigned int mnLoopLineForKF; 106 | long unsigned int mnCorrectedByKF; 107 | long unsigned int mnCorrectedReference; 108 | cv::Mat mPosGBA; 109 | long unsigned int mnBAGlobalForKF; 110 | 111 | public: 112 | // Position in absolute coordinates 113 | Vector6d mWorldPos; 114 | Vector3d mStart3D; 115 | Vector3d mEnd3D; 116 | Vector3d mWorldVector; 117 | Vector3d mNormalVector; //MapPoint中，指的是该MapPoint的平均观测方向，这里指的是观测特征线段的方向 118 | 119 | protected: 120 | 121 | // Keyframes observing the point and associated index in keyframe 122 | std::map mObservations; 123 | 124 | // Best descriptor to fast matching 125 | cv::Mat mDescriptor; 126 | 127 | // Reference KeyFrame 128 | KeyFrame* mpRefKF; 129 | 130 | // Tracking counters 131 | int mnVisible; 132 | int mnFound; 133 | 134 | // Bad flag (we do not currently erase MapPoint from memory) 135 | bool mbBad; 136 | MapLine* mpReplaced; 137 | 138 | Map* mpMap; 139 | 140 | std::mutex mMutexMap; //new 141 | std::mutex mMutexPos; 142 | std::mutex mMutexFeatures; 143 | }; 144 | 145 | } //namespace ORB_SLAM 146 | 147 | #endif // MAPLINE_H 148 | -------------------------------------------------------------------------------- /include/ORBVocabulary.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef ORBVOCABULARY_H 21 | #define ORBVOCABULARY_H 22 | 23 | #include"Thirdparty/DBoW2/DBoW2/FORB.h" 24 | #include"Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h" 25 | 26 | namespace ORB_SLAM3 27 | { 28 | 29 | typedef DBoW2::TemplatedVocabulary 30 | ORBVocabulary; 31 | 32 | typedef DBoW2::TemplatedVocabulary 33 | LineVocabulary; 34 | 35 | } //namespace ORB_SLAM 36 | 37 | #endif // ORBVOCABULARY_H 38 | -------------------------------------------------------------------------------- /include/ORBextractor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | #ifndef ORBEXTRACTOR_H 20 | #define ORBEXTRACTOR_H 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include "Auxiliar.h" 27 | #include "Config.h" 28 | using namespace cv; 29 | 30 | 31 | namespace ORB_SLAM3 32 | { 33 | 34 | class ExtractorNode 35 | { 36 | public: 37 | ExtractorNode():bNoMore(false){} 38 | 39 | void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4); 40 | 41 | std::vector vKeys; 42 | cv::Point2i UL, UR, BL, BR; 43 | std::list::iterator lit; 44 | bool bNoMore; 45 | }; 46 | 47 | class ORBextractor 48 | { 49 | public: 50 | 51 | enum {HARRIS_SCORE=0, FAST_SCORE=1 }; 52 | 53 | ORBextractor(int nfeatures, float scaleFactor, int nlevels, 54 | int iniThFAST, int minThFAST); 55 | 56 | ~ORBextractor(){} 57 | 58 | // Compute the ORB features and descriptors on an image. 59 | // ORB are dispersed on the image using an octree. 60 | // Mask is ignored in the current implementation. 61 | int operator()( cv::InputArray _image, cv::InputArray _mask, 62 | std::vector& _keypoints, 63 | cv::OutputArray _descriptors, std::vector &vLappingArea); 64 | 65 | int inline GetLevels(){ 66 | return nlevels;} 67 | 68 | float inline GetScaleFactor(){ 69 | return scaleFactor;} 70 | 71 | std::vector inline GetScaleFactors(){ 72 | return mvScaleFactor; 73 | } 74 | 75 | std::vector inline GetInverseScaleFactors(){ 76 | return mvInvScaleFactor; 77 | } 78 | 79 | std::vector inline GetScaleSigmaSquares(){ 80 | return mvLevelSigma2; 81 | } 82 | 83 | std::vector inline GetInverseScaleSigmaSquares(){ 84 | return mvInvLevelSigma2; 85 | } 86 | 87 | std::vector mvImagePyramid; 88 | 89 | protected: 90 | 91 | void ComputePyramid(cv::Mat image); 92 | void ComputeKeyPointsOctTree(std::vector >& allKeypoints); 93 | std::vector DistributeOctTree(const std::vector& vToDistributeKeys, const int &minX, 94 | const int &maxX, const int &minY, const int &maxY, const int &nFeatures, const int &level); 95 | 96 | void ComputeKeyPointsOld(std::vector >& allKeypoints); 97 | std::vector pattern; 98 | 99 | int nfeatures; 100 | double scaleFactor; 101 | int nlevels; 102 | int iniThFAST; 103 | int minThFAST; 104 | 105 | std::vector mnFeaturesPerLevel; 106 | 107 | std::vector umax; 108 | 109 | std::vector mvScaleFactor; 110 | std::vector mvInvScaleFactor; 111 | std::vector mvLevelSigma2; 112 | std::vector mvInvLevelSigma2; 113 | }; 114 | 115 | } //namespace ORB_SLAM 116 | 117 | #endif 118 | 119 | -------------------------------------------------------------------------------- /include/SystemSetting.h: -------------------------------------------------------------------------------- 1 | #ifndef SYSTEMSETTING_H 2 | #define SYSTEMSETTING_H 3 | 4 | #include 5 | #include "ORBVocabulary.h" 6 | #include 7 | 8 | namespace ORB_SLAM3 9 | { 10 | class SystemSetting 11 | { 12 | public: 13 | SystemSetting(ORBVocabulary* pORBVoc, LineVocabulary* pLineVoc); 14 | bool LoadSystemSetting(const std::string strSettingPath); 15 | ~SystemSetting(); 16 | 17 | ORBVocabulary* pORBvocabulary; 18 | LineVocabulary* pLinevocabulary; 19 | 20 | float width; 21 | float height; 22 | float fx; 23 | float fy; 24 | float cx; 25 | float cy; 26 | float invfx; 27 | float invfy; 28 | float bf; 29 | float b; 30 | float fps; 31 | cv::Mat K; 32 | cv::Mat DistCoef; 33 | bool initialized; 34 | 35 | //camera feature parameters 36 | int nRGB; 37 | 38 | //ORB feature parameters 39 | int nFeatures; 40 | float fScaleFactor; 41 | int nLevels; 42 | float fIniThFAST; 43 | float fMinThFAST; 44 | 45 | //other 46 | float ThDepth = -1; 47 | float DepthMapFactor = -1; 48 | 49 | }; 50 | } 51 | #endif 52 | -------------------------------------------------------------------------------- /include/Viewer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef VIEWER_H 21 | #define VIEWER_H 22 | 23 | #include "FrameDrawer.h" 24 | #include "MapDrawer.h" 25 | #include "Tracking.h" 26 | #include "System.h" 27 | 28 | #include 29 | 30 | namespace ORB_SLAM3 31 | { 32 | 33 | class Tracking; 34 | class FrameDrawer; 35 | class MapDrawer; 36 | class System; 37 | 38 | class Viewer 39 | { 40 | public: 41 | Viewer(System* pSystem, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Tracking *pTracking, const string &strSettingPath); 42 | 43 | // Main thread function. Draw points, keyframes, the current camera pose and the last processed 44 | // frame. Drawing is refreshed according to the camera fps. We use Pangolin. 45 | void Run(); 46 | 47 | void RequestFinish(); 48 | 49 | void RequestStop(); 50 | 51 | bool isFinished(); 52 | 53 | bool isStopped(); 54 | 55 | bool isStepByStep(); 56 | 57 | void Release(); 58 | 59 | void SetTrackingPause(); 60 | 61 | bool both; 62 | private: 63 | 64 | bool ParseViewerParamFile(cv::FileStorage &fSettings); 65 | 66 | bool Stop(); 67 | 68 | System* mpSystem; 69 | FrameDrawer* mpFrameDrawer; 70 | MapDrawer* mpMapDrawer; 71 | Tracking* mpTracker; 72 | 73 | // 1/fps in ms 74 | double mT; 75 | float mImageWidth, mImageHeight; 76 | 77 | float mViewpointX, mViewpointY, mViewpointZ, mViewpointF; 78 | 79 | bool CheckFinish(); 80 | void SetFinish(); 81 | bool mbFinishRequested; 82 | bool mbFinished; 83 | std::mutex mMutexFinish; 84 | 85 | bool mbStopped; 86 | bool mbStopRequested; 87 | std::mutex mMutexStop; 88 | 89 | bool mbStopTrack; 90 | 91 | }; 92 | 93 | } 94 | 95 | 96 | #endif // VIEWER_H 97 | 98 | 99 | -------------------------------------------------------------------------------- /include/gridStructure.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #pragma once 24 | 25 | //STL 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace ORB_SLAM3 { 32 | 33 | void getLineCoords(double x1, double y1, double x2, double y2, std::list> &line_coords); 34 | 35 | struct GridWindow { 36 | std::pair width, height; 37 | }; 38 | 39 | class GridStructure { 40 | public: 41 | 42 | int rows, cols; 43 | 44 | GridStructure(int rows, int cols); 45 | 46 | ~GridStructure(); 47 | 48 | std::list& at(int x, int y); 49 | 50 | void get(int x, int y, const GridWindow &w, std::unordered_set &indices) const; 51 | 52 | void clear(); 53 | 54 | private: 55 | 56 | std::vector>> grid; 57 | std::list out_of_bounds; 58 | }; 59 | 60 | } // namespace ORB_SLAM3 61 | -------------------------------------------------------------------------------- /src/LineExtractor.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include "LineExtractor.h" 8 | 9 | using namespace cv; 10 | using namespace line_descriptor; 11 | using namespace std; 12 | 13 | namespace ORB_SLAM3 14 | { 15 | 16 | Lineextractor::Lineextractor(int _lsd_nfeatures, double _llength_th, bool _bFLD) 17 | :lsd_nfeatures(_lsd_nfeatures), min_line_length(_llength_th), bFLD(_bFLD) 18 | { 19 | 20 | } 21 | 22 | Lineextractor::Lineextractor(int _lsd_nfeatures, double _llength_th, int _lsd_refine, double _lsd_scale, double _lsd_sigma_scale, 23 | double _lsd_quant, double _lsd_ang_th, double _lsd_log_eps, double _lsd_density_th, int _lsd_n_bins, bool _bFLD) 24 | :lsd_nfeatures(_lsd_nfeatures), min_line_length(_llength_th), lsd_refine(_lsd_refine), lsd_scale(_lsd_scale), 25 | lsd_sigma_scale(_lsd_sigma_scale), lsd_quant(_lsd_quant), lsd_ang_th(_lsd_ang_th), lsd_log_eps(_lsd_log_eps), 26 | lsd_density_th(_lsd_density_th), lsd_n_bins(_lsd_n_bins), bFLD(_bFLD) 27 | { 28 | 29 | } 30 | 31 | void Lineextractor::operator()( const cv::Mat& img, const cv::Mat& mask, 32 | std::vector& keylines, cv::Mat& descriptors_line) 33 | { 34 | // Detect line features 35 | keylines.clear(); 36 | Ptr lbd = BinaryDescriptor::createBinaryDescriptor(); 37 | if( Config::hasLines() ) 38 | { 39 | 40 | if( !bFLD ) 41 | { 42 | Ptr lsd = line_descriptor::LSDDetectorC::createLSDDetectorC(); 43 | // lsd parameters 44 | line_descriptor::LSDDetectorC::LSDOptions opts; 45 | opts.refine = lsd_refine; 46 | opts.scale = lsd_scale; 47 | opts.sigma_scale = lsd_sigma_scale; 48 | opts.quant = lsd_quant; 49 | opts.ang_th = lsd_ang_th; 50 | opts.log_eps = lsd_log_eps; 51 | opts.density_th = lsd_density_th; 52 | opts.n_bins = lsd_n_bins; 53 | opts.min_length = min_line_length*(std::min(img.cols,img.rows)); 54 | lsd->detect( img, keylines, lsd_scale, 1, opts); 55 | // filter keylines 56 | if( int(keylines.size())>lsd_nfeatures && lsd_nfeatures!=0 ) 57 | { 58 | // sort keylines by their response 59 | sort( keylines.begin(), keylines.end(), sort_lines_by_response() ); 60 | //sort( keylines.begin(), keylines.end(), sort_lines_by_length() ); 61 | keylines.resize(lsd_nfeatures); 62 | // reassign index 63 | for( int i = 0; i < lsd_nfeatures; i++ ) 64 | keylines[i].class_id = i; 65 | } 66 | lbd->compute( img, keylines, descriptors_line); 67 | } 68 | else return; 69 | } 70 | } 71 | 72 | } //namespace ORB_SLAM 73 | -------------------------------------------------------------------------------- /src/LineIterator.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #include "LineIterator.h" 24 | 25 | //STL 26 | #include 27 | #include 28 | 29 | //Implementation of Bresenham's line drawing Algorithm 30 | //Adapted from: https://rosettacode.org/wiki/Bitmap/Bresenham%27s_line_algorithm#C.2B.2B 31 | 32 | namespace ORB_SLAM3 { 33 | 34 | LineIterator::LineIterator(const double x1_, const double y1_, const double x2_, const double y2_) 35 | : x1(x1_), y1(y1_), x2(x2_), y2(y2_), steep(std::abs(y2_ - y1_) > std::abs(x2_ - x1_)) { 36 | 37 | if (steep) { 38 | std::swap(x1, y1); 39 | std::swap(x2, y2); 40 | } 41 | 42 | if(x1 > x2) { 43 | std::swap(x1, x2); 44 | std::swap(y1, y2); 45 | } 46 | 47 | dx = x2 - x1; 48 | dy = std::abs(y2 - y1); 49 | 50 | error = dx / 2.0; 51 | ystep = (y1 < y2) ? 1 : -1; 52 | 53 | x = static_cast(x1); 54 | y = static_cast(y1); 55 | 56 | maxX = static_cast(x2); 57 | } 58 | 59 | bool LineIterator::getNext(std::pair &pixel) { 60 | 61 | if (x > maxX) 62 | return false; 63 | 64 | if (steep) 65 | pixel = std::make_pair(y, x); 66 | else 67 | pixel = std::make_pair(x, y); 68 | 69 | error -= dy; 70 | if (error < 0) { 71 | y += ystep; 72 | error += dx; 73 | } 74 | 75 | x++; 76 | return true; 77 | } 78 | 79 | } // namespace ORB_SLAM2 80 | -------------------------------------------------------------------------------- /src/SystemSetting.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include "SystemSetting.h" 3 | 4 | using namespace std; 5 | 6 | namespace ORB_SLAM3 7 | { 8 | SystemSetting::SystemSetting(ORBVocabulary* pORBVoc, LineVocabulary* pLineVoc): 9 | pORBvocabulary(pORBVoc),pLinevocabulary(pLineVoc) 10 | { 11 | 12 | } 13 | 14 | bool SystemSetting::LoadSystemSetting(const std::string strSettingPath) 15 | { 16 | cout<(0,0) = fx; 27 | tmpK.at(1,1) = fy; 28 | tmpK.at(0,2) = cx; 29 | tmpK.at(1,2) = cy; 30 | tmpK.copyTo(K); 31 | 32 | cv::Mat tmpDistCoef(4,1,CV_32F); 33 | tmpDistCoef.at(0) = fSettings["Camera.k1"]; 34 | tmpDistCoef.at(1) = fSettings["Camera.k2"]; 35 | tmpDistCoef.at(2) = fSettings["Camera.p1"]; 36 | tmpDistCoef.at(3) = fSettings["Camera.p2"]; 37 | const float k3 = fSettings["Camera.k3"]; 38 | if( k3 != 0 ) 39 | { 40 | tmpDistCoef.resize(5); 41 | tmpDistCoef.at(4) = k3; 42 | } 43 | tmpDistCoef.copyTo( DistCoef ); 44 | 45 | bf = fSettings["Camera.bf"]; 46 | fps = fSettings["Camera.fps"]; 47 | 48 | invfx = 1.0f/fx; 49 | invfy = 1.0f/fy; 50 | b = bf/fx; 51 | initialized = true; 52 | 53 | cout << "-size:"<(0) << endl; 59 | cout << "- k2: " << DistCoef.at(1) << endl; 60 | if(DistCoef.rows==5) 61 | cout << "- k3: " << DistCoef.at(4) << endl; 62 | cout << "- p1: " << DistCoef.at(2) << endl; 63 | cout << "- p2: " << DistCoef.at(3) << endl; 64 | cout << "- bf: " << bf << endl; 65 | 66 | nRGB = fSettings["Camera.RGB"]; 67 | 68 | nFeatures = fSettings["ORBextractor.nFeatures"]; 69 | fScaleFactor = fSettings["ORBextractor.scaleFactor"]; 70 | nLevels = fSettings["ORBextractor.nLevels"]; 71 | fIniThFAST = fSettings["ORBextractor.iniThFAST"]; 72 | fMinThFAST = fSettings["ORBextractor.minThFAST"]; 73 | 74 | cout << endl << "ORB Extractor Parameters: " << endl; 75 | cout << "- Number of Features: " << nFeatures << endl; 76 | cout << "- Scale Levels: " << nLevels << endl; 77 | cout << "- Scale Factor: " << fScaleFactor << endl; 78 | cout << "- Initial Fast Threshold: " << fIniThFAST << endl; 79 | cout << "- Minimum Fast Threshold: " << fMinThFAST << endl << endl; 80 | 81 | fSettings.release(); 82 | return true; 83 | } 84 | } 85 | -------------------------------------------------------------------------------- /src/gridStructure.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #include "gridStructure.h" 24 | 25 | //STL 26 | #include 27 | #include 28 | 29 | #include "LineIterator.h" 30 | 31 | namespace ORB_SLAM3 { 32 | 33 | void getLineCoords(double x1, double y1, double x2, double y2, std::list> &line_coords) { 34 | line_coords.clear(); 35 | 36 | LineIterator it(x1, y1, x2, y2); 37 | 38 | std::pair p; 39 | while (it.getNext(p)) 40 | line_coords.push_back(p); 41 | } 42 | 43 | GridStructure::GridStructure(int rows, int cols) 44 | : rows(rows), cols(cols) { 45 | 46 | if (rows <= 0 || cols <= 0) 47 | throw std::runtime_error("[GridStructure] invalid dimension"); 48 | 49 | grid.resize(cols, std::vector>(rows)); 50 | } 51 | 52 | GridStructure::~GridStructure() { 53 | 54 | } 55 | 56 | std::list& GridStructure::at(int x, int y) { 57 | 58 | if (x >= 0 && x < cols && 59 | y >= 0 && y < rows) 60 | return grid[x][y]; 61 | else 62 | return out_of_bounds; 63 | } 64 | 65 | void GridStructure::get(int x, int y, const GridWindow &w, std::unordered_set &indices) const { 66 | 67 | int min_x = std::max(0, x - w.width.first); 68 | int max_x = std::min(cols, x + w.width.second + 1); 69 | 70 | int min_y = std::max(0, y - w.height.first); 71 | int max_y = std::min(rows, y + w.height.second + 1); 72 | 73 | for (int x_ = min_x; x_ < max_x; ++x_) 74 | for (int y_ = min_y; y_ < max_y; ++y_) 75 | indices.insert(grid[x_][y_].begin(), grid[x_][y_].end()); 76 | } 77 | 78 | void GridStructure::clear() { 79 | 80 | for (int x = 0; x < cols; ++x) 81 | for (int y = 0; y < rows; ++y) 82 | grid[x][y].clear(); 83 | } 84 | 85 | } //namesapce ORB_SLAM3 86 | --------------------------------------------------------------------------------