├── .gitignore ├── Additional evaluations.pdf ├── CMakeLists.txt ├── CMakeModules ├── FindEigen.cmake └── FindG2O.cmake ├── LICENCE ├── README.md ├── bin └── Monocular-Inertial │ ├── EuRoC.yaml │ ├── EuRoC_TimeStamps │ ├── MH01.txt │ ├── MH02.txt │ ├── MH03.txt │ ├── MH04.txt │ ├── MH05.txt │ ├── V101.txt │ ├── V102.txt │ ├── V103.txt │ ├── V201.txt │ ├── V202.txt │ └── V203.txt │ ├── TUM_512.yaml │ └── TUM_TimeStamps │ ├── dataset-corridor1_512.txt │ ├── dataset-corridor2_1024.txt │ ├── dataset-corridor2_512.txt │ ├── dataset-corridor3_512.txt │ ├── dataset-corridor4_512.txt │ ├── dataset-corridor5_512.txt │ ├── dataset-magistrale1_512.txt │ ├── dataset-magistrale2_512.txt │ ├── dataset-magistrale3_512.txt │ ├── dataset-magistrale4_512.txt │ ├── dataset-magistrale5_512.txt │ ├── dataset-magistrale6_512.txt │ ├── dataset-outdoors1_512.txt │ ├── dataset-outdoors2_512.txt │ ├── dataset-outdoors3_512.txt │ ├── dataset-outdoors4_512.txt │ ├── dataset-outdoors5_512.txt │ ├── dataset-outdoors6_512.txt │ ├── dataset-outdoors7_512.txt │ ├── dataset-outdoors8_512.txt │ ├── dataset-room1_512.txt │ ├── dataset-room2_512.txt │ ├── dataset-room3_512.txt │ ├── dataset-room4_512.txt │ ├── dataset-room5_512.txt │ ├── dataset-room6_512.txt │ ├── dataset-slides1_512.txt │ ├── dataset-slides2_512.txt │ └── dataset-slides3_512.txt ├── build.sh ├── include ├── Config.yaml ├── G2oTypes.h ├── ImuTypes.h ├── LocalMapping.h ├── SettingParameters.h ├── SystemNode.h ├── transformation.h └── vihso │ ├── CoarseTracker.h │ ├── ImageReader.h │ ├── IndexThreadReduce.h │ ├── MatrixAccumulator.h │ ├── PhotomatricCalibration.h │ ├── bundle_adjustment.h │ ├── camera.h │ ├── config.h │ ├── depth_filter.h │ ├── feature.h │ ├── feature_alignment.h │ ├── feature_detection.h │ ├── frame.h │ ├── frame_handler_base.h │ ├── frame_handler_mono.h │ ├── global.h │ ├── initialization.h │ ├── map.h │ ├── matcher.h │ ├── point.h │ ├── pose_optimizer.h │ ├── reprojector.h │ ├── viewer.h │ └── vikit │ ├── aligned_mem.h │ ├── colortable.h │ ├── homography.h │ ├── math_utils.h │ ├── patch_score.h │ ├── performance_monitor.h │ ├── ringbuffer.h │ ├── robust_cost.h │ ├── timer.h │ └── vision.h ├── lib └── libhso.so ├── src ├── CoarseTracker.cpp ├── G2oTypes.cpp ├── ImageReader.cpp ├── ImuTypes.cpp ├── LocalMapping.cpp ├── PhotomatricCalibration.cpp ├── SettingParameters.cpp ├── SystemNode.cpp ├── bundle_adjustment.cpp ├── camera.cpp ├── config.cpp ├── depth_filter.cpp ├── feature_alignment.cpp ├── feature_detection.cpp ├── frame.cpp ├── frame_handler_base.cpp ├── frame_handler_mono.cpp ├── initialization.cpp ├── map.cpp ├── matcher.cpp ├── point.cpp ├── pose_optimizer.cpp ├── reprojector.cpp ├── viewer.cpp └── vikit │ ├── homography.cpp │ ├── math_utils.cpp │ ├── performance_monitor.cpp │ ├── robust_cost.cpp │ └── vision.cpp ├── test ├── vi_euroc.cc └── vi_tum.cc └── thirdparty ├── fast ├── CMakeLists.txt ├── LICENSE ├── README.md ├── fastConfig.cmake.in ├── include │ └── fast │ │ ├── corner_10.h │ │ ├── corner_9.h │ │ ├── fast.h │ │ └── faster_corner_utilities.h ├── package.xml ├── src │ ├── fast_10.cpp │ ├── fast_10_score.cpp │ ├── fast_12_detect.cpp │ ├── fast_12_score.cpp │ ├── fast_7_detect.cpp │ ├── fast_7_score.cpp │ ├── fast_8_detect.cpp │ ├── fast_8_score.cpp │ ├── fast_9.cpp │ ├── fast_9_score.cpp │ ├── faster_corner_10_sse.cpp │ ├── faster_corner_9_sse.cpp │ └── nonmax_3x3.cpp └── test │ ├── data │ └── test1.png │ └── test.cpp └── g2o ├── CMakeLists.txt ├── README.txt ├── cmake_modules ├── FindBLAS.cmake ├── FindCholmod.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 │ ├── g2o_core_api.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 │ ├── sparse_optimizer_terminate_action.cpp │ └── sparse_optimizer_terminate_action.h ├── solvers │ ├── linear_solver_cholmod.h │ ├── linear_solver_dense.h │ ├── linear_solver_eigen.h │ └── solver_cholmod.cpp ├── stuff │ ├── color_macros.h │ ├── g2o_stuff_api.h │ ├── macros.h │ ├── misc.h │ ├── os_specific.c │ ├── os_specific.h │ ├── property.cpp │ ├── property.h │ ├── sparse_helper.cpp │ ├── sparse_helper.h │ ├── string_tools.cpp │ ├── string_tools.h │ ├── timeutil.cpp │ └── timeutil.h └── types │ ├── se3_ops.h │ ├── se3_ops.hpp │ ├── se3quat.h │ ├── sim3.h │ ├── types_sba.cpp │ ├── types_sba.h │ ├── types_seven_dof_expmap.cpp │ ├── types_seven_dof_expmap.h │ ├── types_six_dof_expmap.cpp │ └── types_six_dof_expmap.h ├── lib └── libg2o.so └── license-bsd.txt /.gitignore: -------------------------------------------------------------------------------- 1 | .history -------------------------------------------------------------------------------- /Additional evaluations.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luodongting/VI-HSO/130f1858988b91996ea8a86c80fb5b69afd61f11/Additional evaluations.pdf -------------------------------------------------------------------------------- /CMakeModules/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # 3 | # CMake script for finding the Eigen library. 4 | # 5 | # http://eigen.tuxfamily.org/index.php?title=Main_Page 6 | # 7 | # Copyright (c) 2006, 2007 Montel Laurent, 8 | # Copyright (c) 2008, 2009 Gael Guennebaud, 9 | # Copyright (c) 2009 Benoit Jacob 10 | # Redistribution and use is allowed according to the terms of the 2-clause BSD 11 | # license. 12 | # 13 | # 14 | # Input variables: 15 | # 16 | # - Eigen_ROOT_DIR (optional): When specified, header files and libraries 17 | # will be searched for in `${Eigen_ROOT_DIR}/include` and 18 | # `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order 19 | # will be ignored. When unspecified, the default CMake search order is used. 20 | # This variable can be specified either as a CMake or environment variable. 21 | # If both are set, preference is given to the CMake variable. 22 | # Use this variable for finding packages installed in a nonstandard location, 23 | # or for enforcing that one of multiple package installations is picked up. 24 | # 25 | # Cache variables (not intended to be used in CMakeLists.txt files) 26 | # 27 | # - Eigen_INCLUDE_DIR: Absolute path to package headers. 28 | # 29 | # 30 | # Output variables: 31 | # 32 | # - Eigen_FOUND: Boolean that indicates if the package was found 33 | # - Eigen_INCLUDE_DIRS: Paths to the necessary header files 34 | # - Eigen_VERSION: Version of Eigen library found 35 | # - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen 36 | # 37 | # 38 | # Example usage: 39 | # 40 | # # Passing the version means Eigen_FOUND will only be TRUE if a 41 | # # version >= the provided version is found. 42 | # find_package(Eigen 3.1.2) 43 | # if(NOT Eigen_FOUND) 44 | # # Error handling 45 | # endif() 46 | # ... 47 | # add_definitions(${Eigen_DEFINITIONS}) 48 | # ... 49 | # include_directories(${Eigen_INCLUDE_DIRS} ...) 50 | # 51 | ############################################################################### 52 | 53 | find_package(PkgConfig) 54 | pkg_check_modules(PC_EIGEN eigen3) 55 | set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) 56 | 57 | 58 | find_path(EIGEN_INCLUDE_DIR Eigen/Core 59 | HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} 60 | "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" 61 | "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility 62 | PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" 63 | "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" 64 | PATH_SUFFIXES eigen3 include/eigen3 include) 65 | 66 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 67 | 68 | include(FindPackageHandleStandardArgs) 69 | find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) 70 | 71 | mark_as_advanced(EIGEN_INCLUDE_DIR) 72 | 73 | if(EIGEN_FOUND) 74 | message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") 75 | endif(EIGEN_FOUND) 76 | 77 | 78 | set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 79 | set(Eigen_FOUND ${EIGEN_FOUND}) 80 | set(Eigen_VERSION ${EIGEN_VERSION}) 81 | set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) 82 | -------------------------------------------------------------------------------- /CMakeModules/FindG2O.cmake: -------------------------------------------------------------------------------- 1 | # Find the header files 2 | 3 | FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h 4 | ${G2O_ROOT}/include 5 | $ENV{G2O_ROOT}/include 6 | $ENV{G2O_ROOT} 7 | /usr/local/include 8 | /usr/include 9 | /opt/local/include 10 | /sw/local/include 11 | /sw/include 12 | NO_DEFAULT_PATH 13 | ) 14 | 15 | # Macro to unify finding both the debug and release versions of the 16 | # libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY 17 | # macro. 18 | 19 | MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) 20 | 21 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 22 | NAMES "g2o_${MYLIBRARYNAME}_d" 23 | PATHS 24 | ${G2O_ROOT}/lib/Debug 25 | ${G2O_ROOT}/lib 26 | $ENV{G2O_ROOT}/lib/Debug 27 | $ENV{G2O_ROOT}/lib 28 | NO_DEFAULT_PATH 29 | ) 30 | 31 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 32 | NAMES "g2o_${MYLIBRARYNAME}_d" 33 | PATHS 34 | ~/Library/Frameworks 35 | /Library/Frameworks 36 | /usr/local/lib 37 | /usr/local/lib64 38 | /usr/lib 39 | /usr/lib64 40 | /opt/local/lib 41 | /sw/local/lib 42 | /sw/lib 43 | ) 44 | 45 | FIND_LIBRARY(${MYLIBRARY} 46 | NAMES "g2o_${MYLIBRARYNAME}" 47 | PATHS 48 | ${G2O_ROOT}/lib/Release 49 | ${G2O_ROOT}/lib 50 | $ENV{G2O_ROOT}/lib/Release 51 | $ENV{G2O_ROOT}/lib 52 | NO_DEFAULT_PATH 53 | ) 54 | 55 | FIND_LIBRARY(${MYLIBRARY} 56 | NAMES "g2o_${MYLIBRARYNAME}" 57 | PATHS 58 | ~/Library/Frameworks 59 | /Library/Frameworks 60 | /usr/local/lib 61 | /usr/local/lib64 62 | /usr/lib 63 | /usr/lib64 64 | /opt/local/lib 65 | /sw/local/lib 66 | /sw/lib 67 | ) 68 | 69 | IF(NOT ${MYLIBRARY}_DEBUG) 70 | IF(MYLIBRARY) 71 | SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) 72 | ENDIF(MYLIBRARY) 73 | ENDIF( NOT ${MYLIBRARY}_DEBUG) 74 | 75 | ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) 76 | 77 | # Find the core elements 78 | FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) 79 | FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) 80 | 81 | # Find the CLI library 82 | FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) 83 | 84 | # Find the pluggable solvers 85 | FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) 86 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) 87 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) 88 | FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) 89 | FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) 90 | FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) 91 | FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) 92 | FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) 93 | 94 | # Find the predefined types 95 | FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) 96 | FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) 97 | FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) 98 | FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) 99 | FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) 100 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) 101 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) 102 | 103 | # G2O solvers declared found if we found at least one solver 104 | SET(G2O_SOLVERS_FOUND "NO") 105 | IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 106 | SET(G2O_SOLVERS_FOUND "YES") 107 | ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 108 | 109 | # G2O itself declared found if we found the core libraries and at least one solver 110 | SET(G2O_FOUND "NO") 111 | IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 112 | SET(G2O_FOUND "YES") 113 | ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 114 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # VI-HSO: Hybrid Sparse Monocular Visual-Inertial Odometry 2 | 3 | This released code is the VIO part of VI-HSO based on HSO. 4 | 5 | **Author:** Wenzhe Yang, Yan Zhuang, Dongting Luo , Wei Wang ,and Hong Zhang. 6 | 7 | ## 1. Related Publications 8 | 9 | * Wenzhe Yang, Yan Zhuang, Dongting Luo , Wei Wang ,and Hong Zhang. [VI-HSO: Hybrid Sparse Monocular Visual-Inertial Odometry](https://ieeexplore.ieee.org/document/10218742). In *IEEE Robotics and Automation Letters*. 10 | 11 | ## 2. Required Dependencies 12 | 13 | ### boost 14 | 15 | c++ Librairies (thread and system are needed). Install with 16 | 17 | ``` 18 | sudo apt-get install libboost-all-dev 19 | ``` 20 | 21 | ### Eigen3 22 | 23 | Linear algebra. 24 | 25 | ``` 26 | sudo apt-get install libeigen3-dev 27 | ``` 28 | 29 | ### OpenCV4 30 | 31 | Dowload and install instructions can be found at: http://opencv.org. We tested the code on OpenCV 4.2.0. 32 | 33 | ### Pangolin 34 | 35 | Used for 3D visualization & the GUI. 36 | Dowload and install instructions can be found at: https://github.com/stevenlovegrove/Pangolin. 37 | 38 | ### Sophus, fast and g2o (Included in thirdparty folder) 39 | 40 | The three libraries will be compiled automatically using script `build.sh`. 41 | 42 | ## 2. Build and Compile 43 | 44 | We have tested the code on Ubuntu 20.04. 45 | 46 | ``` 47 | cd VI-HSO 48 | chmod +x build.sh 49 | ./build.sh 50 | ``` 51 | 52 | This will create the executable `vi_euroc` and `vi_tum` in *bin* folder. 53 | 54 | ## 3. Run code 55 | 56 | ### EuRoC Dataset example 57 | 58 | ``` 59 | cd bin 60 | ./vi_euroc ./Monocular-Inertial/EuRoC.yaml PATH_TO_SEQUENCE_FOLDER ./Monocular-Inertial/EuRoC_TimeStamps/SEQUENCE.txt FILE_NAME 61 | ``` 62 | 63 | ### TUM-VI Dataset example 64 | 65 | ``` 66 | cd bin 67 | ./vi_tum ./Monocular-Inertial/TUM_512.yaml PATH_TO_SEQUENCE_FOLDER ./Monocular-Inertial/TUM_TimeStamps/SEQUENCE.txt FILE_NAME 68 | ``` 69 | 70 | The result of trajectory will be saved in the bin folder. 71 | 72 | ## 4. License 73 | 74 | The source code is released under GPLv3 license. We are still working on improving the code. 75 | 76 | If you use VI-HSO in your academic work, please cite: 77 | 78 | @ARTICLE{10218742, 79 | author={Yang, Wenzhe and Zhuang, Yan and Luo, Dongting and Wang, Wei and Zhang, Hong}, 80 | journal={IEEE Robotics and Automation Letters}, 81 | title={VI-HSO: Hybrid Sparse Monocular Visual-Inertial Odometry}, 82 | year={2023}, 83 | volume={8}, 84 | number={10}, 85 | pages={6283-6290}, 86 | doi={10.1109/LRA.2023.3305238}} 87 | 88 | 89 | -------------------------------------------------------------------------------- /bin/Monocular-Inertial/EuRoC.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | Camera.type: "PinHole" 7 | 8 | Camera.fx: 458.654 9 | Camera.fy: 457.296 10 | Camera.cx: 367.215 11 | Camera.cy: 248.375 12 | 13 | Camera.k1: -0.28340811 14 | Camera.k2: 0.07395907 15 | Camera.p1: 0.00019359 16 | Camera.p2: 1.76187114e-05 17 | 18 | Camera.width: 752 19 | Camera.height: 480 20 | Camera.fps: 20.0 21 | 22 | # Transformation from camera to body-frame (imu) 23 | Tbc: !!opencv-matrix 24 | rows: 4 25 | cols: 4 26 | dt: f 27 | data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, 28 | 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, 29 | -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, 30 | 0.0, 0.0, 0.0, 1.0] 31 | 32 | # IMU noise 33 | IMU.NoiseGyro: 1.7e-4 34 | IMU.NoiseAcc: 2.0000e-3 35 | IMU.GyroWalk: 1.9393e-05 36 | IMU.AccWalk: 3.0000e-03 37 | g_norm: 9.8000 38 | IMU.Frequency: 200 39 | 40 | 41 | Viewer.KeyFrameSize: 0.05 42 | Viewer.KeyFrameLineWidth: 1 43 | Viewer.GraphLineWidth: 0.9 44 | Viewer.PointSize: 2 45 | Viewer.CameraSize: 0.08 46 | Viewer.CameraLineWidth: 3 47 | Viewer.ViewpointX: 0 48 | Viewer.ViewpointY: -0.7 49 | Viewer.ViewpointZ: -3.5 50 | Viewer.ViewpointF: 500 51 | 52 | priorG0: 1e2 53 | priorA0: 1e10 54 | priorG1: 1.0 55 | priorA1: 1e5 56 | priorG2: 0.0 57 | priorA2: 0.0 58 | priorGBA0: 1e2 59 | priorABA0: 1e6 60 | priorGBA1: 1.0 61 | priorABA1: 1e6 62 | priorGBA2: 1e2 63 | priorABA2: 1e6 64 | -------------------------------------------------------------------------------- /bin/Monocular-Inertial/TUM_512.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | 4 | Camera.type: "EquiDistant" 5 | 6 | Camera.fx: 190.978477 7 | Camera.fy: 190.973307 8 | Camera.cx: 254.931706 9 | Camera.cy: 256.897442 10 | 11 | Camera.k1: 0.003482389402 12 | Camera.k2: 0.000715034845 13 | Camera.k3: -0.002053236141 14 | Camera.k4: 0.000202936736 15 | 16 | Camera.width: 512 17 | Camera.height: 512 18 | Camera.fps: 20.0 19 | 20 | # Transformation from body-frame (imu) to camera 21 | Tbc: !!opencv-matrix 22 | rows: 4 23 | cols: 4 24 | dt: f 25 | data: [-0.9995250378696743, 0.0075019185074052044, -0.02989013031643309, 0.045574835649698026, 26 | 0.029615343885863205, -0.03439736061393144, -0.998969345370175, -0.071161801837997044, 27 | -0.008522328211654736, -0.9993800792498829, 0.03415885127385616, -0.044681254117144367, 28 | 0.0, 0.0, 0.0, 1.0] 29 | 30 | IMU.NoiseGyro: 0.00016 31 | IMU.NoiseAcc: 0.0028 32 | IMU.GyroWalk: 0.000022 33 | IMU.AccWalk: 0.00086 34 | IMU.Frequency: 200 35 | 36 | 37 | Viewer.KeyFrameSize: 0.05 38 | Viewer.KeyFrameLineWidth: 1 39 | Viewer.GraphLineWidth: 0.9 40 | Viewer.PointSize: 2 41 | Viewer.CameraSize: 0.08 42 | Viewer.CameraLineWidth: 3 43 | Viewer.ViewpointX: 0 44 | Viewer.ViewpointY: -0.7 45 | Viewer.ViewpointZ: -3.5 46 | Viewer.ViewpointF: 500 47 | 48 | 49 | priorG0: 1e2 50 | priorA0: 1e10 51 | priorG1: 1.0 52 | priorA1: 1e5 53 | priorG2: 0.0 54 | priorA2: 0.0 55 | priorGBA0: 1e2 56 | priorABA0: 1e6 57 | priorGBA1: 1.0 58 | priorABA1: 1e6 59 | priorGBA2: 1e2 60 | priorABA2: 1e6 61 | -------------------------------------------------------------------------------- /build.sh: -------------------------------------------------------------------------------- 1 | echo "Configuring and building thirdparty/fast ..." 2 | 3 | cd thirdparty/fast 4 | mkdir build 5 | cd build 6 | cmake .. -DCMAKE_BUILD_TYPE=Release 7 | make -j 8 | 9 | cd ../../Sophus 10 | 11 | echo "Configuring and building thirdparty/Sophus ..." 12 | 13 | mkdir build 14 | cd build 15 | cmake .. -DCMAKE_BUILD_TYPE=Release 16 | make -j 17 | 18 | cd ../../g2o 19 | 20 | echo "Configuring and building thirdparty/g2o ..." 21 | 22 | mkdir build 23 | cd build 24 | cmake .. -DCMAKE_BUILD_TYPE=Release 25 | make -j 26 | 27 | cd ../../../ 28 | 29 | echo "Configuring and building VI-HSO ..." 30 | 31 | mkdir build 32 | cd build 33 | cmake .. -DCMAKE_BUILD_TYPE=Release 34 | make -j 35 | -------------------------------------------------------------------------------- /include/Config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | Track_Name: "VI-HSO" 4 | Trace_Dir: "/temp" 5 | IMU.used: 1 6 | 7 | Initialization.nPyramidLevels: 3 8 | Initialization.MinThDisparity: 40 9 | Initialization.MinThTrackFeatures: 50 10 | Initialization.MinRANSACInliers: 40 11 | Initialization.MapScale: 1.0 12 | 13 | LucasKanade.MaxLevel: 4 14 | LucasKanade.MinLevel: 0 15 | 16 | VisiualOptimize.ThError: 2.0 17 | VisiualOptimize.nIterator: 12 18 | 19 | LocalBA.ThCornorError: 2.0 20 | LocalBA.ThEdgeLetError: 1.2 21 | LocalBA.ThRobustHuberWidth: 1.0 22 | LocalBA.nIterator: 10 23 | LocalBA.nNeighborKFs: 7 24 | 25 | Map.MinDistTwoKF: 0.12 26 | 27 | Map.MaxKFs: 2000 28 | IMU.Delay: 0.0 29 | 30 | Track.MaxFeatures: 200 31 | Track.MinFeatures: 5 32 | Track.MaxDropFeatures: 40 33 | 34 | Feature.EdgeLetCosAngle: 0.86 35 | FeatureExtractor.GridSize: 8 36 | MaxDropKFs: 13 37 | -------------------------------------------------------------------------------- /include/LocalMapping.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCALMAPPING_H 2 | #define LOCALMAPPING_H 3 | 4 | #include "vihso/frame_handler_mono.h" 5 | #include "vihso/depth_filter.h" 6 | #include "vihso/frame.h" 7 | #include "vihso/feature.h" 8 | #include "vihso/point.h" 9 | #include "vihso/map.h" 10 | #include 11 | 12 | 13 | namespace vihso 14 | { 15 | 16 | class SystemNode; 17 | class FrameHandlerMono; 18 | class DepthFilter; 19 | class Frame; 20 | class Feature; 21 | class Seed; 22 | class Point; 23 | class Map; 24 | 25 | 26 | class LocalMapping 27 | { 28 | public: 29 | LocalMapping(vihso::SystemNode* pSys, Map* pMap, bool bInertial); 30 | }; 31 | 32 | } //namespace LocalMapping 33 | 34 | #endif // LOCALMAPPING_H 35 | -------------------------------------------------------------------------------- /include/SettingParameters.h: -------------------------------------------------------------------------------- 1 | #ifndef SETPA_H_ 2 | #define SETPA_H_ 3 | 4 | #include "transformation.h" 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using namespace std; 16 | using namespace cv; 17 | 18 | extern string CAMTYPE; 19 | extern double ROW, COL; 20 | extern int FREQ; 21 | extern double fx, fy, cx, cy; 22 | extern double k1, k2, p1, p2; 23 | 24 | extern vector vRbc; 25 | extern vector vtbc; 26 | extern vector vTbc; 27 | 28 | extern double ACC_N, ACC_W; 29 | extern double GYR_N, GYR_W; 30 | extern double IMUFREQ; 31 | extern double IMUFREQ_sqrt; 32 | extern Eigen::Vector3d G; 33 | 34 | void readParameters(const string& strSettingsFile); 35 | 36 | 37 | enum SIZE_PARAMETERIZATION 38 | { 39 | SIZE_POSE = 7, 40 | SIZE_SPEEDBIAS = 9, 41 | SIZE_FEATURE = 1 42 | }; 43 | 44 | enum StateOrder 45 | { 46 | O_P = 0, 47 | O_R = 3, 48 | O_V = 6, 49 | O_BA = 9, 50 | O_BG = 12 51 | }; 52 | 53 | enum NoiseOrder 54 | { 55 | O_AN = 0, 56 | O_GN = 3, 57 | O_AW = 6, 58 | O_GW = 9 59 | }; 60 | 61 | extern double priorG0, priorA0, priorG1, priorA1, priorG2, priorA2; 62 | extern double priorGBA0, priorABA0, priorGBA1, priorABA1, priorGBA2, priorABA2; 63 | 64 | #endif //SETPA_H_ -------------------------------------------------------------------------------- /include/SystemNode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @Project: HSO_IMU 3 | * @Remark: 4 | * @Author: YWZ 5 | * @Date: 2022-05-07 14:41:08 6 | * @LastEditors: YWZ 7 | * @LastEditTime: 2023-09-01 15:31:54 8 | * @FilePath: /hso_imu/include/SystemNode.h 9 | */ 10 | #ifndef SYSTEMNODE_H 11 | #define SYSTEMNODE_H 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include "vihso/PhotomatricCalibration.h" 38 | #include "vihso/camera.h" 39 | #include "vihso/ImageReader.h" 40 | #include "vihso/depth_filter.h" 41 | 42 | #include "SettingParameters.h" 43 | #include "ImuTypes.h" 44 | #include "LocalMapping.h" 45 | 46 | namespace vihso 47 | { 48 | 49 | class SystemNode 50 | { 51 | 52 | public: 53 | SystemNode(const std::string &strSettingsFile, bool g_use_pc=false, bool g_show_pc=false, int g_start=0, int g_end=10000); 54 | ~SystemNode(); 55 | 56 | void TrackMonoInertial(cv::Mat &img, double dframestamp, vector& vIMUData); 57 | void saveResult(); 58 | void SaveKeyFrameTrajectoryEuRoC(const string &fFile, const string &kfFile, const string &sPointCloudFile=NULL); 59 | 60 | public: 61 | 62 | Map* mpMap; 63 | 64 | hso::AbstractCamera* cam_; 65 | 66 | vihso::FrameHandlerMono* vo_; 67 | 68 | vihso::DepthFilter* depth_filter_; 69 | 70 | vihso::LocalMapping* LocalMapper_; 71 | 72 | hso::Viewer* viewer_; 73 | boost::thread * viewer_thread_; 74 | 75 | public: 76 | bool mb_usePCC, mb_showPCC; 77 | int mi_IMGstart, mi_IMGend; 78 | const int G_MAX_RESOLUTION = 848*800; 79 | 80 | }; 81 | 82 | } 83 | 84 | #endif 85 | -------------------------------------------------------------------------------- /include/vihso/ImageReader.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | namespace hso { 7 | class ImageReader 8 | { 9 | public: 10 | ImageReader(std::string image_folder, cv::Size new_size); 11 | int getDir(std::string dir, std::vector &files); 12 | cv::Mat readImage(int image_index); 13 | int getNumImages() { return (int)m_files.size(); } 14 | private: 15 | cv::Size m_img_new_size; 16 | std::vector m_files; 17 | }; 18 | } 19 | -------------------------------------------------------------------------------- /include/vihso/IndexThreadReduce.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #define MAPPING_THREADS 4 7 | struct RunningStats 8 | { 9 | int n_updates; 10 | int n_failed_matches; 11 | int n_out_views; 12 | int n_seeds; 13 | int n_fail_triangulation; 14 | int n_fail_score; 15 | int n_fail_alignment; 16 | int n_fail_lsd; 17 | inline RunningStats() 18 | { 19 | setZero(); 20 | } 21 | inline void setZero() 22 | { 23 | memset(this,0,sizeof(RunningStats)); 24 | } 25 | inline void add(RunningStats* r) 26 | { 27 | int* pt = (int*)this; 28 | int* pt_r = (int*)r; 29 | for(int i=0;i(sizeof(RunningStats)/sizeof(int));i++) 30 | pt[i] += pt_r[i]; 31 | } 32 | }; 33 | namespace lsd_slam 34 | { 35 | class IndexThreadReduce 36 | { 37 | public: 38 | inline IndexThreadReduce() 39 | { 40 | nextIndex = 0; 41 | maxIndex = 0; 42 | stepSize = 1; 43 | callPerIndex = boost::bind(&IndexThreadReduce::callPerIndexDefault, this, _1, _2, _3); 44 | running = true; 45 | for(int i=0;i callPerIndex, int first, int end, RunningStats* stats, int stepSize = 0) 61 | { 62 | runningStats = stats; 63 | if(stepSize == 0) 64 | stepSize = ((end-first)+MAPPING_THREADS-1)/MAPPING_THREADS; 65 | boost::unique_lock lock(exMutex); 66 | this->callPerIndex = callPerIndex; 67 | nextIndex = first; 68 | maxIndex = end; 69 | this->stepSize = stepSize; 70 | for(int i=0;icallPerIndex = boost::bind(&IndexThreadReduce::callPerIndexDefault, this, _1, _2, _3); 85 | } 86 | private: 87 | boost::thread workerThreads[MAPPING_THREADS]; 88 | bool isDone[MAPPING_THREADS]; 89 | boost::mutex exMutex; 90 | boost::condition_variable todo_signal; 91 | boost::condition_variable done_signal; 92 | int nextIndex; 93 | int maxIndex; 94 | int stepSize; 95 | RunningStats* runningStats; 96 | bool running; 97 | boost::function callPerIndex; 98 | void callPerIndexDefault(int i, int j, RunningStats* k) 99 | { 100 | printf("ERROR: should never be called....\n"); 101 | } 102 | void workerLoop(int idx) 103 | { 104 | boost::unique_lock lock(exMutex); 105 | while(running) 106 | { 107 | int todo = 0; 108 | bool gotSomething = false; 109 | if(nextIndex < maxIndex) 110 | { 111 | todo = nextIndex; 112 | nextIndex+=stepSize; 113 | gotSomething = true; 114 | } 115 | if(gotSomething) 116 | { 117 | lock.unlock(); 118 | assert(callPerIndex != 0); 119 | RunningStats* s = new RunningStats(); 120 | callPerIndex(todo, std::min(todo+stepSize, maxIndex), s); 121 | lock.lock(); 122 | runningStats->add(s); 123 | } 124 | else 125 | { 126 | isDone[idx] = true; 127 | done_signal.notify_all(); 128 | todo_signal.wait(lock); 129 | } 130 | } 131 | } 132 | }; 133 | } 134 | -------------------------------------------------------------------------------- /include/vihso/MatrixAccumulator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "vihso/global.h" 4 | #include 5 | 6 | class Accumulator7 7 | { 8 | public: 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | 11 | Eigen::Matrix H; 12 | Eigen::Matrix b; 13 | size_t num; 14 | 15 | inline void initialize() 16 | { 17 | H.setZero(); 18 | b.setZero(); 19 | std::memset(SSEData, 0, sizeof(float)*4*28); 20 | std::memset(SSEData1k, 0, sizeof(float)*4*28); 21 | std::memset(SSEData1m, 0, sizeof(float)*4*28); 22 | num = numIn1 = numIn1k = numIn1m = 0; 23 | } 24 | 25 | inline void finish() 26 | { 27 | shiftUp(true); 28 | assert(numIn1==0); 29 | assert(numIn1k==0); 30 | 31 | int idx=0; 32 | for(int r=0;r<7;r++) 33 | for(int c=r;c<7;c++) 34 | { 35 | float d = SSEData1m[idx+0] + SSEData1m[idx+1] + SSEData1m[idx+2] + SSEData1m[idx+3]; 36 | H(r,c) = H(c,r) = d; 37 | idx+=4; 38 | } 39 | assert(idx==4*28); 40 | } 41 | 42 | inline void updateSingleWeighted( 43 | float J0, 44 | float J1, 45 | float J2, 46 | float J3, 47 | float J4, 48 | float J5, 49 | float J6, 50 | float w, 51 | int off=0) 52 | { 53 | float* pt=SSEData+off; 54 | *pt += J0*J0*w; pt+=4; J0*=w; 55 | *pt += J1*J0; pt+=4; 56 | *pt += J2*J0; pt+=4; 57 | *pt += J3*J0; pt+=4; 58 | *pt += J4*J0; pt+=4; 59 | *pt += J5*J0; pt+=4; 60 | *pt += J6*J0; pt+=4; 61 | 62 | *pt += J1*J1*w; pt+=4; J1*=w; 63 | *pt += J2*J1; pt+=4; 64 | *pt += J3*J1; pt+=4; 65 | *pt += J4*J1; pt+=4; 66 | *pt += J5*J1; pt+=4; 67 | *pt += J6*J1; pt+=4; 68 | 69 | *pt += J2*J2*w; pt+=4; J2*=w; 70 | *pt += J3*J2; pt+=4; 71 | *pt += J4*J2; pt+=4; 72 | *pt += J5*J2; pt+=4; 73 | *pt += J6*J2; pt+=4; 74 | 75 | *pt += J3*J3*w; pt+=4; J3*=w; 76 | *pt += J4*J3; pt+=4; 77 | *pt += J5*J3; pt+=4; 78 | *pt += J6*J3; pt+=4; 79 | 80 | *pt += J4*J4*w; pt+=4; J4*=w; 81 | *pt += J5*J4; pt+=4; 82 | *pt += J6*J4; pt+=4; 83 | 84 | *pt += J5*J5*w; pt+=4; J5*=w; 85 | *pt += J6*J5; pt+=4; 86 | 87 | *pt += J6*J6*w; pt+=4; 88 | 89 | num++; 90 | numIn1++; 91 | shiftUp(false); 92 | } 93 | 94 | private: 95 | EIGEN_ALIGN16 float SSEData[4*28]; 96 | EIGEN_ALIGN16 float SSEData1k[4*28]; 97 | EIGEN_ALIGN16 float SSEData1m[4*28]; 98 | float numIn1; 99 | float numIn1k; 100 | float numIn1m; 101 | 102 | void shiftUp(bool force) 103 | { 104 | if(numIn1 > 1000 || force) 105 | { 106 | for(int i=0;i<28;i++) 107 | _mm_store_ps(SSEData1k+4*i, _mm_add_ps(_mm_load_ps(SSEData+4*i),_mm_load_ps(SSEData1k+4*i))); 108 | 109 | numIn1k+=numIn1; 110 | numIn1=0; 111 | std::memset(SSEData,0, sizeof(float)*4*28); 112 | } 113 | 114 | if(numIn1k > 1000 || force) 115 | { 116 | for(int i=0;i<28;i++) 117 | _mm_store_ps(SSEData1m+4*i, _mm_add_ps(_mm_load_ps(SSEData1k+4*i),_mm_load_ps(SSEData1m+4*i))); 118 | 119 | numIn1m+=numIn1k; 120 | numIn1k=0; 121 | std::memset(SSEData1k,0, sizeof(float)*4*28); 122 | } 123 | } 124 | }; 125 | 126 | -------------------------------------------------------------------------------- /include/vihso/PhotomatricCalibration.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "vihso/global.h" 3 | #include 4 | #include 5 | #include 6 | namespace vihso { 7 | struct Feature; 8 | class Frame; 9 | 10 | class PhotomatricCalibration 11 | { 12 | public: 13 | 14 | PhotomatricCalibration(int patch_size, int width, int height); 15 | 16 | private: 17 | 18 | void startThread(); 19 | void Run(); 20 | 21 | std::thread* m_thread; 22 | 23 | }; 24 | } 25 | -------------------------------------------------------------------------------- /include/vihso/config.h: -------------------------------------------------------------------------------- 1 | #ifndef VIHSO_CONFIG_H_ 2 | #define VIHSO_CONFIG_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace vihso { 9 | 10 | using std::string; 11 | 12 | class Config 13 | { 14 | public: 15 | static Config& getInstance(); 16 | static string& traceName() { return getInstance().trace_name; } 17 | static string& traceDir() { return getInstance().trace_dir; } 18 | static int& nPyrLevels() { return getInstance().n_pyr_levels; } 19 | static bool& useImu() { return getInstance().use_imu; } 20 | static int& coreNKfs() { return getInstance().core_n_kfs; } 21 | static double& mapScale() { return getInstance().map_scale; } 22 | static int& gridSize() { return getInstance().grid_size; } 23 | static double& initMinDisparity() { return getInstance().init_min_disparity; } 24 | static int& initMinTracked() { return getInstance().init_min_tracked; } 25 | static int& initMinInliers() { return getInstance().init_min_inliers; } 26 | static int& kltMaxLevel() { return getInstance().klt_max_level; } 27 | static int& kltMinLevel() { return getInstance().klt_min_level; } 28 | static double& poseOptimThresh() { return getInstance().poseoptim_thresh; } 29 | static int& poseOptimNumIter() { return getInstance().poseoptim_num_iter; } 30 | static double& lobaCorThresh() { return getInstance().loba_CornorThresh; } 31 | static double& lobaEdgeThresh() { return getInstance().loba_EdgeLetThresh; } 32 | static double& lobaRobustHuberWidth() { return getInstance().loba_robust_huber_width; } 33 | static int& lobaNumIter() { return getInstance().loba_num_iter; } 34 | static double& SelsectKFsMinDist() { return getInstance().kfselect_mindist; } 35 | static int& maxNKfs() { return getInstance().max_n_kfs; } 36 | static double& imgImuDelay() { return getInstance().img_imu_delay; } 37 | static int& maxFts() { return getInstance().Track_Max_fts; } 38 | static void setmaxFts(const int& max_nfts) { getInstance().Track_Max_fts = max_nfts; } 39 | static int& qualityMinFts() { return getInstance().Track_Min_fts; } 40 | static int& qualityMaxFtsDrop() { return getInstance().Track_MaxDrop_fts; } 41 | static double& edgeLetCosAngle() { return getInstance().edgelet_angle; } 42 | static int& maxDropKeyframe() { return getInstance().n_max_drop_keyframe; } 43 | static void setMaxKLTLevel(const int& max_level) { getInstance().klt_max_level = max_level; } 44 | 45 | private: 46 | Config(); 47 | Config(Config const&); 48 | void operator=(Config const&); 49 | string trace_name; 50 | string trace_dir; 51 | int n_pyr_levels; 52 | bool use_imu; 53 | int core_n_kfs; 54 | double map_scale; 55 | int grid_size; 56 | double init_min_disparity; 57 | int init_min_tracked; 58 | int init_min_inliers; 59 | int klt_max_level; 60 | int klt_min_level; 61 | double poseoptim_thresh; 62 | int poseoptim_num_iter; 63 | double loba_CornorThresh; 64 | double loba_EdgeLetThresh; 65 | double loba_robust_huber_width; 66 | int loba_num_iter; 67 | double kfselect_mindist; 68 | int max_n_kfs; 69 | double img_imu_delay; 70 | int Track_Max_fts; 71 | int Track_Min_fts; 72 | int Track_MaxDrop_fts; 73 | double edgelet_angle; 74 | 75 | int n_max_drop_keyframe; 76 | }; 77 | 78 | } // namespace vihso 79 | 80 | #endif // VIHSO_CONFIG_H_ 81 | -------------------------------------------------------------------------------- /include/vihso/feature.h: -------------------------------------------------------------------------------- 1 | #ifndef VIHSO_FEATURE_H_ 2 | #define VIHSO_FEATURE_H_ 3 | 4 | #include 5 | 6 | namespace vihso { 7 | struct Feature 8 | { 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | 11 | enum FeatureType {CORNER, EDGELET, GRADIENT}; 12 | 13 | FeatureType type; 14 | Frame* frame; 15 | Vector2d px; 16 | Vector3d f; 17 | int level; 18 | Point* point; 19 | Vector2d grad; 20 | 21 | vector outputs; 22 | vector radiances; 23 | vector outputs_grad; 24 | vector rad_mean; 25 | Feature* m_prev_feature = NULL; 26 | Feature* m_next_feature = NULL; 27 | bool m_added = false; 28 | bool m_non_point = false; 29 | Matrix2d rotate_plane; 30 | bool imuBAOutlier = false; 31 | 32 | Feature(Frame* _frame, const Vector2d& _px, int _level) : 33 | type(CORNER), 34 | frame(_frame), 35 | px(_px), 36 | f(frame->cam_->cam2world(px)), 37 | level(_level), 38 | point(NULL), 39 | grad(1.0,0.0) 40 | { 41 | } 42 | 43 | Feature(Frame* _frame, const Vector2d& _px, const Vector3d& _f, int _level) : 44 | type(CORNER), 45 | frame(_frame), 46 | px(_px), 47 | f(_f), 48 | level(_level), 49 | point(NULL), 50 | grad(1.0,0.0) 51 | { 52 | } 53 | 54 | Feature(Frame* _frame, Point* _point, const Vector2d& _px, const Vector3d& _f, int _level) : 55 | type(CORNER), 56 | frame(_frame), 57 | px(_px), 58 | f(_f), 59 | level(_level), 60 | point(_point), 61 | grad(1.0,0.0) 62 | { 63 | } 64 | 65 | Feature(Frame* _frame, const Vector2d& _px, const Vector2d& _grad, int _level) : 66 | type(EDGELET), 67 | frame(_frame), 68 | px(_px), 69 | f(frame->cam_->cam2world(px)), 70 | level(_level), 71 | point(NULL), 72 | grad(_grad) 73 | { 74 | } 75 | 76 | Feature(Frame* _frame, Point* _point, const Vector2d& _px, const Vector3d& _f, const Vector2d& _grad, int _level) : 77 | type(EDGELET), 78 | frame(_frame), 79 | px(_px), 80 | f(_f), 81 | level(_level), 82 | point(_point), 83 | grad(_grad) 84 | { 85 | } 86 | 87 | Feature(Frame* _frame, const Vector2d& _px, int _level, FeatureType _type) : 88 | type(_type), 89 | frame(_frame), 90 | px(_px), 91 | f(frame->cam_->cam2world(px)), 92 | level(_level), 93 | point(NULL), 94 | grad(1.0,0.0) 95 | { 96 | } 97 | 98 | Feature(Frame* _frame, Point* _point, const Vector2d& _px, int _level, FeatureType _type) : 99 | type(_type), 100 | frame(_frame), 101 | px(_px), 102 | f(frame->cam_->cam2world(px)), 103 | level(_level), 104 | point(_point), 105 | grad(1.0,0.0) 106 | { 107 | } 108 | 109 | }; 110 | 111 | } // namespace vihso 112 | 113 | #endif // VIHSO_FEATURE_H_ 114 | -------------------------------------------------------------------------------- /include/vihso/feature_alignment.h: -------------------------------------------------------------------------------- 1 | #ifndef VIHSO_FEATURE_ALIGNMENT_H_ 2 | #define VIHSO_FEATURE_ALIGNMENT_H_ 3 | 4 | #include 5 | 6 | namespace vihso { 7 | 8 | class Point; 9 | struct Feature; 10 | 11 | namespace feature_alignment { 12 | 13 | bool align1D( 14 | const cv::Mat& cur_img, 15 | const Vector2f& dir, 16 | float* ref_patch_with_border, 17 | float* ref_patch, 18 | const int n_iter, 19 | Vector2d& cur_px_estimate, 20 | double& h_inv, 21 | float* cur_patch = NULL); 22 | 23 | bool align2D( 24 | const cv::Mat& cur_img, 25 | float* ref_patch_with_border, 26 | float* ref_patch, 27 | const int n_iter, 28 | Vector2d& cur_px_estimate, 29 | bool no_simd = true, 30 | float* cur_patch = NULL); 31 | 32 | } // namespace feature_alignment 33 | } // namespace vihso 34 | 35 | #endif // VIHSO_FEATURE_ALIGNMENT_H_ 36 | -------------------------------------------------------------------------------- /include/vihso/global.h: -------------------------------------------------------------------------------- 1 | #ifndef VIHSO_GLOBAL_H_ 2 | #define VIHSO_GLOBAL_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include "vihso/vikit/performance_monitor.h" 18 | 19 | #define RESET "\033[0m" 20 | #define BLACK "\033[30m" /* Black */ 21 | #define RED "\033[31m" /* Red */ 22 | #define GREEN "\033[32m" /* Green */ 23 | #define YELLOW "\033[33m" /* Yellow */ 24 | #define BLUE "\033[34m" /* Blue */ 25 | #define MAGENTA "\033[35m" /* Magenta */ 26 | #define CYAN "\033[36m" /* Cyan */ 27 | #define WHITE "\033[37m" /* White */ 28 | #define BOLDBLACK "\033[1m\033[30m" /* Bold Black */ 29 | #define BOLDRED "\033[1m\033[31m" /* Bold Red */ 30 | #define BOLDGREEN "\033[1m\033[32m" /* Bold Green */ 31 | #define BOLDYELLOW "\033[1m\033[33m" /* Bold Yellow */ 32 | #define BOLDBLUE "\033[1m\033[34m" /* Bold Blue */ 33 | #define BOLDMAGENTA "\033[1m\033[35m" /* Bold Magenta */ 34 | #define BOLDCYAN "\033[1m\033[36m" /* Bold Cyan */ 35 | #define BOLDWHITE "\033[1m\033[37m" /* Bold White */ 36 | 37 | #ifndef RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED 38 | #define RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED 39 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d) 40 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d) 41 | #endif 42 | 43 | #define VIHSO_INFO_STREAM(x) std::cerr<<"\033[0;0m[INFO] "< 52 | #define VIHSO_WARN_STREAM_THROTTLE(rate, x) \ 53 | do { \ 54 | static double __log_stream_throttle__last_hit__ = 0.0; \ 55 | std::chrono::time_point __log_stream_throttle__now__ = \ 56 | std::chrono::system_clock::now(); \ 57 | if (__log_stream_throttle__last_hit__ + rate <= \ 58 | std::chrono::duration_cast( \ 59 | __log_stream_throttle__now__.time_since_epoch()).count()) { \ 60 | __log_stream_throttle__last_hit__ = \ 61 | std::chrono::duration_cast( \ 62 | __log_stream_throttle__now__.time_since_epoch()).count(); \ 63 | VIHSO_WARN_STREAM(x); \ 64 | } \ 65 | } while(0) 66 | 67 | namespace vihso 68 | { 69 | using namespace Eigen; 70 | using namespace Sophus; 71 | 72 | const double EPS = 0.0000000001; 73 | const double PI = 3.14159265; 74 | 75 | #ifdef VIHSO_TRACE 76 | extern hso::PerformanceMonitor* g_permon; 77 | #define VIHSO_LOG(value) g_permon->log(std::string((#value)),(value)) 78 | #define VIHSO_LOG2(value1, value2) VIHSO_LOG(value1); VIHSO_LOG(value2) 79 | #define VIHSO_LOG3(value1, value2, value3) VIHSO_LOG2(value1, value2); VIHSO_LOG(value3) 80 | #define VIHSO_LOG4(value1, value2, value3, value4) VIHSO_LOG2(value1, value2); VIHSO_LOG2(value3, value4) 81 | #define VIHSO_START_TIMER(name) g_permon->startTimer((name)) 82 | #define VIHSO_STOP_TIMER(name) g_permon->stopTimer((name)) 83 | #else 84 | #define VIHSO_LOG(v) 85 | #define VIHSO_LOG2(v1, v2) 86 | #define VIHSO_LOG3(v1, v2, v3) 87 | #define VIHSO_LOG4(v1, v2, v3, v4) 88 | #define VIHSO_START_TIMER(name) 89 | #define VIHSO_STOP_TIMER(name) 90 | #endif 91 | 92 | 93 | class Frame; 94 | typedef boost::shared_ptr FramePtr; 95 | 96 | 97 | } 98 | 99 | #endif // VIHSO_GLOBAL_H_ 100 | -------------------------------------------------------------------------------- /include/vihso/initialization.h: -------------------------------------------------------------------------------- 1 | #ifndef VIHSO_INITIALIZATION_H 2 | #define VIHSO_INITIALIZATION_H 3 | 4 | #include 5 | 6 | namespace vihso { 7 | 8 | class FrameHandlerMono; 9 | 10 | namespace initialization 11 | { 12 | 13 | enum InitResult { FAILURE, NO_KEYFRAME, SUCCESS }; 14 | 15 | enum class InitializerType { 16 | kHomography, 17 | kTwoPoint, 18 | kFivePoint, 19 | kOneShot, 20 | kStereo, 21 | kArrayGeometric, 22 | kArrayOptimization 23 | }; 24 | 25 | class KltHomographyInit 26 | { 27 | 28 | friend class vihso::FrameHandlerMono; 29 | 30 | public: 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | KltHomographyInit(): mbref(false) 33 | {} 34 | ~KltHomographyInit() 35 | {} 36 | 37 | FramePtr frame_ref_; 38 | bool mbref; 39 | cv::Mat cvImgshow; 40 | InitResult addFirstFrame(FramePtr frame_ref); 41 | InitResult addSecondFrame(FramePtr frame_ref); 42 | void reset(); 43 | int ComputeTwoFrameKLT(FramePtr frame_ref, FramePtr frame_cur, Sophus::SE3 Tcr); 44 | 45 | protected: 46 | vector px_ref_; 47 | vector px_cur_; 48 | vector f_ref_; 49 | vector f_cur_; 50 | vector disparities_; 51 | vector inliers_; 52 | vector xyz_in_cur_; 53 | SE3 T_cur_from_ref_; 54 | InitializerType init_type_; 55 | cv::Mat img_prev_; 56 | vector px_prev_; 57 | vector ftr_type_; 58 | }; 59 | 60 | void detectFeatures(FramePtr frame, vector& px_vec, vector& f_vec, vector& ftr_type); 61 | 62 | void trackKlt( FramePtr frame_ref, 63 | FramePtr frame_cur, 64 | vector& px_ref, 65 | vector& px_cur, 66 | vector& f_ref, 67 | vector& f_cur, 68 | vector& disparities, 69 | cv::Mat& img_prev, 70 | vector& px_prev, 71 | vector& fts_type); 72 | 73 | void computeInitializeMatrix( const vector& f_ref, 74 | const vector& f_cur, 75 | double focal_length, 76 | double reprojection_threshold, 77 | vector& inliers, 78 | vector& xyz_in_cur, 79 | SE3& T_cur_from_ref); 80 | 81 | double computeP3D( const vector& vBearingRef, 82 | const vector& vBearingCur, 83 | const Matrix3d& R, 84 | const Vector3d& t, 85 | const double reproj_thresh, 86 | double error_multiplier2, 87 | vector& vP3D, 88 | vector& inliers); 89 | 90 | bool patchCheck(const cv::Mat& imgPre, const cv::Mat& imgCur, const cv::Point2f& pxPre, const cv::Point2f& pxCur); 91 | bool createPatch(const cv::Mat& img, const cv::Point2f& px, float* patch); 92 | bool checkSSD(float* patch1, float* patch2); 93 | 94 | Vector3d distancePointOnce(const Vector3d pointW, Vector3d bearingRef, Vector3d bearingCur, SE3 T_c_r); 95 | 96 | } // namespace initialization 97 | 98 | } // namespace vihso 99 | 100 | #endif // VIHSO_INITIALIZATION_H 101 | -------------------------------------------------------------------------------- /include/vihso/map.h: -------------------------------------------------------------------------------- 1 | #ifndef VIHSO_MAP_H_ 2 | #define VIHSO_MAP_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace vihso { 10 | 11 | class Point; 12 | class Feature; 13 | class Seed; 14 | 15 | class CandidatesManager 16 | { 17 | public: 18 | typedef pair CandidatePoint; 19 | typedef list LCandidatePoints; 20 | 21 | LCandidatePoints mlCandidatePoints; 22 | list> mlTemporaryPoints; 23 | list mlTrashPoints; 24 | 25 | std::mutex mMutexCandidates; 26 | 27 | public: 28 | 29 | CandidatesManager(); 30 | ~CandidatesManager(); 31 | 32 | void newCandidatePoint(Point* point, double depth_sigma2); 33 | void addPauseSeedPoint(Point* point); 34 | void addCandidatePointToFrame(FramePtr pFrame); 35 | void addCandidatePointToFrame(Frame* Frame); 36 | bool deleteCandidatePoint(Point* point); 37 | void removeFrameCandidates(FramePtr pFrame); 38 | void removeFrameCandidates(Frame* Frame); 39 | void reset(); 40 | void deleteCandidate(CandidatePoint& c); 41 | void emptyTrash(); 42 | void changeCandidatePosition(Frame* frame); 43 | void ApplyScaledRotationCandidate(const Eigen::Matrix3d& Ryw_, Eigen::Vector3d& tyw_, const double s, const int nMapChange); 44 | 45 | }; 46 | 47 | class Map : boost::noncopyable 48 | { 49 | public: 50 | 51 | list< FramePtr > keyframes_; 52 | list< Point* > mlTrashPoints; 53 | CandidatesManager mCandidatesManager; 54 | 55 | public: 56 | 57 | Map(); 58 | ~Map(); 59 | 60 | void reset(); 61 | void safeDeletePoint(Point* pt); 62 | void deletePoint(Point* pt); 63 | bool safeDeleteFrame(FramePtr pFrame); 64 | bool safeDeleteFrameID(int id); 65 | void removePtFrameRef(Frame* pFrame, Feature* ftr); 66 | void addKeyframe(FramePtr new_keyframe); 67 | void GetListCovisibleKeyFrames(FramePtr& pFrame, list< pair >& CovisibleKFs); 68 | 69 | FramePtr GetClosestKeyframe(FramePtr& pFrame); 70 | FramePtr GetFurthestKeyframe(const Vector3d& pos); 71 | 72 | bool getKeyframeById(const int id, FramePtr& pFrame); 73 | void TransformMap(const Matrix3d& R, const Vector3d& t, const double& s); 74 | void emptyTrash(); 75 | inline FramePtr lastKeyframe() { unique_lock lock(mMutexMap); return keyframes_.back(); } 76 | inline size_t size() { unique_lock lock(mMutexMap); return keyframes_.size(); } 77 | 78 | void safeDeleteTempPoint(pair& p); 79 | 80 | static int getpoint_counter_; 81 | bool mbImuInitialized; 82 | bool mbIMU_BA1; 83 | bool mbIMU_BA2; 84 | int mnMapChange; 85 | int mnMapChangeNotified; 86 | 87 | std::mutex mMutexMap; 88 | std::mutex mMutexMapUpdate; 89 | 90 | int nKFsInMap=0; 91 | 92 | void SetImuInitialized(); 93 | void SetIniertialBA1(); 94 | void SetIniertialBA2(); 95 | 96 | bool isImuInitialized(); 97 | bool isIniertialBA1(); 98 | bool isIniertialBA2(); 99 | 100 | std::vector GetAllKeyFrames(); 101 | std::list GetAllKeyFramesList(); 102 | std::vector GetAllKeyFramesFromSec(); 103 | size_t GetNumOfKF(); 104 | size_t GetNumOfMP(); 105 | int GetMaxKFid(); 106 | std::vector GetAllMapPoints(); 107 | list> GetAllCandidates(); 108 | 109 | void ApplyScaledRotation(const Eigen::Matrix3d &Rgw, const double s, const bool bScaledVel=false, const Eigen::Vector3d t=Eigen::Vector3d::Zero()); 110 | void IncreaseChangeIndex(); 111 | int GetMapChangeIndex(); 112 | void SetLastMapChange(int currentChangeId); 113 | int GetLastMapChange(); 114 | 115 | }; 116 | 117 | namespace map_debug 118 | { 119 | void mapStatistics(Map* map); 120 | void mapValidation(Map* map, int id); 121 | void frameValidation(Frame* frame, int id); 122 | void pointValidation(Point* point, int id); 123 | 124 | } // namespace map_debug 125 | 126 | 127 | } // namespace vihso 128 | 129 | #endif // VIHSO_MAP_H_ 130 | -------------------------------------------------------------------------------- /include/vihso/point.h: -------------------------------------------------------------------------------- 1 | #ifndef VIHSO_POINT_H_ 2 | #define VIHSO_POINT_H_ 3 | #include 4 | #include 5 | #include 6 | #include "vihso/vikit/math_utils.h" 7 | 8 | namespace g2o { 9 | class VertexSBAPointXYZ; } 10 | typedef g2o::VertexSBAPointXYZ g2oPoint; 11 | 12 | namespace vihso 13 | { 14 | class Feature; 15 | class VertexSBAPointID; 16 | typedef Matrix Matrix23d; 17 | class Point : boost::noncopyable 18 | { 19 | public: 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 21 | enum PointType {TYPE_DELETED, TYPE_TEMPORARY, TYPE_CANDIDATE, TYPE_UNKNOWN, TYPE_GOOD}; 22 | enum FeatureType {FEATURE_GRADIENT, FEATURE_EDGELET, FEATURE_CORNER}; 23 | static int point_counter_; 24 | int id_; 25 | Vector3d pos_; 26 | double idist_; 27 | Feature* hostFeature_; 28 | list obs_; 29 | PointType type_; 30 | FeatureType ftr_type_; 31 | int last_published_ts_; 32 | int last_structure_optim_; 33 | int last_projected_kf_id_; 34 | int n_failed_reproj_; 35 | int n_succeeded_reproj_; 36 | int seedStates_; 37 | bool isBad_; 38 | g2oPoint* v_pt_; 39 | VertexSBAPointID* vPoint_; 40 | size_t nBA_; 41 | float color_ = 128; 42 | Feature* m_last_feature = NULL; 43 | int m_last_feature_kf_id = -1; 44 | vector m_rad_est; 45 | Vector3d normal_; 46 | Matrix3d normal_information_; 47 | bool normal_set_; 48 | public: 49 | Point(const Vector3d& pos); 50 | Point(const Vector3d& pos, Feature* ftr); 51 | ~Point(); 52 | void addFrameRef(Feature* ftr); 53 | bool deleteFrameRef(Frame* frame); 54 | Feature* findFrameRef(Frame* frame); 55 | void initNormal(); 56 | bool getCloseViewObs(const Vector3d& pos, Feature*& obs); 57 | void optimize(const size_t n_iter); 58 | void optimizeLM(const size_t n_iter); 59 | void optimizeID(const size_t n_iter); 60 | inline static void jacobian_xyz2uv(const Vector3d& p_in_f,const Matrix3d& R_f_w, Matrix23d& point_jac) 61 | { 62 | const double z_inv = 1.0/p_in_f[2]; 63 | const double z_inv_sq = z_inv*z_inv; 64 | point_jac(0, 0) = z_inv; 65 | point_jac(0, 1) = 0.0; 66 | point_jac(0, 2) = -p_in_f[0] * z_inv_sq; 67 | point_jac(1, 0) = 0.0; 68 | point_jac(1, 1) = z_inv; 69 | point_jac(1, 2) = -p_in_f[1] * z_inv_sq; 70 | point_jac = - point_jac * R_f_w; 71 | } 72 | inline static void jacobian_id2uv(const Vector3d& p_in_f, const SE3& Tth, const double idH, const Vector3d& fH, Vector2d& point_jac) 73 | { 74 | Vector2d proj = hso::project2d(p_in_f); 75 | Vector3d t_th = Tth.translation(); 76 | Matrix3d R_th = Tth.rotation_matrix(); 77 | Vector3d Rf = R_th*fH; 78 | point_jac[0] = -(t_th[0] - proj[0]*t_th[2]) / (Rf[2] + t_th[2]*idH); 79 | point_jac[1] = -(t_th[1] - proj[1]*t_th[2]) / (Rf[2] + t_th[2]*idH); 80 | } 81 | int nApplyScaled; 82 | int nGetAllMapPoints; 83 | int nFullBAmaxKFID; 84 | int mnBALocalForKF; 85 | void SetWorldPos(const Vector3d &pos); 86 | void SetIdist(const double &idist); 87 | Vector3d GetWorldPos(); 88 | double GetIdist(); 89 | void UpdatePose(); 90 | void UpdatePoseScale(const double &scale); 91 | void UpdatePoseidist(const double &idist); 92 | list GetObservations(); 93 | size_t GetNumofObs(); 94 | void ClearObservations(); 95 | std::mutex mMutexPos; 96 | std::mutex mMutexFeatures; 97 | static std::mutex mGlobalMutex; 98 | }; 99 | } 100 | #endif 101 | -------------------------------------------------------------------------------- /include/vihso/pose_optimizer.h: -------------------------------------------------------------------------------- 1 | #ifndef SVO_POSE_OPTIMIZER_H_ 2 | #define SVO_POSE_OPTIMIZER_H_ 3 | #include 4 | #include 5 | namespace vihso { 6 | using namespace Eigen; 7 | using namespace Sophus; 8 | using namespace std; 9 | typedef Matrix Matrix6d; 10 | typedef Matrix Matrix26d; 11 | typedef Matrix Vector6d; 12 | namespace pose_optimizer { 13 | void optimizeGaussNewton( 14 | const double reproj_thresh, 15 | const size_t n_iter, 16 | const bool verbose, 17 | FramePtr& frame, 18 | double& estimated_scale, 19 | double& error_init, 20 | double& error_final, 21 | size_t& num_obs); 22 | void optimizeLevenbergMarquardt2nd( 23 | const double reproj_thresh, const size_t n_iter, const bool verbose, 24 | FramePtr& frame, double& estimated_scale, double& error_init, double& error_final, 25 | size_t& num_obs); 26 | void optimizeLevenbergMarquardt3rd( 27 | const double reproj_thresh, const size_t n_iter, const bool verbose, 28 | FramePtr& frame, double& estimated_scale, double& error_init, double& error_final, 29 | size_t& num_obs); 30 | void optimizeLevenbergMarquardtMagnitude( 31 | const double reproj_thresh, const size_t n_iter, const bool verbose, 32 | FramePtr& frame, double& estimated_scale, double& error_init, double& error_final, 33 | size_t& num_obs); 34 | static int residual_buffer[10000]={0}; 35 | } 36 | } 37 | #endif 38 | -------------------------------------------------------------------------------- /include/vihso/reprojector.h: -------------------------------------------------------------------------------- 1 | #ifndef VIHSO_REPROJECTION_H_ 2 | #define VIHSO_REPROJECTION_H_ 3 | #include 4 | #include 5 | #include "vihso/camera.h" 6 | namespace vihso { 7 | class Map; 8 | class Point; 9 | class DepthFilter; 10 | struct Seed; 11 | class Reprojector 12 | { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | struct Options 16 | { 17 | size_t max_n_kfs; 18 | bool find_match_direct; 19 | bool reproject_unconverged_seeds; 20 | float reproject_seed_thresh; 21 | Options() 22 | : max_n_kfs(10), 23 | find_match_direct(true), 24 | reproject_unconverged_seeds(true), 25 | reproject_seed_thresh(86) 26 | {} 27 | } options_; 28 | Reprojector(hso::AbstractCamera* cam, Map* pMap); 29 | ~Reprojector(); 30 | int caculateGridSize(const int wight, const int height, const int N); 31 | void reprojectMap(FramePtr frame, std::vector< std::pair >& overlap_kfs); 32 | void reprojectNeighbors(FramePtr curFrame, FramePtr refKF, list> &lNeighborKFs); 33 | private: 34 | struct Candidate 35 | { 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | Point* pt; 38 | Vector2d px; 39 | Candidate(Point* pt, Vector2d& px) : pt(pt), px(px) 40 | {} 41 | }; 42 | typedef std::list Cell; 43 | typedef std::vector CandidateGrid; 44 | struct SeedCandidate 45 | { 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | Seed& seed; 48 | Vector2d px; 49 | list< Seed, aligned_allocator >::iterator index; 50 | SeedCandidate(Seed& _seed, Vector2d _uv, 51 | list< Seed, aligned_allocator >::iterator _i):seed(_seed), px(_uv), index(_i) 52 | {} 53 | }; 54 | typedef std::list Sell; 55 | typedef std::vector SeedGrid; 56 | struct Grid 57 | { 58 | CandidateGrid cells; 59 | SeedGrid seeds; 60 | vector cell_order; 61 | int cell_size; 62 | int grid_n_cols; 63 | int grid_n_rows; 64 | int cell_size_w; 65 | int cell_size_h; 66 | }; 67 | static bool pointQualityComparator(Candidate& lhs, Candidate& rhs); 68 | static bool seedComparator(SeedCandidate& lhs, SeedCandidate& rhs); 69 | void initializeGrid(hso::AbstractCamera* cam); 70 | void resetGrid(); 71 | bool reprojectCell(Cell& cell, FramePtr frame, bool is_2nd = false, bool is_3rd = false); 72 | bool reprojectorSeeds(Sell& sell, FramePtr frame); 73 | bool reprojectPoint(FramePtr frame, Point* point, vector< pair >& cells); 74 | bool reprojectorSeed(FramePtr frame, Seed& seed, list< Seed, aligned_allocator >::iterator index); 75 | void reprojectCellAll(vector< pair >& cell, FramePtr frame); 76 | public: 77 | size_t n_matches_; 78 | size_t n_trials_; 79 | size_t n_seeds_; 80 | size_t n_filters_; 81 | DepthFilter* depth_filter_; 82 | private: 83 | Grid grid_; 84 | Matcher matcher_; 85 | Map* mpMap; 86 | size_t sum_seed_; 87 | size_t sum_temp_; 88 | size_t nFeatures_; 89 | }; 90 | } 91 | #endif 92 | -------------------------------------------------------------------------------- /include/vihso/viewer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | namespace vihso 9 | { 10 | class Frame; 11 | class Map; 12 | class FrameHandlerMono; 13 | } 14 | namespace hso { 15 | class Viewer 16 | { 17 | public: 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | Viewer(vihso::FrameHandlerMono* vo, vihso::Map* pMap); 20 | void run(); 21 | bool CheckFinish(); 22 | void DrawKeyFrames(const bool bDrawTrajactor, const bool bDrawKF); 23 | void DrawMapRegionPoints(); 24 | void DrawMapSeeds(); 25 | void DrawConnections(const bool bDrawGraph, const bool bDrawSpanningTree, const bool bDrawConstraints); 26 | cv::Mat DrawFrame(); 27 | void DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText); 28 | void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw); 29 | void DrawCurrentCamera(pangolin::OpenGlMatrix &Twc); 30 | void SuphusToOpenGLMatrix(Sophus::SE3 &T, pangolin::OpenGlMatrix &M); 31 | private: 32 | vihso::FrameHandlerMono* _vo; 33 | vihso::Map* mpMap; 34 | std::mutex mMutexCurrentPose; 35 | std::mutex mMutexIMG; 36 | std::vector< Sophus::SE3 > _pos; 37 | std::vector< Sophus::SE3 > _KFpos; 38 | std::list _qProcessTimes; 39 | Sophus::SE3 _CurrentPoseTwc ; 40 | int _drawedframeID=0; 41 | void SetFinish(); 42 | bool mbFinished; 43 | std::mutex mMutexFinish; 44 | bool mbStopTrack; 45 | float mKeyFrameSize; 46 | float mKeyFrameLineWidth; 47 | float mGraphLineWidth; 48 | float mPointSize; 49 | float mCameraSize; 50 | float mCameraLineWidth; 51 | float mViewpointX, mViewpointY, mViewpointZ, mViewpointF; 52 | }; 53 | } -------------------------------------------------------------------------------- /include/vihso/vikit/aligned_mem.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include // memset 4 | #include // memset 5 | #include 6 | #include 7 | 8 | namespace hso { 9 | namespace aligned_mem { 10 | 11 | /// Check if the pointer is aligned to the specified byte granularity 12 | inline bool 13 | is_aligned8(const void* ptr) 14 | { 15 | return ((reinterpret_cast(ptr)) & 0x7) == 0; 16 | } 17 | 18 | inline bool 19 | is_aligned16(const void* ptr) 20 | { 21 | return ((reinterpret_cast(ptr)) & 0xF) == 0; 22 | } 23 | 24 | template struct placement_delete 25 | { 26 | enum { Size = (1<= Size) { 40 | placement_delete::free(buf+Size,M-Size); 41 | placement_delete::destruct(buf); 42 | } else { 43 | placement_delete::free(buf, M); 44 | } 45 | } 46 | }; 47 | 48 | template struct placement_delete 49 | { 50 | static inline void free(T*, size_t ) {} 51 | }; 52 | 53 | inline void * aligned_alloc(size_t count, size_t alignment){ 54 | void * mem = NULL; 55 | assert(posix_memalign(&mem, alignment, count) == 0); 56 | return mem; 57 | } 58 | 59 | inline void aligned_free(void * memory) { 60 | free(memory); 61 | } 62 | 63 | template 64 | inline T * aligned_alloc(size_t count, size_t alignment){ 65 | void * data = aligned_alloc(sizeof(T)* count, alignment); 66 | return new (data) T[count]; 67 | } 68 | 69 | template 70 | inline void aligned_free(T * memory, size_t count){ 71 | placement_delete::free(memory, count); 72 | aligned_free(memory); 73 | } 74 | 75 | template inline void memfill(T* data, int n, const T val) 76 | { 77 | T* de = data + n; 78 | for(;data < de; data++) 79 | *data=val; 80 | } 81 | 82 | template<> inline void memfill(unsigned char* data, int n, const unsigned char val) 83 | { 84 | memset(data, val, n); 85 | } 86 | 87 | template<> inline void memfill(signed char* data, int n, const signed char val) 88 | { 89 | memset(data, val, n); 90 | } 91 | 92 | template<> inline void memfill(char* data, int n, const char val) 93 | { 94 | memset(data, val, n); 95 | } 96 | 97 | template 98 | struct AlignedMem { 99 | T* mem; 100 | size_t count; 101 | AlignedMem(size_t c) : count(c) { 102 | mem = aligned_alloc(count, N); 103 | } 104 | ~AlignedMem() { 105 | aligned_free(mem, count); 106 | } 107 | T* data() { return mem; } 108 | const T* data() const { return mem; } 109 | }; 110 | 111 | } // namespace aligned_mem 112 | } // namespace vikit 113 | -------------------------------------------------------------------------------- /include/vihso/vikit/homography.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include "vihso/vikit/math_utils.h" 6 | namespace hso { 7 | using namespace Eigen; 8 | using namespace std; 9 | struct HomographyDecomposition 10 | { 11 | Vector3d t; 12 | Matrix3d R; 13 | double d; 14 | Vector3d n; 15 | Sophus::SE3 T; 16 | int score; 17 | }; 18 | class Homography 19 | { 20 | public: 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | Homography (const vector >& _fts1, 23 | const vector >& _fts2, 24 | double _error_multiplier2, 25 | double _thresh_in_px); 26 | void 27 | calcFromPlaneParams (const Vector3d & normal, 28 | const Vector3d & point_on_plane); 29 | void 30 | calcFromMatches (); 31 | size_t 32 | computeMatchesInliers (); 33 | bool 34 | computeSE3fromMatches (); 35 | bool 36 | decompose (); 37 | void 38 | findBestDecomposition (); 39 | double thresh; 40 | double error_multiplier2; 41 | const vector >& fts_c1; 42 | const vector >& fts_c2; 43 | vector inliers; 44 | SE3 T_c2_from_c1; 45 | Matrix3d H_c2_from_c1; 46 | vector decompositions; 47 | }; 48 | } /* end namespace vk */ 49 | -------------------------------------------------------------------------------- /include/vihso/vikit/math_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace hso { 8 | 9 | using namespace Eigen; 10 | using namespace std; 11 | using namespace Sophus; 12 | 13 | Vector3d triangulateFeatureNonLin(const Matrix3d& R, 14 | const Vector3d& t, 15 | const Vector3d& feature1, 16 | const Vector3d& feature2); 17 | 18 | 19 | 20 | 21 | bool depthFromTriangulationExact(const Matrix3d& R_r_c, 22 | const Vector3d& t_r_c, 23 | const Vector3d& f_r, 24 | const Vector3d& f_c, 25 | double& depth_in_r, 26 | double& depth_in_c); 27 | 28 | double reprojError(const Vector3d& f1, const Vector3d& f2, double error_multiplier2); 29 | 30 | double computeInliers(const vector& features1, 31 | const vector& features2, 32 | const Matrix3d& R, 33 | const Vector3d& t, 34 | const double reproj_thresh, 35 | double error_multiplier2, 36 | vector& xyz_vec, 37 | vector& inliers, 38 | vector& outliers); 39 | 40 | void computeInliersOneView(const vector & feature_sphere_vec, 41 | const vector & xyz_vec, 42 | const Matrix3d &R, 43 | const Vector3d &t, 44 | const double reproj_thresh, 45 | const double error_multiplier2, 46 | vector& inliers, 47 | vector& outliers); 48 | 49 | 50 | Vector3d dcm2rpy(const Matrix3d &R); 51 | 52 | 53 | Matrix3d rpy2dcm(const Vector3d &rpy); 54 | 55 | 56 | Quaterniond angax2quat(const Vector3d& n, const double& angle); 57 | 58 | 59 | Matrix3d angax2dcm(const Vector3d& n, const double& angle); 60 | 61 | double sampsonusError(const Vector2d &v2Dash, const Matrix3d& m3Essential, const Vector2d& v2); 62 | 63 | inline Matrix3d sqew(const Vector3d& v) 64 | { 65 | Matrix3d v_sqew; 66 | v_sqew << 0, -v[2], v[1], 67 | v[2], 0, -v[0], 68 | -v[1], v[0], 0; 69 | return v_sqew; 70 | } 71 | 72 | inline double norm_max(const Eigen::VectorXd & v) 73 | { 74 | double max = -1; 75 | for (int i=0; imax)max = abs; 79 | } 80 | return max; 81 | } 82 | 83 | 84 | inline Vector2d project2d(const Vector3d& v) 85 | { 86 | return v.head<2>()/v[2]; 87 | } 88 | 89 | inline Vector3d unproject2d(const Vector2d& v) 90 | { 91 | return Vector3d(v[0], v[1], 1.0); 92 | } 93 | 94 | inline Vector3d project3d(const Vector4d& v) 95 | { 96 | return v.head<3>()/v[3]; 97 | } 98 | 99 | inline Vector4d unproject3d(const Vector3d& v) 100 | { 101 | return Vector4d(v[0], v[1], v[2], 1.0); 102 | } 103 | 104 | template 105 | T getMedian(vector& data_vec) 106 | { 107 | assert(!data_vec.empty()); 108 | typename vector::iterator it = data_vec.begin()+floor(data_vec.size()/2); 109 | nth_element(data_vec.begin(), it, data_vec.end()); 110 | return *it; 111 | } 112 | 113 | inline double pyrFromZero_d(double x_0, int level) 114 | { 115 | return x_0/(1< & frame_jac) 126 | { 127 | const double x = xyz[0]; 128 | const double y = xyz[1]; 129 | const double z = xyz[2]; 130 | const double z_2 = z*z; 131 | 132 | frame_jac(0,0) = -1./z *focal_length; 133 | frame_jac(0,1) = 0; 134 | frame_jac(0,2) = x/z_2 *focal_length; 135 | frame_jac(0,3) = x*y/z_2 * focal_length; 136 | frame_jac(0,4) = -(1+(x*x/z_2)) *focal_length; 137 | frame_jac(0,5) = y/z *focal_length; 138 | 139 | frame_jac(1,0) = 0; 140 | frame_jac(1,1) = -1./z *focal_length; 141 | frame_jac(1,2) = y/z_2 *focal_length; 142 | frame_jac(1,3) = (1+y*y/z_2) *focal_length; 143 | frame_jac(1,4) = -x*y/z_2 *focal_length; 144 | frame_jac(1,5) = -x/z *focal_length; 145 | } 146 | 147 | } -------------------------------------------------------------------------------- /include/vihso/vikit/performance_monitor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "vihso/vikit/timer.h" 7 | namespace hso 8 | { 9 | struct LogItem 10 | { 11 | double data; 12 | bool set; 13 | }; 14 | class PerformanceMonitor 15 | { 16 | public: 17 | PerformanceMonitor(); 18 | ~PerformanceMonitor(); 19 | void init(const std::string& trace_name, const std::string& trace_dir); 20 | void addTimer(const std::string& name); 21 | void addLog(const std::string& name); 22 | void writeToFile(); 23 | void startTimer(const std::string& name); 24 | void stopTimer(const std::string& name); 25 | double getTime(const std::string& name) const; 26 | void log(const std::string& name, double data); 27 | private: 28 | std::map timers_; 29 | std::map logs_; 30 | std::string trace_name_; 31 | std::string trace_dir_; 32 | std::ofstream ofs_; 33 | void trace(); 34 | void traceHeader(); 35 | }; 36 | } 37 | -------------------------------------------------------------------------------- /include/vihso/vikit/ringbuffer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | namespace hso 6 | { 7 | template 8 | class RingBuffer 9 | { 10 | public: 11 | RingBuffer (int size); 12 | void 13 | push_back (const T & elem); 14 | bool 15 | empty () const; 16 | T 17 | get (int i); 18 | T 19 | getSum () const; 20 | T 21 | getMean () const; 22 | int size() 23 | { 24 | return num_elem_; 25 | } 26 | private: 27 | std::vector arr_; 28 | int begin_; 29 | int end_; 30 | int num_elem_; 31 | int arr_size_; 32 | }; 33 | template 34 | RingBuffer 35 | ::RingBuffer(int size) : 36 | arr_(size), 37 | begin_(0), 38 | end_(-1), 39 | num_elem_(0), 40 | arr_size_(size) 41 | {} 42 | template 43 | bool RingBuffer 44 | ::empty() const 45 | { 46 | return arr_.empty(); 47 | } 48 | template 49 | void RingBuffer 50 | ::push_back(const T & elem) 51 | { 52 | if (num_elem_ 65 | T RingBuffer 66 | ::get(int i) 67 | { 68 | assert(i 72 | T RingBuffer 73 | ::getSum() const 74 | { 75 | T sum=0; 76 | for(int i=0; i 81 | T RingBuffer 82 | ::getMean() const 83 | { 84 | if(num_elem_ == 0) 85 | return 0; 86 | return getSum()/num_elem_; 87 | } 88 | } 89 | -------------------------------------------------------------------------------- /include/vihso/vikit/robust_cost.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | namespace hso { 6 | namespace robust_cost { 7 | class ScaleEstimator 8 | { 9 | public: 10 | virtual ~ScaleEstimator() {}; 11 | virtual float compute(std::vector& errors) const = 0; 12 | }; 13 | typedef std::shared_ptr ScaleEstimatorPtr; 14 | class UnitScaleEstimator : public ScaleEstimator 15 | { 16 | public: 17 | UnitScaleEstimator() {} 18 | virtual ~UnitScaleEstimator() {} 19 | virtual float compute(std::vector& errors) const { return 1.0f; }; 20 | }; 21 | class TDistributionScaleEstimator : public ScaleEstimator 22 | { 23 | public: 24 | TDistributionScaleEstimator(const float dof = DEFAULT_DOF); 25 | virtual ~TDistributionScaleEstimator() {}; 26 | virtual float compute(std::vector& errors) const; 27 | static const float DEFAULT_DOF; 28 | static const float INITIAL_SIGMA; 29 | protected: 30 | float dof_; 31 | float initial_sigma_; 32 | }; 33 | class MADScaleEstimator : public ScaleEstimator 34 | { 35 | public: 36 | MADScaleEstimator() {}; 37 | virtual ~MADScaleEstimator() {}; 38 | virtual float compute(std::vector& errors) const; 39 | private: 40 | static const float NORMALIZER;; 41 | }; 42 | class NormalDistributionScaleEstimator : public ScaleEstimator 43 | { 44 | public: 45 | NormalDistributionScaleEstimator() {}; 46 | virtual ~NormalDistributionScaleEstimator() {}; 47 | virtual float compute(std::vector& errors) const; 48 | private: 49 | }; 50 | class WeightFunction 51 | { 52 | public: 53 | virtual ~WeightFunction() {}; 54 | virtual float value(const float& x) const = 0; 55 | virtual void configure(const float& param) {}; 56 | }; 57 | typedef std::shared_ptr WeightFunctionPtr; 58 | class UnitWeightFunction : public WeightFunction 59 | { 60 | public: 61 | UnitWeightFunction() {}; 62 | virtual ~UnitWeightFunction() {}; 63 | virtual float value(const float& x) const { return 1.0f; }; 64 | }; 65 | class TukeyWeightFunction : public WeightFunction 66 | { 67 | public: 68 | TukeyWeightFunction(const float b = DEFAULT_B); 69 | virtual ~TukeyWeightFunction() {}; 70 | virtual float value(const float& x) const; 71 | virtual void configure(const float& param); 72 | static const float DEFAULT_B; 73 | private: 74 | float b_square; 75 | }; 76 | class TDistributionWeightFunction : public WeightFunction 77 | { 78 | public: 79 | TDistributionWeightFunction(const float dof = DEFAULT_DOF); 80 | virtual ~TDistributionWeightFunction() {}; 81 | virtual float value(const float& x) const; 82 | virtual void configure(const float& param); 83 | static const float DEFAULT_DOF; 84 | private: 85 | float dof_; 86 | float normalizer_; 87 | }; 88 | class HuberWeightFunction : public WeightFunction 89 | { 90 | public: 91 | HuberWeightFunction(const float k = DEFAULT_K); 92 | virtual ~HuberWeightFunction() {}; 93 | virtual float value(const float& x) const; 94 | virtual void configure(const float& param); 95 | static const float DEFAULT_K; 96 | private: 97 | float k; 98 | }; 99 | } 100 | } 101 | -------------------------------------------------------------------------------- /include/vihso/vikit/timer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | namespace hso 6 | { 7 | class Timer 8 | { 9 | private: 10 | timeval start_time_; 11 | double time_; 12 | double accumulated_; 13 | public: 14 | Timer() : time_(0), accumulated_(0) 15 | { 16 | start(); 17 | } 18 | ~Timer() 19 | {} 20 | inline void start() 21 | { 22 | accumulated_ = 0.0; 23 | gettimeofday(&start_time_, NULL); 24 | } 25 | inline void resume() 26 | { 27 | gettimeofday(&start_time_, NULL); 28 | } 29 | inline double stop() 30 | { 31 | timeval end_time; 32 | gettimeofday(&end_time, NULL); 33 | long seconds = end_time.tv_sec - start_time_.tv_sec; 34 | long useconds = end_time.tv_usec - start_time_.tv_usec; 35 | time_ = ((seconds) + useconds*0.000001) + accumulated_; 36 | accumulated_ = time_; 37 | return time_; 38 | } 39 | inline double getTime() const 40 | { 41 | return time_; 42 | } 43 | inline void reset() 44 | { 45 | time_ = 0; 46 | accumulated_ = 0; 47 | } 48 | static double getCurrentTime() 49 | { 50 | timeval time_now; 51 | gettimeofday(&time_now, NULL); 52 | return time_now.tv_sec + time_now.tv_usec*0.000001; 53 | } 54 | static double getCurrentSecond() 55 | { 56 | timeval time_now; 57 | gettimeofday(&time_now, NULL); 58 | return time_now.tv_sec; 59 | } 60 | }; 61 | } // end namespace vk 62 | -------------------------------------------------------------------------------- /include/vihso/vikit/vision.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "vihso/vikit/aligned_mem.h" 3 | #include 4 | #include 5 | namespace hso { 6 | inline float interpolateMat_32f(const cv::Mat& mat, float u, float v) 7 | { 8 | assert(mat.type()==CV_32F); 9 | float x = floor(u); 10 | float y = floor(v); 11 | float subpix_x = u-x; 12 | float subpix_y = v-y; 13 | float wx0 = 1.0-subpix_x; 14 | float wx1 = subpix_x; 15 | float wy0 = 1.0-subpix_y; 16 | float wy1 = subpix_y; 17 | float val00 = mat.at(y,x); 18 | float val10 = mat.at(y,x+1); 19 | float val01 = mat.at(y+1,x); 20 | float val11 = mat.at(y+1,x+1); 21 | return (wx0*wy0)*val00 + (wx1*wy0)*val10 + (wx0*wy1)*val01 + (wx1*wy1)*val11; 22 | } 23 | inline float interpolateMat_8u(const cv::Mat& mat, float u, float v) 24 | { 25 | assert(mat.type()==CV_8U); 26 | int x = floor(u); 27 | int y = floor(v); 28 | float subpix_x = u-x; 29 | float subpix_y = v-y; 30 | float w00 = (1.0f-subpix_x)*(1.0f-subpix_y); 31 | float w01 = (1.0f-subpix_x)*subpix_y; 32 | float w10 = subpix_x*(1.0f-subpix_y); 33 | float w11 = 1.0f - w00 - w01 - w10; 34 | const int stride = mat.step.p[0]; 35 | unsigned char* ptr = mat.data + y*stride + x; 36 | return w00*ptr[0] + w01*ptr[stride] + w10*ptr[1] + w11*ptr[stride+1]; 37 | } 38 | void halfSample(const cv::Mat& in, cv::Mat& out); 39 | float shiTomasiScore(const cv::Mat& img, int u, int v); 40 | void calcSharrDeriv(const cv::Mat& src, cv::Mat& dst); 41 | #ifdef __SSE2__ 42 | void convertRawDepthImageSse_16u_to_32f(cv::Mat& depth_16u, cv::Mat& depth_32f, float scale); 43 | #endif 44 | } -------------------------------------------------------------------------------- /lib/libhso.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luodongting/VI-HSO/130f1858988b91996ea8a86c80fb5b69afd61f11/lib/libhso.so -------------------------------------------------------------------------------- /src/ImageReader.cpp: -------------------------------------------------------------------------------- 1 | #include "vihso/ImageReader.h" 2 | #include 3 | #include 4 | namespace hso { 5 | ImageReader::ImageReader(std::string image_folder, cv::Size new_size) 6 | { 7 | getDir(image_folder, m_files); 8 | printf("ImageReader: got %d files in %s!\n", (int)m_files.size(), image_folder.c_str()); 9 | m_img_new_size = new_size; 10 | } 11 | cv::Mat ImageReader::readImage(int image_index) 12 | { 13 | cv::Mat imgC2 = cv::imread(m_files.at(image_index)); 14 | cv::Mat image; 15 | cv::cvtColor(imgC2, image, CV_BGR2GRAY); 16 | if(!image.data) 17 | { 18 | std::cout << "ERROR READING IMAGE " << m_files.at(image_index) << std::endl; 19 | return cv::Mat(); 20 | } 21 | cv::resize(image, image, m_img_new_size); 22 | return image; 23 | } 24 | int ImageReader::getDir(std::string dir, std::vector &files) 25 | { 26 | DIR *dp; 27 | struct dirent *dirp; 28 | if((dp = opendir(dir.c_str())) == NULL) 29 | { 30 | return -1; 31 | } 32 | while ((dirp = readdir(dp)) != NULL) 33 | { 34 | std::string name = std::string(dirp->d_name); 35 | if(name != "." && name != "..") 36 | files.push_back(name); 37 | } 38 | closedir(dp); 39 | std::sort(files.begin(), files.end()); 40 | if(dir.at(dir.length() - 1) != '/') 41 | dir = dir+"/"; 42 | for(unsigned int i = 0; i < files.size(); i++) 43 | { 44 | if(files[i].at(0) != '/') 45 | files[i] = dir + files[i]; 46 | } 47 | return (int)files.size(); 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /src/LocalMapping.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "LocalMapping.h" 4 | #include 5 | 6 | namespace vihso 7 | { 8 | 9 | LocalMapping::LocalMapping(vihso::SystemNode* pSys, Map* pMap, bool bInertial) 10 | {} 11 | 12 | } //namespace vihso 13 | -------------------------------------------------------------------------------- /src/PhotomatricCalibration.cpp: -------------------------------------------------------------------------------- 1 | #include "vihso/PhotomatricCalibration.h" 2 | #include "vihso/frame.h" 3 | #include "vihso/feature.h" 4 | #include "vihso/config.h" 5 | #include "vihso/point.h" 6 | #include "vihso/vikit/math_utils.h" 7 | namespace vihso{ 8 | PhotomatricCalibration::PhotomatricCalibration(int patch_size, int width, int height) 9 | { 10 | startThread(); 11 | } 12 | void PhotomatricCalibration::startThread() 13 | { 14 | m_thread = new thread(&vihso::PhotomatricCalibration::Run, this); 15 | } 16 | void PhotomatricCalibration::Run() 17 | { 18 | 19 | } 20 | 21 | } //namespace vihso -------------------------------------------------------------------------------- /src/config.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace vihso { 9 | 10 | Config::Config() 11 | { 12 | cv::FileStorage fsConfig("../include/Config.yaml", cv::FileStorage::READ); //Config.yaml 13 | if(!fsConfig.isOpened()) 14 | { 15 | std::cerr << "Failed to open settings file at: " << "/include/Config.yaml" << std::endl; 16 | exit(-1); 17 | } 18 | 19 | trace_name = (string)fsConfig["Track_Name"]; 20 | trace_dir = (string)fsConfig["Trace_Dir"]; 21 | 22 | int USE_IMU = fsConfig["IMU.used"]; 23 | if(USE_IMU==1) 24 | use_imu = true; 25 | else 26 | use_imu = false; 27 | 28 | n_pyr_levels = fsConfig["Initialization.nPyramidLevels"]; 29 | map_scale = fsConfig["Initialization.MapScale"]; 30 | init_min_disparity = fsConfig["Initialization.MinThDisparity"]; 31 | init_min_tracked = fsConfig["Initialization.MinThTrackFeatures"]; 32 | init_min_inliers = fsConfig["Initialization.MinRANSACInliers"]; 33 | 34 | grid_size = fsConfig["FeatureExtractor.GridSize"]; 35 | 36 | klt_max_level = fsConfig["LucasKanade.MaxLevel"]; 37 | klt_min_level = fsConfig["LucasKanade.MinLevel"]; 38 | 39 | poseoptim_thresh = fsConfig["VisiualOptimize.ThError"]; 40 | poseoptim_num_iter = fsConfig["VisiualOptimize.nIterator"]; 41 | 42 | core_n_kfs = fsConfig["LocalBA.nNeighborKFs"]; 43 | loba_CornorThresh = fsConfig["LocalBA.ThCornorError"]; 44 | loba_EdgeLetThresh = fsConfig["LocalBA.ThEdgeLetError"]; 45 | loba_robust_huber_width = fsConfig["ThRobustHuberWidth"]; 46 | loba_num_iter = fsConfig["LocalBA.nIterator"]; 47 | 48 | kfselect_mindist = fsConfig["Map.MinDistTwoKF"]; 49 | 50 | max_n_kfs = fsConfig["Map.MaxKFs"]; 51 | img_imu_delay = fsConfig["IMU.Delay"]; 52 | 53 | Track_Max_fts = fsConfig["Track.MaxFeatures"]; 54 | Track_Min_fts = fsConfig["Track.MinFeatures"]; 55 | Track_MaxDrop_fts = fsConfig["Track.MaxDropFeatures"]; 56 | 57 | edgelet_angle = fsConfig["Feature.EdgeLetCosAngle"]; 58 | n_max_drop_keyframe = fsConfig["MaxDropKFs"]; 59 | } 60 | 61 | Config& Config::getInstance() 62 | { 63 | static Config instance; 64 | return instance; 65 | } 66 | 67 | } // namespace vihso 68 | 69 | -------------------------------------------------------------------------------- /src/vikit/robust_cost.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "vihso/vikit/robust_cost.h" 5 | namespace hso { 6 | namespace robust_cost { 7 | const float TDistributionScaleEstimator::INITIAL_SIGMA = 5.0f; 8 | const float TDistributionScaleEstimator::DEFAULT_DOF = 5.0f; 9 | TDistributionScaleEstimator::TDistributionScaleEstimator(const float dof) : dof_(dof), initial_sigma_(INITIAL_SIGMA) 10 | {} 11 | float TDistributionScaleEstimator::compute(std::vector& errors) const 12 | { 13 | float initial_lamda = 1.0f / (initial_sigma_ * initial_sigma_); 14 | int num = 0; 15 | float lambda = initial_lamda; 16 | int iterations = 0; 17 | do 18 | { 19 | ++iterations; 20 | initial_lamda = lambda; 21 | num = 0; 22 | lambda = 0.0f; 23 | for(std::vector::iterator it=errors.begin(); it!=errors.end(); ++it) 24 | { 25 | if(std::isfinite(*it)) 26 | { 27 | ++num; 28 | const float error2 = (*it)*(*it); 29 | lambda += error2 * ( (dof_ + 1.0f) / (dof_ + initial_lamda * error2) ); 30 | } 31 | } 32 | lambda = float(num) / lambda; 33 | } while(std::abs(lambda - initial_lamda) > 1e-3); 34 | return std::sqrt(1.0f / lambda); 35 | } 36 | const float MADScaleEstimator::NORMALIZER = 1.4826f; 37 | float MADScaleEstimator::compute(std::vector& errors) const 38 | { 39 | std::vector::iterator it = errors.begin()+floor(errors.size()/2); 40 | nth_element(errors.begin(), it, errors.end()); 41 | return NORMALIZER * (*it); 42 | } 43 | float NormalDistributionScaleEstimator::compute(std::vector& errors) const 44 | { 45 | const float mean = std::accumulate(errors.begin(), errors.end(), 0)/errors.size(); 46 | float var = 0.0; 47 | std::for_each(errors.begin(), errors.end(), [&](const float d) { 48 | var += (d - mean) * (d - mean); 49 | }); 50 | return std::sqrt(var); 51 | } 52 | const float TukeyWeightFunction::DEFAULT_B = 4.6851f; 53 | TukeyWeightFunction::TukeyWeightFunction(const float b) 54 | { 55 | configure(b); 56 | } 57 | float TukeyWeightFunction::value(const float& x) const 58 | { 59 | const float x_square = x * x; 60 | if(x_square <= b_square) 61 | { 62 | const float tmp = 1.0f - x_square / b_square; 63 | return tmp * tmp; 64 | } 65 | else 66 | return 0; 67 | } 68 | void TukeyWeightFunction::configure(const float& param) 69 | { 70 | b_square = param * param; 71 | } 72 | const float TDistributionWeightFunction::DEFAULT_DOF = 5.0f; 73 | TDistributionWeightFunction::TDistributionWeightFunction(const float dof) 74 | { 75 | configure(dof); 76 | } 77 | float TDistributionWeightFunction:: 78 | value(const float & x) const 79 | { 80 | return ((dof_ + 1.0f) / (dof_ + (x * x))); 81 | } 82 | void TDistributionWeightFunction::configure(const float& param) 83 | { 84 | dof_ = param; 85 | normalizer_ = dof_ / (dof_ + 1.0f); 86 | } 87 | const float HuberWeightFunction::DEFAULT_K = 1.345f; 88 | HuberWeightFunction::HuberWeightFunction(const float k) 89 | { 90 | configure(k); 91 | } 92 | void HuberWeightFunction::configure(const float& param) 93 | { 94 | k = param; 95 | } 96 | float HuberWeightFunction::value(const float& t) const 97 | { 98 | const float t_abs = std::abs(t); 99 | if(t_abs < k) 100 | return 1.0f; 101 | else 102 | return k / t_abs; 103 | } 104 | } 105 | } 106 | -------------------------------------------------------------------------------- /thirdparty/fast/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # user config 3 | 4 | # Based on the ARM_ARCHITECTURE environment variable 5 | # Set IS_ARM to build on the Odroid board 6 | IF(DEFINED ENV{ARM_ARCHITECTURE}) 7 | MESSAGE("ARM_ARCHITECTURE environment variable set to " $ENV{ARM_ARCHITECTURE}) 8 | SET(IS_ARM TRUE) 9 | ELSE() 10 | SET(IS_ARM FALSE) 11 | ENDIF() 12 | 13 | SET(BUILD_TEST TRUE) 14 | 15 | ####################################### 16 | SET(PROJECT_NAME fast) 17 | PROJECT(${PROJECT_NAME}) 18 | CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3) 19 | SET(CMAKE_VERBOSE_MAKEFILE ON) 20 | 21 | # Build type and flags 22 | SET(CMAKE_BUILD_TYPE Release) # Options: Debug, RelWithDebInfo, Release 23 | SET(CMAKE_CXX_FLAGS "-Wall -Werror -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas") 24 | SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS} -O0 -g") 25 | IF(IS_ARM) 26 | SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -mfpu=neon -march=armv7-a") 27 | ELSE() 28 | SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -mmmx -msse -msse -msse2 -msse3 -mssse3 -fomit-frame-pointer") 29 | ENDIF() 30 | 31 | # Add Sources 32 | SET(SOURCES ${PROJECT_SOURCE_DIR}/src/fast_10.cpp 33 | ${PROJECT_SOURCE_DIR}/src/fast_9.cpp 34 | ${PROJECT_SOURCE_DIR}/src/fast_8_detect.cpp 35 | ${PROJECT_SOURCE_DIR}/src/fast_7_detect.cpp 36 | ${PROJECT_SOURCE_DIR}/src/fast_12_detect.cpp 37 | ${PROJECT_SOURCE_DIR}/src/fast_10_score.cpp 38 | ${PROJECT_SOURCE_DIR}/src/fast_9_score.cpp 39 | ${PROJECT_SOURCE_DIR}/src/fast_8_score.cpp 40 | ${PROJECT_SOURCE_DIR}/src/fast_7_score.cpp 41 | ${PROJECT_SOURCE_DIR}/src/fast_12_score.cpp 42 | ${PROJECT_SOURCE_DIR}/src/nonmax_3x3.cpp) 43 | IF(IS_ARM) 44 | # LIST(APPEND SOURCES ${PROJECT_SOURCE_DIR}/src/faster_corner_9_neon.cpp) 45 | ELSE() 46 | LIST(APPEND SOURCES ${PROJECT_SOURCE_DIR}/src/faster_corner_10_sse.cpp) 47 | LIST(APPEND SOURCES ${PROJECT_SOURCE_DIR}/src/faster_corner_9_sse.cpp) 48 | ENDIF() 49 | 50 | # Add library 51 | INCLUDE_DIRECTORIES(include src) 52 | ADD_LIBRARY(${PROJECT_NAME} SHARED ${SOURCES}) 53 | 54 | # Add Tests 55 | #IF(BUILD_TEST) 56 | # MESSAGE("Building Fast Test") 57 | # FIND_PACKAGE(OpenCV REQUIRED) 58 | # ADD_DEFINITIONS(-DTEST_DATA_DIR=\"${PROJECT_SOURCE_DIR}/test/data\") 59 | # ADD_EXECUTABLE(fast_test test/test.cpp) 60 | # TARGET_LINK_LIBRARIES(fast_test ${OpenCV_LIBS} ${PROJECT_NAME}) 61 | #ENDIF() 62 | 63 | 64 | ################################################################################ 65 | # Create the fastConfig.cmake file for other cmake projects. 66 | #GET_TARGET_PROPERTY( FULL_LIBRARY_NAME ${PROJECT_NAME} LOCATION ) 67 | #SET(fast_LIBRARIES ${FULL_LIBRARY_NAME} ) 68 | #SET(fast_LIBRARY_DIR ${PROJECT_BINARY_DIR} ) 69 | #SET(fast_INCLUDE_DIR "${PROJECT_SOURCE_DIR}/include") 70 | #CONFIGURE_FILE( ${CMAKE_CURRENT_SOURCE_DIR}/fastConfig.cmake.in 71 | # ${CMAKE_CURRENT_BINARY_DIR}/fastConfig.cmake @ONLY IMMEDIATE ) 72 | #export( PACKAGE fast ) 73 | 74 | #INSTALL(DIRECTORY include/fast DESTINATION ${CMAKE_INSTALL_PREFIX}/include FILES_MATCHING PATTERN "*.h" ) 75 | #INSTALL(TARGETS ${PROJECT_NAME} DESTINATION ${CMAKE_INSTALL_PREFIX}/lib ) 76 | -------------------------------------------------------------------------------- /thirdparty/fast/README.md: -------------------------------------------------------------------------------- 1 | FAST corner detector by Edward Rosten. 2 | The sourcefiles are from the CVD library: http://www.edwardrosten.com/cvd/ 3 | -------------------------------------------------------------------------------- /thirdparty/fast/fastConfig.cmake.in: -------------------------------------------------------------------------------- 1 | ####################################################### 2 | # Fast source dir 3 | set( fast_SOURCE_DIR "@CMAKE_CURRENT_SOURCE_DIR@") 4 | 5 | ####################################################### 6 | # Fast build dir 7 | set( fast_DIR "@CMAKE_CURRENT_BINARY_DIR@") 8 | 9 | ####################################################### 10 | set( fast_INCLUDE_DIR "@fast_INCLUDE_DIR@" ) 11 | set( fast_INCLUDE_DIRS "@fast_INCLUDE_DIR@" ) 12 | 13 | set( fast_LIBRARIES "@fast_LIBRARIES@" ) 14 | set( fast_LIBRARY "@fast_LIBRARIES@" ) 15 | 16 | set( fast_LIBRARY_DIR "@fast_LIBRARY_DIR@" ) 17 | set( fast_LIBRARY_DIRS "@fast_LIBRARY_DIR@" ) 18 | -------------------------------------------------------------------------------- /thirdparty/fast/include/fast/fast.h: -------------------------------------------------------------------------------- 1 | #ifndef FAST_H 2 | #define FAST_H 3 | 4 | #include 5 | 6 | namespace fast 7 | { 8 | 9 | using ::std::vector; 10 | 11 | struct fast_xy 12 | { 13 | short x, y; 14 | fast_xy(short x_, short y_) : x(x_), y(y_) {} 15 | }; 16 | 17 | typedef unsigned char fast_byte; 18 | 19 | /// SSE2 optimized version of the corner 10 20 | void fast_corner_detect_10_sse2(const fast_byte* img, int imgWidth, int imgHeight, int widthStep, short barrier, vector& corners); 21 | void fast_corner_detect_9_sse2(const fast_byte* img, int imgWidth, int imgHeight, int widthStep, short barrier, vector& corners); 22 | 23 | /// plain C++ version of the corner 10 24 | void fast_corner_detect_10(const fast_byte* img, int imgWidth, int imgHeight, int widthStep, short barrier, vector& corners); 25 | 26 | void fast_corner_detect_plain_9(const fast_byte* img, int imgWidth, int imgHeight, int widthStep, short barrier, vector& corners); 27 | 28 | void fast_corner_detect_plain_8(const fast_byte* img, int imgWidth, int imgHeight, int widthStep, short barrier, vector& corners); 29 | 30 | /// plain C++ version of the corner 7 31 | void fast_corner_detect_plain_7(const fast_byte* img, int imgWidth, int imgHeight, int widthStep, short barrier, vector& corners); 32 | 33 | /// plain C++ version of the corner 12 34 | void fast_corner_detect_plain_12(const fast_byte* img, int imgWidth, int imgHeight, int widthStep, short barrier, vector& corners); 35 | 36 | /// corner score 10 37 | void fast_corner_score_10(const fast_byte* img, const int img_stride, const vector& corners, const int threshold, vector& scores); 38 | 39 | void fast_corner_score_9(const fast_byte* img, const int img_stride, const vector& corners, const int threshold, vector& scores); 40 | 41 | void fast_corner_score_8(const fast_byte* img, const int img_stride, const vector& corners, const int threshold, vector& scores); 42 | 43 | /// corner score 7 44 | void fast_corner_score_7(const fast_byte* img, const int img_stride, const vector& corners, const int threshold, vector& scores); 45 | 46 | /// corner score 12 47 | void fast_corner_score_12(const fast_byte* img, const int img_stride, const vector& corners, const int threshold, vector& scores); 48 | 49 | /// Nonmax Suppression on a 3x3 Window 50 | void fast_nonmax_3x3(const vector& corners, const vector& scores, vector& nonmax_corners); 51 | 52 | /// NEON optimized version of the corner 9 53 | void fast_corner_detect_9_neon(const fast_byte* img, int imgWidth, int imgHeight, int widthStep, short barrier, vector& corners); 54 | 55 | } // namespace fast 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /thirdparty/fast/include/fast/faster_corner_utilities.h: -------------------------------------------------------------------------------- 1 | #ifndef FAST_CORNER_UTILITIES_H 2 | #define FAST_CORNER_UTILITIES_H 3 | 4 | #if __ARM_NEON__ 5 | #include 6 | #elif __SSE2__ 7 | #include 8 | #endif 9 | 10 | namespace fast 11 | { 12 | 13 | /// Check if the pointer is aligned to the specified byte granularity 14 | template bool is_aligned(const void* ptr); 15 | template<> inline bool is_aligned<8>(const void* ptr) { return ((reinterpret_cast(ptr)) & 0x7) == 0; } 16 | template<> inline bool is_aligned<16>(const void* ptr) { return ((reinterpret_cast(ptr)) & 0xF) == 0; } 17 | 18 | 19 | struct Less 20 | { 21 | template static bool eval(const T1 a, const T2 b) 22 | { 23 | return a < b; 24 | } 25 | static short prep_t(short pixel_val, short barrier) 26 | { 27 | return pixel_val - barrier; 28 | } 29 | }; 30 | 31 | struct Greater 32 | { 33 | template static bool eval(const T1 a, const T2 b) 34 | { 35 | return a > b; 36 | } 37 | static short prep_t(short pixel_val, short barrier) 38 | { 39 | return pixel_val + barrier; 40 | } 41 | }; 42 | 43 | #if __SSE2__ 44 | 45 | #define CHECK_BARRIER(lo, hi, other, flags) \ 46 | { \ 47 | __m128i diff = _mm_subs_epu8(lo, other); \ 48 | __m128i diff2 = _mm_subs_epu8(other, hi); \ 49 | __m128i z = _mm_setzero_si128(); \ 50 | diff = _mm_cmpeq_epi8(diff, z); \ 51 | diff2 = _mm_cmpeq_epi8(diff2, z); \ 52 | flags = ~(_mm_movemask_epi8(diff) | (_mm_movemask_epi8(diff2) << 16)); \ 53 | } 54 | 55 | template inline __m128i load_si128(const void* addr) { return _mm_loadu_si128((const __m128i*)addr); } 56 | template <> inline __m128i load_si128(const void* addr) { return _mm_load_si128((const __m128i*)addr); } 57 | 58 | #endif 59 | 60 | } // namespace fast 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /thirdparty/fast/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fast 4 | 1.0.0 5 | 6 | A ROS wrapper for the FAST corner detector by Edward Rosten. 7 | The sourcefiles are from the CVD library: http://www.edwardrosten.com/cvd/ 8 | 9 | Christian Forster 10 | LGPL 11 | http://www.edwardrosten.com/work/fast.html 12 | catkin 13 | -------------------------------------------------------------------------------- /thirdparty/fast/test/data/test1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luodongting/VI-HSO/130f1858988b91996ea8a86c80fb5b69afd61f11/thirdparty/fast/test/data/test1.png -------------------------------------------------------------------------------- /thirdparty/fast/test/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | int main (int argc, char * argv[]) { 8 | const int n_trials = 1000; 9 | std::vector corners; 10 | cv::Mat img = cv::imread(std::string(TEST_DATA_DIR) + "/test1.png", 0); 11 | cv::Mat downSampled; 12 | cv::resize(img, downSampled, cv::Size(752, 480)); 13 | img = downSampled; 14 | 15 | printf("\nTesting PLAIN version\n"); 16 | double time_accumulator = 0; 17 | for (int i = 0; i < n_trials; ++i) { 18 | corners.clear(); 19 | double t = (double)cv::getTickCount(); 20 | fast::fast_corner_detect_10((fast::fast_byte *)(img.data), img.cols, img.rows, img.cols, 75, corners); 21 | time_accumulator += ((cv::getTickCount() - t) / cv::getTickFrequency()); 22 | } 23 | printf("PLAIN took %f ms (average over %d trials).\n", time_accumulator/((double)n_trials)*1000.0, n_trials ); 24 | printf("PLAIN version extracted %zu features.\n", corners.size()); 25 | 26 | #if __NEON__ 27 | printf("\nTesting NEON version\n"); 28 | time_accumulator = 0; 29 | for (int i = 0; i < n_trials; ++i) { 30 | corners.clear(); 31 | double t = (double)cv::getTickCount(); 32 | fast::fast_corner_detect_9_neon((fast::fast_byte *)(img.data), img.cols, img.rows, img.cols, 75, corners); 33 | time_accumulator += ((cv::getTickCount() - t) / cv::getTickFrequency()); 34 | } 35 | printf("NEON version took %f ms (average over %d trials).\n", time_accumulator/((double)n_trials)*1000.0, n_trials); 36 | printf("NEON version extracted %zu features.\n", corners.size()); 37 | #endif 38 | 39 | #if __SSE2__ 40 | printf("\nTesting SSE2 version\n"); 41 | time_accumulator = 0; 42 | for (int i = 0; i < n_trials; ++i) { 43 | corners.clear(); 44 | double t = (double)cv::getTickCount(); 45 | fast::fast_corner_detect_10_sse2((fast::fast_byte *)(img.data), img.cols, img.rows, img.cols, 75, corners); 46 | time_accumulator += ((cv::getTickCount() - t) / cv::getTickFrequency()); 47 | } 48 | printf("SSE2 version took %f ms (average over %d trials).\n", time_accumulator/((double)n_trials)*1000.0, n_trials); 49 | printf("SSE2 version extracted %zu features.\n", corners.size()); 50 | #endif 51 | 52 | printf("\nBENCHMARK version extracted 167 features.\n"); 53 | return 0; 54 | } 55 | -------------------------------------------------------------------------------- /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/FindCholmod.cmake: -------------------------------------------------------------------------------- 1 | # Cholmod lib usually requires linking to a blas and lapack library. 2 | # It is up to the user of this module to find a BLAS and link to it. 3 | 4 | if (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 5 | set(CHOLMOD_FIND_QUIETLY TRUE) 6 | endif (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 7 | 8 | find_path(CHOLMOD_INCLUDE_DIR 9 | NAMES 10 | cholmod.h 11 | PATHS 12 | $ENV{CHOLMODDIR} 13 | ${INCLUDE_INSTALL_DIR} 14 | PATH_SUFFIXES 15 | suitesparse 16 | ufsparse 17 | ) 18 | 19 | find_library(CHOLMOD_LIBRARY cholmod PATHS $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 20 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY}) 21 | 22 | if(CHOLMOD_LIBRARIES) 23 | 24 | get_filename_component(CHOLMOD_LIBDIR ${CHOLMOD_LIBRARIES} PATH) 25 | 26 | find_library(AMD_LIBRARY amd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 27 | if (AMD_LIBRARY) 28 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${AMD_LIBRARY}) 29 | else () 30 | set(CHOLMOD_LIBRARIES FALSE) 31 | endif () 32 | 33 | endif(CHOLMOD_LIBRARIES) 34 | 35 | if(CHOLMOD_LIBRARIES) 36 | 37 | find_library(COLAMD_LIBRARY colamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 38 | if (COLAMD_LIBRARY) 39 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${COLAMD_LIBRARY}) 40 | else () 41 | set(CHOLMOD_LIBRARIES FALSE) 42 | endif () 43 | 44 | endif(CHOLMOD_LIBRARIES) 45 | 46 | if(CHOLMOD_LIBRARIES) 47 | 48 | find_library(CAMD_LIBRARY camd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 49 | if (CAMD_LIBRARY) 50 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CAMD_LIBRARY}) 51 | else () 52 | set(CHOLMOD_LIBRARIES FALSE) 53 | endif () 54 | 55 | endif(CHOLMOD_LIBRARIES) 56 | 57 | if(CHOLMOD_LIBRARIES) 58 | 59 | find_library(CCOLAMD_LIBRARY ccolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 60 | if (CCOLAMD_LIBRARY) 61 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CCOLAMD_LIBRARY}) 62 | else () 63 | set(CHOLMOD_LIBRARIES FALSE) 64 | endif () 65 | 66 | endif(CHOLMOD_LIBRARIES) 67 | 68 | if(CHOLMOD_LIBRARIES) 69 | 70 | find_library(CHOLMOD_METIS_LIBRARY metis PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 71 | if (CHOLMOD_METIS_LIBRARY) 72 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_METIS_LIBRARY}) 73 | endif () 74 | 75 | endif(CHOLMOD_LIBRARIES) 76 | 77 | if(CHOLMOD_LIBRARIES) 78 | find_library(CHOLMOD_SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig 79 | PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 80 | if (CHOLMOD_SUITESPARSECONFIG_LIBRARY) 81 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_SUITESPARSECONFIG_LIBRARY}) 82 | endif () 83 | endif(CHOLMOD_LIBRARIES) 84 | 85 | include(FindPackageHandleStandardArgs) 86 | find_package_handle_standard_args(CHOLMOD DEFAULT_MSG 87 | CHOLMOD_INCLUDE_DIR CHOLMOD_LIBRARIES) 88 | 89 | mark_as_advanced(CHOLMOD_LIBRARIES) 90 | -------------------------------------------------------------------------------- /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 | // available sparse matrix libraries 8 | #define G2O_HAVE_CHOLMOD 1 9 | 10 | // give a warning if Eigen defaults to row-major matrices. 11 | // We internally assume column-major matrices throughout the code. 12 | #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR 13 | # error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)" 14 | #endif 15 | 16 | #endif 17 | -------------------------------------------------------------------------------- /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 | // available sparse matrix libraries 8 | #cmakedefine G2O_HAVE_CHOLMOD 1 9 | 10 | // give a warning if Eigen defaults to row-major matrices. 11 | // We internally assume column-major matrices throughout the code. 12 | #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR 13 | # error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)" 14 | #endif 15 | 16 | #endif 17 | -------------------------------------------------------------------------------- /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/g2o_core_api.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @Project: HSO_IMU 3 | * @Remark: 4 | * @Author: YWZ 5 | * @Date: 2022-06-11 16:49:14 6 | * @LastEditors: YWZ 7 | * @LastEditTime: 2022-06-11 17:14:07 8 | * @FilePath: /hso_imu/thirdparty/g2o/g2o/core/g2o_core_api.h 9 | */ 10 | /*************************************************************************** 11 | * Description: import/export macros for creating DLLS with Microsoft 12 | * compiler. Any exported function needs to be declared with the 13 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 14 | * for each DLL (arrrrrgh!!!) 15 | * 16 | * 17 Jan 2012 17 | * Email: pupilli@cs.bris.ac.uk 18 | ****************************************************************************/ 19 | #ifndef G2O_CORE_API_H 20 | #define G2O_CORE_API_H 21 | 22 | // #include "g2o/config.h" 23 | #include "config.h" 24 | 25 | #ifdef _MSC_VER 26 | // We are using a Microsoft compiler: 27 | #ifdef G2O_SHARED_LIBS 28 | #ifdef core_EXPORTS 29 | #define G2O_CORE_API __declspec(dllexport) 30 | #else 31 | #define G2O_CORE_API __declspec(dllimport) 32 | #endif 33 | #else 34 | #define G2O_CORE_API 35 | #endif 36 | 37 | #else 38 | // Not Microsoft compiler so set empty definition: 39 | #define G2O_CORE_API 40 | #endif 41 | 42 | #endif // G2O_CORE_API_H 43 | -------------------------------------------------------------------------------- /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.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 "matrix_structure.h" 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | using namespace std; 34 | 35 | namespace g2o { 36 | 37 | struct ColSort 38 | { 39 | bool operator()(const pair& e1, const pair& e2) const 40 | { 41 | return e1.second < e2.second || (e1.second == e2.second && e1.first < e2.first); 42 | } 43 | }; 44 | 45 | MatrixStructure::MatrixStructure() : 46 | n(0), m(0), Ap(0), Aii(0), maxN(0), maxNz(0) 47 | { 48 | } 49 | 50 | MatrixStructure::~MatrixStructure() 51 | { 52 | free(); 53 | } 54 | 55 | void MatrixStructure::alloc(int n_, int nz) 56 | { 57 | if (n == 0) { 58 | maxN = n = n_; 59 | maxNz = nz; 60 | Ap = new int[maxN + 1]; 61 | Aii = new int[maxNz]; 62 | } 63 | else { 64 | n = n_; 65 | if (maxNz < nz) { 66 | maxNz = 2 * nz; 67 | delete[] Aii; 68 | Aii = new int[maxNz]; 69 | } 70 | if (maxN < n) { 71 | maxN = 2 * n; 72 | delete[] Ap; 73 | Ap = new int[maxN + 1]; 74 | } 75 | } 76 | } 77 | 78 | void MatrixStructure::free() 79 | { 80 | n = 0; 81 | m = 0; 82 | maxN = 0; 83 | maxNz = 0; 84 | delete[] Aii; Aii = 0; 85 | delete[] Ap; Ap = 0; 86 | } 87 | 88 | bool MatrixStructure::write(const char* filename) const 89 | { 90 | const int& cols = n; 91 | const int& rows = m; 92 | 93 | string name = filename; 94 | std::string::size_type lastDot = name.find_last_of('.'); 95 | if (lastDot != std::string::npos) 96 | name = name.substr(0, lastDot); 97 | 98 | vector > entries; 99 | for (int i=0; i < cols; ++i) { 100 | const int& rbeg = Ap[i]; 101 | const int& rend = Ap[i+1]; 102 | for (int j = rbeg; j < rend; ++j) { 103 | entries.push_back(make_pair(Aii[j], i)); 104 | if (Aii[j] != i) 105 | entries.push_back(make_pair(i, Aii[j])); 106 | } 107 | } 108 | 109 | sort(entries.begin(), entries.end(), ColSort()); 110 | 111 | std::ofstream fout(filename); 112 | fout << "# name: " << name << std::endl; 113 | fout << "# type: sparse matrix" << std::endl; 114 | fout << "# nnz: " << entries.size() << std::endl; 115 | fout << "# rows: " << rows << std::endl; 116 | fout << "# columns: " << cols << std::endl; 117 | for (vector >::const_iterator it = entries.begin(); it != entries.end(); ++it) { 118 | const pair& entry = *it; 119 | fout << entry.first << " " << entry.second << " 0" << std::endl; // write a constant value of 0 120 | } 121 | 122 | return fout.good(); 123 | } 124 | 125 | } // end namespace 126 | -------------------------------------------------------------------------------- /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/core/sparse_optimizer_terminate_action.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @Project: HSO_IMU 3 | * @Remark: 4 | * @Author: YWZ 5 | * @Date: 2022-06-11 16:44:10 6 | * @LastEditors: YWZ 7 | * @LastEditTime: 2022-06-11 16:55:57 8 | * @FilePath: /hso_imu/thirdparty/g2o/g2o/core/sparse_optimizer_terminate_action.h 9 | */ 10 | // g2o - General Graph Optimization 11 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard 12 | // All rights reserved. 13 | // 14 | // Redistribution and use in source and binary forms, with or without 15 | // modification, are permitted provided that the following conditions are 16 | // met: 17 | // 18 | // * Redistributions of source code must retain the above copyright notice, 19 | // this list of conditions and the following disclaimer. 20 | // * Redistributions in binary form must reproduce the above copyright 21 | // notice, this list of conditions and the following disclaimer in the 22 | // documentation and/or other materials provided with the distribution. 23 | // 24 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 25 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 26 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 27 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 28 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 29 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 30 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 31 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 32 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 33 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 34 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | 36 | #ifndef SPARSE_OPTIMIZER_TERMINATE_ACTION_H 37 | #define SPARSE_OPTIMIZER_TERMINATE_ACTION_H 38 | 39 | #include "g2o_core_api.h" 40 | #include "hyper_graph_action.h" 41 | 42 | namespace g2o { 43 | 44 | class SparseOptimizer; 45 | 46 | /** 47 | * \brief stop iterating based on the gain which is (oldChi - currentChi) / 48 | * currentChi. 49 | * 50 | * stop iterating based on the gain which is (oldChi - currentChi) / currentChi. 51 | * If the gain is larger than zero and below the threshold, then the optimizer 52 | * is stopped. Typically usage of this action includes adding it as a 53 | * postIteration action, by calling addPostIterationAction on a sparse 54 | * optimizer. 55 | */ 56 | class G2O_CORE_API SparseOptimizerTerminateAction : public HyperGraphAction { 57 | public: 58 | SparseOptimizerTerminateAction(); 59 | virtual HyperGraphAction* operator()(const HyperGraph* graph, 60 | Parameters* parameters = 0); 61 | 62 | double gainThreshold() const { return _gainThreshold; } 63 | void setGainThreshold(double gainThreshold); 64 | 65 | int maxIterations() const { return _maxIterations; } 66 | void setMaxIterations(int maxit); 67 | 68 | protected: 69 | double _gainThreshold; 70 | double _lastChi; 71 | bool _auxTerminateFlag; 72 | int _maxIterations; 73 | 74 | void setOptimizerStopFlag(const SparseOptimizer* optimizer, bool stop); 75 | }; 76 | 77 | } // namespace g2o 78 | 79 | #endif 80 | -------------------------------------------------------------------------------- /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/g2o_stuff_api.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Description: import/export macros for creating DLLS with Microsoft 3 | * compiler. Any exported function needs to be declared with the 4 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 5 | * for each DLL (arrrrrgh!!!) 6 | * 7 | * 17 Jan 2012 8 | * Email: pupilli@cs.bris.ac.uk 9 | ****************************************************************************/ 10 | #ifndef G2O_STUFF_API_H 11 | #define G2O_STUFF_API_H 12 | 13 | #include "config.h" 14 | 15 | #ifdef _MSC_VER 16 | // We are using a Microsoft compiler: 17 | 18 | #ifdef G2O_SHARED_LIBS 19 | #ifdef stuff_EXPORTS 20 | #define G2O_STUFF_API __declspec(dllexport) 21 | #else 22 | #define G2O_STUFF_API __declspec(dllimport) 23 | #endif 24 | #else 25 | #define G2O_STUFF_API 26 | #endif 27 | 28 | #else 29 | // Not Microsoft compiler so set empty definition: 30 | #define G2O_STUFF_API 31 | #endif 32 | 33 | #endif // G2O_STUFF_API_H 34 | -------------------------------------------------------------------------------- /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/stuff/sparse_helper.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_helper.h" 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | using namespace std; 36 | 37 | namespace g2o { 38 | 39 | namespace { 40 | struct TripletEntry 41 | { 42 | int r, c; 43 | double x; 44 | TripletEntry(int r_, int c_, double x_) : r(r_), c(c_), x(x_) {} 45 | }; 46 | struct TripletColSort 47 | { 48 | bool operator()(const TripletEntry& e1, const TripletEntry& e2) const 49 | { 50 | return e1.c < e2.c || (e1.c == e2.c && e1.r < e2.r); 51 | } 52 | }; 53 | } 54 | 55 | bool writeVector(const string& filename, const double*v, int n) 56 | { 57 | ofstream os(filename.c_str()); 58 | os << fixed; 59 | for (int i=0; i entries; 67 | entries.reserve((size_t)Ap[cols]); 68 | for (int i=0; i < cols; i++) { 69 | const int& rbeg = Ap[i]; 70 | const int& rend = Ap[i+1]; 71 | for (int j = rbeg; j < rend; j++) { 72 | entries.push_back(TripletEntry(Ai[j], i, Ax[j])); 73 | if (upperTriangleSymmetric && Ai[j] != i) 74 | entries.push_back(TripletEntry(i, Ai[j], Ax[j])); 75 | } 76 | } 77 | sort(entries.begin(), entries.end(), TripletColSort()); 78 | 79 | string name = filename; 80 | std::string::size_type lastDot = name.find_last_of('.'); 81 | if (lastDot != std::string::npos) 82 | name = name.substr(0, lastDot); 83 | 84 | std::ofstream fout(filename.c_str()); 85 | fout << "# name: " << name << std::endl; 86 | fout << "# type: sparse matrix" << std::endl; 87 | fout << "# nnz: " << entries.size() << std::endl; 88 | fout << "# rows: " << rows << std::endl; 89 | fout << "# columns: " << cols << std::endl; 90 | //fout << fixed; 91 | fout << setprecision(9) << endl; 92 | for (vector::const_iterator it = entries.begin(); it != entries.end(); ++it) { 93 | const TripletEntry& entry = *it; 94 | fout << entry.r+1 << " " << entry.c+1 << " " << entry.x << std::endl; 95 | } 96 | return fout.good(); 97 | } 98 | 99 | } // end namespace 100 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/stuff/sparse_helper.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_SPARSE_HELPER_H 28 | #define G2O_SPARSE_HELPER_H 29 | 30 | #include "g2o_stuff_api.h" 31 | 32 | #include 33 | 34 | namespace g2o { 35 | 36 | /** 37 | * write an array to a file, debugging 38 | */ 39 | G2O_STUFF_API bool writeVector(const std::string& filename, const double*v, int n); 40 | 41 | /** 42 | * write a CCS matrix given by pointer to column, row, and values 43 | */ 44 | G2O_STUFF_API bool writeCCSMatrix(const std::string& filename, int rows, int cols, const int* p, const int* i, const double* v, bool upperTriangleSymmetric = true); 45 | 46 | } // end namespace 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/types/se3_ops.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 H. Strasdat 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_MATH_STUFF 28 | #define G2O_MATH_STUFF 29 | 30 | #include 31 | #include 32 | 33 | namespace g2o { 34 | using namespace Eigen; 35 | 36 | inline Matrix3d skew(const Vector3d&v); 37 | inline Vector3d deltaR(const Matrix3d& R); 38 | inline Vector2d project(const Vector3d&); 39 | inline Vector3d project(const Vector4d&); 40 | inline Vector3d unproject(const Vector2d&); 41 | inline Vector4d unproject(const Vector3d&); 42 | 43 | #include "se3_ops.hpp" 44 | 45 | } 46 | 47 | #endif //MATH_STUFF 48 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/types/se3_ops.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 H. Strasdat 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | Matrix3d skew(const Vector3d&v) 28 | { 29 | Matrix3d m; 30 | m.fill(0.); 31 | m(0,1) = -v(2); 32 | m(0,2) = v(1); 33 | m(1,2) = -v(0); 34 | m(1,0) = v(2); 35 | m(2,0) = -v(1); 36 | m(2,1) = v(0); 37 | return m; 38 | } 39 | 40 | Vector3d deltaR(const Matrix3d& R) 41 | { 42 | Vector3d v; 43 | v(0)=R(2,1)-R(1,2); 44 | v(1)=R(0,2)-R(2,0); 45 | v(2)=R(1,0)-R(0,1); 46 | return v; 47 | } 48 | 49 | Vector2d project(const Vector3d& v) 50 | { 51 | Vector2d res; 52 | res(0) = v(0)/v(2); 53 | res(1) = v(1)/v(2); 54 | return res; 55 | } 56 | 57 | Vector3d project(const Vector4d& v) 58 | { 59 | Vector3d res; 60 | res(0) = v(0)/v(3); 61 | res(1) = v(1)/v(3); 62 | res(2) = v(2)/v(3); 63 | return res; 64 | } 65 | 66 | Vector3d unproject(const Vector2d& v) 67 | { 68 | Vector3d res; 69 | res(0) = v(0); 70 | res(1) = v(1); 71 | res(2) = 1; 72 | return res; 73 | } 74 | 75 | Vector4d unproject(const Vector3d& v) 76 | { 77 | Vector4d res; 78 | res(0) = v(0); 79 | res(1) = v(1); 80 | res(2) = v(2); 81 | res(3) = 1; 82 | return res; 83 | } 84 | 85 | 86 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/types/types_sba.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 Kurt Konolige 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "types_sba.h" 28 | #include 29 | 30 | namespace g2o { 31 | 32 | using namespace std; 33 | 34 | 35 | VertexSBAPointXYZ::VertexSBAPointXYZ() : BaseVertex<3, Vector3d>() 36 | { 37 | } 38 | 39 | bool VertexSBAPointXYZ::read(std::istream& is) 40 | { 41 | Vector3d lv; 42 | for (int i=0; i<3; i++) 43 | is >> _estimate[i]; 44 | return true; 45 | } 46 | 47 | bool VertexSBAPointXYZ::write(std::ostream& os) const 48 | { 49 | Vector3d lv=estimate(); 50 | for (int i=0; i<3; i++){ 51 | os << lv[i] << " "; 52 | } 53 | return os.good(); 54 | } 55 | 56 | } // end namespace 57 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/types/types_sba.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 Kurt Konolige 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_SBA_TYPES 28 | #define G2O_SBA_TYPES 29 | 30 | #include "../core/base_vertex.h" 31 | 32 | #include 33 | #include 34 | 35 | namespace g2o { 36 | 37 | /** 38 | * \brief Point vertex, XYZ 39 | */ 40 | class VertexSBAPointXYZ : public BaseVertex<3, Vector3d> 41 | { 42 | public: 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 44 | VertexSBAPointXYZ(); 45 | virtual bool read(std::istream& is); 46 | virtual bool write(std::ostream& os) const; 47 | 48 | virtual void setToOriginImpl() { 49 | _estimate.fill(0.); 50 | } 51 | 52 | virtual void oplusImpl(const double* update) 53 | { 54 | Eigen::Map v(update); 55 | _estimate += v; 56 | } 57 | }; 58 | 59 | } // end namespace 60 | 61 | #endif // SBA_TYPES 62 | -------------------------------------------------------------------------------- /thirdparty/g2o/lib/libg2o.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luodongting/VI-HSO/130f1858988b91996ea8a86c80fb5b69afd61f11/thirdparty/g2o/lib/libg2o.so -------------------------------------------------------------------------------- /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 | --------------------------------------------------------------------------------