├── .gitignore ├── README.md ├── build.sh └── g2o ├── CMakeLists.txt ├── Thirdparty └── g2o │ ├── CMakeLists.txt │ ├── README.txt │ ├── cmake_modules │ ├── FindBLAS.cmake │ ├── FindCSparse.cmake │ ├── FindCholmod.cmake │ ├── FindEigen3.cmake │ ├── FindLAPACK.cmake │ └── FindSuiteSparse.cmake │ ├── config.h │ ├── config.h.in │ ├── g2o │ ├── core │ │ ├── base_binary_edge.h │ │ ├── base_binary_edge.hpp │ │ ├── base_edge.h │ │ ├── base_multi_edge.h │ │ ├── base_multi_edge.hpp │ │ ├── base_unary_edge.h │ │ ├── base_unary_edge.hpp │ │ ├── base_vertex.h │ │ ├── base_vertex.hpp │ │ ├── batch_stats.cpp │ │ ├── batch_stats.h │ │ ├── block_solver.h │ │ ├── block_solver.hpp │ │ ├── cache.cpp │ │ ├── cache.h │ │ ├── creators.h │ │ ├── eigen_types.h │ │ ├── estimate_propagator.cpp │ │ ├── estimate_propagator.h │ │ ├── factory.cpp │ │ ├── factory.h │ │ ├── hyper_dijkstra.cpp │ │ ├── hyper_dijkstra.h │ │ ├── hyper_graph.cpp │ │ ├── hyper_graph.h │ │ ├── hyper_graph_action.cpp │ │ ├── hyper_graph_action.h │ │ ├── jacobian_workspace.cpp │ │ ├── jacobian_workspace.h │ │ ├── linear_solver.h │ │ ├── marginal_covariance_cholesky.cpp │ │ ├── marginal_covariance_cholesky.h │ │ ├── matrix_operations.h │ │ ├── matrix_structure.cpp │ │ ├── matrix_structure.h │ │ ├── openmp_mutex.h │ │ ├── optimizable_graph.cpp │ │ ├── optimizable_graph.h │ │ ├── optimization_algorithm.cpp │ │ ├── optimization_algorithm.h │ │ ├── optimization_algorithm_dogleg.cpp │ │ ├── optimization_algorithm_dogleg.h │ │ ├── optimization_algorithm_factory.cpp │ │ ├── optimization_algorithm_factory.h │ │ ├── optimization_algorithm_gauss_newton.cpp │ │ ├── optimization_algorithm_gauss_newton.h │ │ ├── optimization_algorithm_levenberg.cpp │ │ ├── optimization_algorithm_levenberg.h │ │ ├── optimization_algorithm_property.h │ │ ├── optimization_algorithm_with_hessian.cpp │ │ ├── optimization_algorithm_with_hessian.h │ │ ├── parameter.cpp │ │ ├── parameter.h │ │ ├── parameter_container.cpp │ │ ├── parameter_container.h │ │ ├── robust_kernel.cpp │ │ ├── robust_kernel.h │ │ ├── robust_kernel_factory.cpp │ │ ├── robust_kernel_factory.h │ │ ├── robust_kernel_impl.cpp │ │ ├── robust_kernel_impl.h │ │ ├── solver.cpp │ │ ├── solver.h │ │ ├── sparse_block_matrix.h │ │ ├── sparse_block_matrix.hpp │ │ ├── sparse_block_matrix_ccs.h │ │ ├── sparse_block_matrix_diagonal.h │ │ ├── sparse_block_matrix_test.cpp │ │ ├── sparse_optimizer.cpp │ │ └── sparse_optimizer.h │ ├── solvers │ │ ├── linear_solver_cholmod.h │ │ ├── linear_solver_dense.h │ │ └── linear_solver_eigen.h │ ├── stuff │ │ ├── color_macros.h │ │ ├── macros.h │ │ ├── misc.h │ │ ├── os_specific.c │ │ ├── os_specific.h │ │ ├── property.cpp │ │ ├── property.h │ │ ├── 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 │ └── license-bsd.txt ├── cmake_modules ├── FindCholmod.cmake └── FindEigen3.cmake ├── include ├── IMU_Data.h ├── IMU_Preintegrator.h └── so3.h └── src ├── IMU_Data.cpp ├── IMU_Preintegrator.cpp ├── main.cpp └── so3.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | #IDE Project 5 | .kdev4/ 6 | *.kdev4 7 | *.idea 8 | 9 | 10 | # Compiled Object files 11 | *.slo 12 | *.lo 13 | *.o 14 | *.obj 15 | build 16 | tmp 17 | *~ 18 | cmake-build-debug 19 | bin 20 | 21 | # Precompiled Headers 22 | *.gch 23 | *.pch 24 | 25 | # Compiled Dynamic libraries 26 | *.so 27 | *.dylib 28 | *.dll 29 | 30 | # Fortran module files 31 | *.mod 32 | *.smod 33 | *.bak 34 | *.yaml 35 | *.log 36 | 37 | # Compiled Static libraries 38 | *.lai 39 | *.la 40 | *.a 41 | *.lib 42 | 43 | # Executables 44 | *.exe 45 | *.out 46 | *.app 47 | 48 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # IMU Preintegration 2 | 3 | **Current version:** 0.0.1 4 | 5 | IMU Preintegration is a real-time module library. It is able to use for VIO and VI SLAM. We provide two parameterized method (rotation matrix and quaternion) using two non-linear optimizations libraries for estimating the states. We also provide the examples to run this library and show how to use this IMU Preintegration module library. 6 | 7 | This library is not bug-free, and welcome to improve it together! 8 | 9 | 10 | *version*: 0.0.1 11 | The version 0.0.1 only provide the API for computing the IMU Preintegration measurments. In next version will provide the g2o library state vertex and error edge for IMU Preintegration. 12 | 13 | ## Related Publications: 14 | 15 | * Forster C, Carlone L, Dellaert F, et al. **On-Manifold Preintegration for Real-Time Visual--Inertial Odometry**. IEEE Transactions on Robotics, 2017, 33(1): 1-21. **[PDF](http://rpg.ifi.uzh.ch/docs/TRO16_forster.pdf)**. 16 | 17 | * Tong Qin, Peiliang Li, Zhenfei Yang, Shaojie Shen, **VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator**, IEEE Transactions on Robotics. **[PDF](https://ieeexplore.ieee.org/document/8421746/?arnumber=8421746&source=authoralert)** 18 | 19 | 20 | ## 1. License 21 | IMU Preintegration is released under a [GPLv3 license](https://github.com/raulmur/ORB_SLAM2/blob/master/License-gpl.txt). For a list of all code/library dependencies (and associated licenses), please see [Dependencies.md](https://github.com/raulmur/ORB_SLAM2/blob/master/Dependencies.md). 22 | 23 | 24 | ## 2. Prerequisites 25 | We have tested the library in **Ubuntu 14.04** and **16.04**, but it should be easy to compile in other platforms. A powerful computer (e.g. i7) will ensure real-time performance and provide more stable and accurate results. 26 | 27 | ### C++11 or C++0x Compiler 28 | We use the new thread and chrono functionalities of C++11. 29 | 30 | ### Eigen3 31 | Required by g2o (see below). Download and install instructions can be found at: http://eigen.tuxfamily.org. **Required at least 3.1.0**. 32 | 33 | ### BLAS and LAPACK 34 | [BLAS](http://www.netlib.org/blas) and [LAPACK](http://www.netlib.org/lapack) libraries are requiered by g2o (see below). On ubuntu: 35 | ``` 36 | sudo apt-get install libblas-dev 37 | sudo apt-get install liblapack-dev 38 | ``` 39 | 40 | ### g2o (Included in Thirdparty folder) 41 | We use modified versions of the [g2o](https://github.com/RainerKuemmerle/g2o) library to perform non-linear optimizations. The modified libraries (which are BSD) are included in the *Thirdparty* folder. 42 | 43 | ### ceres 44 | We also use [ceres](https://github.com/ceres-solver/ceres-solver) library to perform non-linear optimizations. 45 | 46 | 47 | ## 3. Building IMU Preintegration library 48 | 49 | Clone the repository: 50 | ``` 51 | git clone git@github.com:mc275/IMU_Preintegration.git 52 | ``` 53 | 54 | We provide a script `build.sh` to build the *Thirdparty* libraries and *IMU Preintegration*. Please make sure you have installed all required dependencies (see section 2). Execute: 55 | ``` 56 | cd IMU_Preintegration 57 | chmod +x build.sh 58 | ./build.sh 59 | ``` 60 | 61 | This will create **libIMU_Preintegration.so** at *lib* folder and the executables **main** in *bin* folder. 62 | 63 | ## 4. Run Examples 64 | 65 | ### EuRoC Dataset 66 | 67 | 1. Download a sequence (ASL format) from http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets 68 | 69 | 2. Execute the following first command for V1 and V2 sequences, or the second command for MH sequences. Change PATH_TO_SEQUENCE_FOLDER and SEQUENCE according to the sequence you want to run. 70 | ``` 71 | ./bin/main PATH_TO_SEQUENCE_FOLDER/imu0/data.csv 72 | ``` 73 | 74 | ## 5. Processing your own sequences 75 | You can refer to the examples for IMU Preintegration and call this library API for your own datasets. 76 | -------------------------------------------------------------------------------- /build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | cd g2o/ 4 | 5 | echo "Configuring and building Thirdparty/g2o ..." 6 | cd Thirdparty/g2o 7 | if [ ! -d "build/" ];then 8 | mkdir build 9 | fi 10 | cd build 11 | cmake .. -DCMAKE_BUILD_TYPE=Release 12 | make -j 13 | 14 | cd ../../../ 15 | echo "Configuring and building IMU_Preintegration ..." 16 | if [ ! -d "/build" ];then 17 | mkdir build 18 | fi 19 | cd build 20 | cmake .. -DCMAKE_BUILD_TYPE=Release 21 | make -j 22 | cd .. 23 | -------------------------------------------------------------------------------- /g2o/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # ORB-SLAM 项目管理 2 | 3 | # 确定最低版本CMake。 4 | cmake_minimum_required(VERSION 2.8) 5 | project(IMU_Preintegration) 6 | 7 | # 设置项目编译方式 Release。 8 | IF(NOT CMAKE_BUILD_TYPE) 9 | SET(CMAKE_BUILD_TYPE Release ) 10 | ENDIF() 11 | 12 | MESSAGE("Build Type: " ${CMAKE_BUILD_TYPE}) 13 | 14 | # 检查C++11或C++0x 特性支持。 15 | add_definitions(-DCOMPILEDWITHC11) 16 | if( CMAKE_BUILD_TYPE MATCHES "Debug") 17 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -msse3 -std=c++11 -pthread -g -march=native") 18 | else( CMAKE_BUILD_TYPE MATCHES "Release") 19 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -msse3 -std=c++11 -pthread -O3 -march=native") 20 | endif( CMAKE_BUILD_TYPE MATCHES "Debug") 21 | 22 | # CMake文件列表。 23 | LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) 24 | 25 | # 查找依赖库。 26 | find_package(Eigen3 REQUIRED) 27 | find_package(Cholmod REQUIRED) 28 | 29 | # include工程目录。 30 | include_directories( 31 | ${PROJECT_SOURCE_DIR} 32 | ${PROJECT_SOURCE_DIR}/include 33 | ${EIGEN3_INCLUDE_DIR} 34 | ${CHOLMOD_INCLUDE_DIR} 35 | ) 36 | 37 | # 设置CMake库输出目录。 38 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) 39 | 40 | # 添加库 41 | add_library(${PROJECT_NAME} SHARED 42 | src/so3.cpp 43 | src/IMU_Data.cpp 44 | src/IMU_Preintegrator.cpp 45 | ) 46 | 47 | # 链接库。 48 | target_link_libraries(${PROJECT_NAME} 49 | ${EIGEN3_LIBS} 50 | ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so 51 | cholmod 52 | ${CHOLMOD_LIBRARIES} 53 | ${BLAS_LIBRARIES} 54 | ${LAPACK_LIBRARIES} 55 | ) 56 | 57 | 58 | # 编译单目KITTI数据集可执行文件 59 | SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin) 60 | add_executable(main src/main.cpp) 61 | target_link_libraries(main ${PROJECT_NAME}) 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/Thirdparty/g2o/cmake_modules/FindCSparse.cmake: -------------------------------------------------------------------------------- 1 | # Look for csparse; note the difference in the directory specifications! 2 | FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h 3 | PATHS 4 | /usr/include/suitesparse 5 | /usr/include 6 | /opt/local/include 7 | /usr/local/include 8 | /sw/include 9 | /usr/include/ufsparse 10 | /opt/local/include/ufsparse 11 | /usr/local/include/ufsparse 12 | /sw/include/ufsparse 13 | ) 14 | 15 | FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse 16 | PATHS 17 | /usr/lib 18 | /usr/local/lib 19 | /opt/local/lib 20 | /sw/lib 21 | ) 22 | 23 | include(FindPackageHandleStandardArgs) 24 | find_package_handle_standard_args(CSPARSE DEFAULT_MSG 25 | CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY) 26 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/Thirdparty/g2o/cmake_modules/FindSuiteSparse.cmake: -------------------------------------------------------------------------------- 1 | FIND_PATH(CHOLMOD_INCLUDE_DIR NAMES cholmod.h amd.h camd.h 2 | PATHS 3 | ${SUITE_SPARSE_ROOT}/include 4 | /usr/include/suitesparse 5 | /usr/include/ufsparse 6 | /opt/local/include/ufsparse 7 | /usr/local/include/ufsparse 8 | /sw/include/ufsparse 9 | ) 10 | 11 | FIND_LIBRARY(CHOLMOD_LIBRARY NAMES cholmod 12 | PATHS 13 | ${SUITE_SPARSE_ROOT}/lib 14 | /usr/lib 15 | /usr/local/lib 16 | /opt/local/lib 17 | /sw/lib 18 | ) 19 | 20 | FIND_LIBRARY(AMD_LIBRARY NAMES SHARED NAMES amd 21 | PATHS 22 | ${SUITE_SPARSE_ROOT}/lib 23 | /usr/lib 24 | /usr/local/lib 25 | /opt/local/lib 26 | /sw/lib 27 | ) 28 | 29 | FIND_LIBRARY(CAMD_LIBRARY NAMES camd 30 | PATHS 31 | ${SUITE_SPARSE_ROOT}/lib 32 | /usr/lib 33 | /usr/local/lib 34 | /opt/local/lib 35 | /sw/lib 36 | ) 37 | 38 | FIND_LIBRARY(SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig 39 | PATHS 40 | ${SUITE_SPARSE_ROOT}/lib 41 | /usr/lib 42 | /usr/local/lib 43 | /opt/local/lib 44 | /sw/lib 45 | ) 46 | 47 | 48 | # Different platforms seemingly require linking against different sets of libraries 49 | IF(CYGWIN) 50 | FIND_PACKAGE(PkgConfig) 51 | FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd 52 | PATHS 53 | /usr/lib 54 | /usr/local/lib 55 | /opt/local/lib 56 | /sw/lib 57 | ) 58 | PKG_CHECK_MODULES(LAPACK lapack) 59 | 60 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${LAPACK_LIBRARIES}) 61 | 62 | # MacPorts build of the SparseSuite requires linking against extra libraries 63 | 64 | ELSEIF(APPLE) 65 | 66 | FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd 67 | PATHS 68 | /usr/lib 69 | /usr/local/lib 70 | /opt/local/lib 71 | /sw/lib 72 | ) 73 | 74 | FIND_LIBRARY(CCOLAMD_LIBRARY NAMES ccolamd 75 | PATHS 76 | /usr/lib 77 | /usr/local/lib 78 | /opt/local/lib 79 | /sw/lib 80 | ) 81 | 82 | FIND_LIBRARY(METIS_LIBRARY NAMES metis 83 | PATHS 84 | /usr/lib 85 | /usr/local/lib 86 | /opt/local/lib 87 | /sw/lib 88 | ) 89 | 90 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${METIS_LIBRARY} "-framework Accelerate") 91 | ELSE(APPLE) 92 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY}) 93 | ENDIF(CYGWIN) 94 | 95 | IF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 96 | SET(CHOLMOD_FOUND TRUE) 97 | ELSE(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 98 | SET(CHOLMOD_FOUND FALSE) 99 | ENDIF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 100 | 101 | # Look for csparse; note the difference in the directory specifications! 102 | FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h 103 | PATHS 104 | /usr/include/suitesparse 105 | /usr/include 106 | /opt/local/include 107 | /usr/local/include 108 | /sw/include 109 | /usr/include/ufsparse 110 | /opt/local/include/ufsparse 111 | /usr/local/include/ufsparse 112 | /sw/include/ufsparse 113 | ) 114 | 115 | FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse 116 | PATHS 117 | /usr/lib 118 | /usr/local/lib 119 | /opt/local/lib 120 | /sw/lib 121 | ) 122 | 123 | IF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 124 | SET(CSPARSE_FOUND TRUE) 125 | ELSE(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 126 | SET(CSPARSE_FOUND FALSE) 127 | ENDIF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 128 | -------------------------------------------------------------------------------- /g2o/Thirdparty/g2o/config.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_CONFIG_H 2 | #define G2O_CONFIG_H 3 | 4 | /* #undef G2O_OPENMP */ 5 | /* #undef G2O_SHARED_LIBS */ 6 | 7 | // give a warning if Eigen defaults to row-major matrices. 8 | // We internally assume column-major matrices throughout the code. 9 | #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR 10 | # error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)" 11 | #endif 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /g2o/Thirdparty/g2o/config.h.in: -------------------------------------------------------------------------------- 1 | #ifndef G2O_CONFIG_H 2 | #define G2O_CONFIG_H 3 | 4 | #cmakedefine G2O_OPENMP 1 5 | #cmakedefine G2O_SHARED_LIBS 1 6 | 7 | // give a warning if Eigen defaults to row-major matrices. 8 | // We internally assume column-major matrices throughout the code. 9 | #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR 10 | # error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)" 11 | #endif 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /g2o/Thirdparty/g2o/g2o/core/base_binary_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_BINARY_EDGE_H 28 | #define G2O_BASE_BINARY_EDGE_H 29 | 30 | #include 31 | #include 32 | 33 | #include "base_edge.h" 34 | #include "robust_kernel.h" 35 | #include "../../config.h" 36 | 37 | namespace g2o { 38 | 39 | using namespace Eigen; 40 | 41 | template 42 | class BaseBinaryEdge : public BaseEdge 43 | { 44 | public: 45 | 46 | typedef VertexXi VertexXiType; 47 | typedef VertexXj VertexXjType; 48 | 49 | static const int Di = VertexXiType::Dimension; 50 | static const int Dj = VertexXjType::Dimension; 51 | 52 | static const int Dimension = BaseEdge::Dimension; 53 | typedef typename BaseEdge::Measurement Measurement; 54 | typedef typename Matrix::AlignedMapType JacobianXiOplusType; 55 | typedef typename Matrix::AlignedMapType JacobianXjOplusType; 56 | typedef typename BaseEdge::ErrorVector ErrorVector; 57 | typedef typename BaseEdge::InformationType InformationType; 58 | 59 | typedef Eigen::Map, Matrix::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType; 60 | typedef Eigen::Map, Matrix::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockTransposedType; 61 | 62 | BaseBinaryEdge() : BaseEdge(), 63 | _hessianRowMajor(false), 64 | _hessian(0, VertexXiType::Dimension, VertexXjType::Dimension), // HACK we map to the null pointer for initializing the Maps 65 | _hessianTransposed(0, VertexXjType::Dimension, VertexXiType::Dimension), 66 | _jacobianOplusXi(0, D, Di), _jacobianOplusXj(0, D, Dj) 67 | { 68 | _vertices.resize(2); 69 | } 70 | 71 | virtual OptimizableGraph::Vertex* createFrom(); 72 | virtual OptimizableGraph::Vertex* createTo(); 73 | 74 | virtual void resize(size_t size); 75 | 76 | virtual bool allVerticesFixed() const; 77 | 78 | virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace); 79 | 80 | /** 81 | * Linearizes the oplus operator in the vertex, and stores 82 | * the result in temporary variables _jacobianOplusXi and _jacobianOplusXj 83 | */ 84 | virtual void linearizeOplus(); 85 | 86 | //! returns the result of the linearization in the manifold space for the node xi 87 | const JacobianXiOplusType& jacobianOplusXi() const { return _jacobianOplusXi;} 88 | //! returns the result of the linearization in the manifold space for the node xj 89 | const JacobianXjOplusType& jacobianOplusXj() const { return _jacobianOplusXj;} 90 | 91 | virtual void constructQuadraticForm() ; 92 | 93 | virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor); 94 | 95 | using BaseEdge::resize; 96 | using BaseEdge::computeError; 97 | 98 | protected: 99 | using BaseEdge::_measurement; 100 | using BaseEdge::_information; 101 | using BaseEdge::_error; 102 | using BaseEdge::_vertices; 103 | using BaseEdge::_dimension; 104 | 105 | bool _hessianRowMajor; 106 | HessianBlockType _hessian; 107 | HessianBlockTransposedType _hessianTransposed; 108 | JacobianXiOplusType _jacobianOplusXi; 109 | JacobianXjOplusType _jacobianOplusXj; 110 | 111 | public: 112 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 113 | }; 114 | 115 | #include "base_binary_edge.hpp" 116 | 117 | } // end namespace g2o 118 | 119 | #endif 120 | -------------------------------------------------------------------------------- /g2o/Thirdparty/g2o/g2o/core/base_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_EDGE_H 28 | #define G2O_BASE_EDGE_H 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | #include "optimizable_graph.h" 36 | 37 | namespace g2o { 38 | 39 | using namespace Eigen; 40 | 41 | template 42 | class BaseEdge : public OptimizableGraph::Edge 43 | { 44 | public: 45 | 46 | static const int Dimension = D; 47 | typedef E Measurement; 48 | typedef Matrix ErrorVector; 49 | typedef Matrix InformationType; 50 | 51 | BaseEdge() : OptimizableGraph::Edge() 52 | { 53 | _dimension = D; 54 | } 55 | 56 | virtual ~BaseEdge() {} 57 | 58 | virtual double chi2() const 59 | { 60 | return _error.dot(information()*_error); 61 | } 62 | 63 | virtual const double* errorData() const { return _error.data();} 64 | virtual double* errorData() { return _error.data();} 65 | const ErrorVector& error() const { return _error;} 66 | ErrorVector& error() { return _error;} 67 | 68 | //! information matrix of the constraint 69 | const InformationType& information() const { return _information;} 70 | InformationType& information() { return _information;} 71 | void setInformation(const InformationType& information) { _information = information;} 72 | 73 | virtual const double* informationData() const { return _information.data();} 74 | virtual double* informationData() { return _information.data();} 75 | 76 | //! accessor functions for the measurement represented by the edge 77 | const Measurement& measurement() const { return _measurement;} 78 | virtual void setMeasurement(const Measurement& m) { _measurement = m;} 79 | 80 | virtual int rank() const {return _dimension;} 81 | 82 | virtual void initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*) 83 | { 84 | std::cerr << "inititialEstimate() is not implemented, please give implementation in your derived class" << std::endl; 85 | } 86 | 87 | protected: 88 | 89 | Measurement _measurement; 90 | InformationType _information; 91 | ErrorVector _error; 92 | 93 | /** 94 | * calculate the robust information matrix by updating the information matrix of the error 95 | */ 96 | InformationType robustInformation(const Eigen::Vector3d& rho) 97 | { 98 | InformationType result = rho[1] * _information; 99 | //ErrorVector weightedErrror = _information * _error; 100 | //result.noalias() += 2 * rho[2] * (weightedErrror * weightedErrror.transpose()); 101 | return result; 102 | } 103 | 104 | public: 105 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 106 | }; 107 | 108 | } // end namespace g2o 109 | 110 | #endif 111 | -------------------------------------------------------------------------------- /g2o/Thirdparty/g2o/g2o/core/base_multi_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_MULTI_EDGE_H 28 | #define G2O_BASE_MULTI_EDGE_H 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | #include "base_edge.h" 37 | #include "robust_kernel.h" 38 | #include "../../config.h" 39 | 40 | namespace g2o { 41 | 42 | using namespace Eigen; 43 | 44 | /** 45 | * \brief base class to represent an edge connecting an arbitrary number of nodes 46 | * 47 | * D - Dimension of the measurement 48 | * E - type to represent the measurement 49 | */ 50 | template 51 | class BaseMultiEdge : public BaseEdge 52 | { 53 | public: 54 | /** 55 | * \brief helper for mapping the Hessian memory of the upper triangular block 56 | */ 57 | struct HessianHelper { 58 | Eigen::Map matrix; ///< the mapped memory 59 | bool transposed; ///< the block has to be transposed 60 | HessianHelper() : matrix(0, 0, 0), transposed(false) {} 61 | }; 62 | 63 | public: 64 | static const int Dimension = BaseEdge::Dimension; 65 | typedef typename BaseEdge::Measurement Measurement; 66 | typedef MatrixXd::MapType JacobianType; 67 | typedef typename BaseEdge::ErrorVector ErrorVector; 68 | typedef typename BaseEdge::InformationType InformationType; 69 | typedef Eigen::Map HessianBlockType; 70 | 71 | BaseMultiEdge() : BaseEdge() 72 | { 73 | } 74 | 75 | virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace); 76 | 77 | /** 78 | * Linearizes the oplus operator in the vertex, and stores 79 | * the result in temporary variable vector _jacobianOplus 80 | */ 81 | virtual void linearizeOplus(); 82 | 83 | virtual void resize(size_t size); 84 | 85 | virtual bool allVerticesFixed() const; 86 | 87 | virtual void constructQuadraticForm() ; 88 | 89 | virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor); 90 | 91 | using BaseEdge::computeError; 92 | 93 | protected: 94 | using BaseEdge::_measurement; 95 | using BaseEdge::_information; 96 | using BaseEdge::_error; 97 | using BaseEdge::_vertices; 98 | using BaseEdge::_dimension; 99 | 100 | std::vector _hessian; 101 | std::vector > _jacobianOplus; ///< jacobians of the edge (w.r.t. oplus) 102 | 103 | void computeQuadraticForm(const InformationType& omega, const ErrorVector& weightedError); 104 | 105 | public: 106 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 107 | }; 108 | 109 | #include "base_multi_edge.hpp" 110 | 111 | } // end namespace g2o 112 | 113 | #endif 114 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/Thirdparty/g2o/g2o/core/base_vertex.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_BASE_VERTEX_H 28 | #define G2O_BASE_VERTEX_H 29 | 30 | #include "optimizable_graph.h" 31 | #include "creators.h" 32 | #include "../stuff/macros.h" 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | namespace g2o { 41 | 42 | using namespace Eigen; 43 | 44 | 45 | /** 46 | * \brief Templatized BaseVertex 47 | * 48 | * Templatized BaseVertex 49 | * D : minimal dimension of the vertex, e.g., 3 for rotation in 3D 50 | * T : internal type to represent the estimate, e.g., Quaternion for rotation in 3D 51 | */ 52 | template 53 | class BaseVertex : public OptimizableGraph::Vertex { 54 | public: 55 | typedef T EstimateType; 56 | typedef std::stack > > 58 | BackupStackType; 59 | 60 | static const int Dimension = D; ///< dimension of the estimate (minimal) in the manifold space 61 | 62 | typedef Eigen::Map, Matrix::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType; 63 | 64 | public: 65 | BaseVertex(); 66 | 67 | virtual const double& hessian(int i, int j) const { assert(i(_hessian.data());} 71 | 72 | virtual void mapHessianMemory(double* d); 73 | 74 | virtual int copyB(double* b_) const { 75 | memcpy(b_, _b.data(), Dimension * sizeof(double)); 76 | return Dimension; 77 | } 78 | 79 | virtual const double& b(int i) const { assert(i < D); return _b(i);} 80 | virtual double& b(int i) { assert(i < D); return _b(i);} 81 | virtual double* bData() { return _b.data();} 82 | 83 | virtual void clearQuadraticForm(); 84 | 85 | //! updates the current vertex with the direct solution x += H_ii\b_ii 86 | //! @returns the determinant of the inverted hessian 87 | virtual double solveDirect(double lambda=0); 88 | 89 | //! return right hand side b of the constructed linear system 90 | Matrix& b() { return _b;} 91 | const Matrix& b() const { return _b;} 92 | //! return the hessian block associated with the vertex 93 | HessianBlockType& A() { return _hessian;} 94 | const HessianBlockType& A() const { return _hessian;} 95 | 96 | virtual void push() { _backup.push(_estimate);} 97 | virtual void pop() { assert(!_backup.empty()); _estimate = _backup.top(); _backup.pop(); updateCache();} 98 | virtual void discardTop() { assert(!_backup.empty()); _backup.pop();} 99 | virtual int stackSize() const {return _backup.size();} 100 | 101 | //! return the current estimate of the vertex 102 | const EstimateType& estimate() const { return _estimate;} 103 | //! set the estimate for the vertex also calls updateCache() 104 | void setEstimate(const EstimateType& et) { _estimate = et; updateCache();} 105 | 106 | protected: 107 | HessianBlockType _hessian; 108 | Matrix _b; 109 | EstimateType _estimate; 110 | BackupStackType _backup; 111 | public: 112 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 113 | }; 114 | 115 | #include "base_vertex.hpp" 116 | 117 | } // end namespace g2o 118 | 119 | 120 | #endif 121 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/Thirdparty/g2o/g2o/core/eigen_types.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_EIGEN_TYPES_H 28 | #define G2O_EIGEN_TYPES_H 29 | 30 | #include 31 | #include 32 | 33 | namespace g2o { 34 | 35 | typedef Eigen::Matrix Vector2I; 36 | typedef Eigen::Matrix Vector3I; 37 | typedef Eigen::Matrix Vector4I; 38 | typedef Eigen::Matrix VectorXI; 39 | 40 | typedef Eigen::Matrix Vector2F; 41 | typedef Eigen::Matrix Vector3F; 42 | typedef Eigen::Matrix Vector4F; 43 | typedef Eigen::Matrix VectorXF; 44 | 45 | typedef Eigen::Matrix Vector2D; 46 | typedef Eigen::Matrix Vector3D; 47 | typedef Eigen::Matrix Vector4D; 48 | typedef Eigen::Matrix VectorXD; 49 | 50 | typedef Eigen::Matrix Matrix2I; 51 | typedef Eigen::Matrix Matrix3I; 52 | typedef Eigen::Matrix Matrix4I; 53 | typedef Eigen::Matrix MatrixXI; 54 | 55 | typedef Eigen::Matrix Matrix2F; 56 | typedef Eigen::Matrix Matrix3F; 57 | typedef Eigen::Matrix Matrix4F; 58 | typedef Eigen::Matrix MatrixXF; 59 | 60 | typedef Eigen::Matrix Matrix2D; 61 | typedef Eigen::Matrix Matrix3D; 62 | typedef Eigen::Matrix Matrix4D; 63 | typedef Eigen::Matrix MatrixXD; 64 | 65 | typedef Eigen::Transform Isometry2D; 66 | typedef Eigen::Transform Isometry3D; 67 | 68 | typedef Eigen::Transform Affine2D; 69 | typedef Eigen::Transform Affine3D; 70 | 71 | } // end namespace g2o 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /g2o/Thirdparty/g2o/g2o/core/hyper_dijkstra.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_AIS_GENERAL_DIJKSTRA_HH 28 | #define G2O_AIS_GENERAL_DIJKSTRA_HH 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "hyper_graph.h" 35 | 36 | namespace g2o{ 37 | 38 | struct HyperDijkstra{ 39 | struct CostFunction { 40 | virtual double operator() (HyperGraph::Edge* e, HyperGraph::Vertex* from, HyperGraph::Vertex* to)=0; 41 | virtual ~CostFunction() { } 42 | }; 43 | 44 | struct TreeAction { 45 | virtual double perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e); 46 | virtual double perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e, double distance); 47 | }; 48 | 49 | 50 | struct AdjacencyMapEntry{ 51 | friend struct HyperDijkstra; 52 | AdjacencyMapEntry(HyperGraph::Vertex* _child=0, 53 | HyperGraph::Vertex* _parent=0, 54 | HyperGraph::Edge* _edge=0, 55 | double _distance=std::numeric_limits::max()); 56 | HyperGraph::Vertex* child() const {return _child;} 57 | HyperGraph::Vertex* parent() const {return _parent;} 58 | HyperGraph::Edge* edge() const {return _edge;} 59 | double distance() const {return _distance;} 60 | HyperGraph::VertexSet& children() {return _children;} 61 | const HyperGraph::VertexSet& children() const {return _children;} 62 | protected: 63 | HyperGraph::Vertex* _child; 64 | HyperGraph::Vertex* _parent; 65 | HyperGraph::Edge* _edge; 66 | double _distance; 67 | HyperGraph::VertexSet _children; 68 | }; 69 | 70 | typedef std::map AdjacencyMap; 71 | HyperDijkstra(HyperGraph* g); 72 | HyperGraph::VertexSet& visited() {return _visited; } 73 | AdjacencyMap& adjacencyMap() {return _adjacencyMap; } 74 | HyperGraph* graph() {return _graph;} 75 | 76 | void shortestPaths(HyperGraph::Vertex* v, 77 | HyperDijkstra::CostFunction* cost, 78 | double maxDistance=std::numeric_limits< double >::max(), 79 | double comparisonConditioner=1e-3, 80 | bool directed=false, 81 | double maxEdgeCost=std::numeric_limits< double >::max()); 82 | 83 | void shortestPaths(HyperGraph::VertexSet& vset, 84 | HyperDijkstra::CostFunction* cost, 85 | double maxDistance=std::numeric_limits< double >::max(), 86 | double comparisonConditioner=1e-3, 87 | bool directed=false, 88 | double maxEdgeCost=std::numeric_limits< double >::max()); 89 | 90 | 91 | static void computeTree(AdjacencyMap& amap); 92 | static void visitAdjacencyMap(AdjacencyMap& amap, TreeAction* action, bool useDistance=false); 93 | static void connectedSubset(HyperGraph::VertexSet& connected, HyperGraph::VertexSet& visited, 94 | HyperGraph::VertexSet& startingSet, 95 | HyperGraph* g, HyperGraph::Vertex* v, 96 | HyperDijkstra::CostFunction* cost, double distance, double comparisonConditioner, 97 | double maxEdgeCost=std::numeric_limits< double >::max() ); 98 | 99 | protected: 100 | void reset(); 101 | 102 | AdjacencyMap _adjacencyMap; 103 | HyperGraph::VertexSet _visited; 104 | HyperGraph* _graph; 105 | }; 106 | 107 | struct UniformCostFunction: public HyperDijkstra::CostFunction { 108 | virtual double operator ()(HyperGraph::Edge* edge, HyperGraph::Vertex* from, HyperGraph::Vertex* to); 109 | }; 110 | 111 | } 112 | #endif 113 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/Thirdparty/g2o/g2o/core/linear_solver.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_LINEAR_SOLVER_H 28 | #define G2O_LINEAR_SOLVER_H 29 | #include "sparse_block_matrix.h" 30 | #include "sparse_block_matrix_ccs.h" 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief basic solver for Ax = b 36 | * 37 | * basic solver for Ax = b which has to reimplemented for different linear algebra libraries. 38 | * A is assumed to be symmetric (only upper triangular block is stored) and positive-semi-definit. 39 | */ 40 | template 41 | class LinearSolver 42 | { 43 | public: 44 | LinearSolver() {}; 45 | virtual ~LinearSolver() {} 46 | 47 | /** 48 | * init for operating on matrices with a different non-zero pattern like before 49 | */ 50 | virtual bool init() = 0; 51 | 52 | /** 53 | * Assumes that A is the same matrix for several calls. 54 | * Among other assumptions, the non-zero pattern does not change! 55 | * If the matrix changes call init() before. 56 | * solve system Ax = b, x and b have to allocated beforehand!! 57 | */ 58 | virtual bool solve(const SparseBlockMatrix& A, double* x, double* b) = 0; 59 | 60 | /** 61 | * Inverts the diagonal blocks of A 62 | * @returns false if not defined. 63 | */ 64 | virtual bool solveBlocks(double**&blocks, const SparseBlockMatrix& A) { (void)blocks; (void) A; return false; } 65 | 66 | 67 | /** 68 | * Inverts the a block pattern of A in spinv 69 | * @returns false if not defined. 70 | */ 71 | virtual bool solvePattern(SparseBlockMatrix& spinv, const std::vector >& blockIndices, const SparseBlockMatrix& A){ 72 | (void) spinv; 73 | (void) blockIndices; 74 | (void) A; 75 | return false; 76 | } 77 | 78 | //! write a debug dump of the system matrix if it is not PSD in solve 79 | virtual bool writeDebug() const { return false;} 80 | virtual void setWriteDebug(bool) {} 81 | }; 82 | 83 | /** 84 | * \brief Solver with faster iterating structure for the linear matrix 85 | */ 86 | template 87 | class LinearSolverCCS : public LinearSolver 88 | { 89 | public: 90 | LinearSolverCCS() : LinearSolver(), _ccsMatrix(0) {} 91 | ~LinearSolverCCS() 92 | { 93 | delete _ccsMatrix; 94 | } 95 | 96 | protected: 97 | SparseBlockMatrixCCS* _ccsMatrix; 98 | 99 | void initMatrixStructure(const SparseBlockMatrix& A) 100 | { 101 | delete _ccsMatrix; 102 | _ccsMatrix = new SparseBlockMatrixCCS(A.rowBlockIndices(), A.colBlockIndices()); 103 | A.fillSparseBlockMatrixCCS(*_ccsMatrix); 104 | } 105 | }; 106 | 107 | } // end namespace 108 | 109 | #endif 110 | -------------------------------------------------------------------------------- /g2o/Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.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_MARGINAL_COVARIANCE_CHOLESKY_H 28 | #define G2O_MARGINAL_COVARIANCE_CHOLESKY_H 29 | 30 | #include "optimizable_graph.h" 31 | #include "sparse_block_matrix.h" 32 | 33 | #include 34 | #include 35 | 36 | #ifdef _MSC_VER 37 | #include 38 | #else 39 | #include 40 | #endif 41 | 42 | 43 | namespace g2o { 44 | 45 | /** 46 | * \brief computing the marginal covariance given a cholesky factor (lower triangle of the factor) 47 | */ 48 | class MarginalCovarianceCholesky { 49 | protected: 50 | /** 51 | * hash struct for storing the matrix elements needed to compute the covariance 52 | */ 53 | typedef std::tr1::unordered_map LookupMap; 54 | 55 | public: 56 | MarginalCovarianceCholesky(); 57 | ~MarginalCovarianceCholesky(); 58 | 59 | /** 60 | * compute the marginal cov for the given block indices, write the result to the covBlocks memory (which has to 61 | * be provided by the caller). 62 | */ 63 | void computeCovariance(double** covBlocks, const std::vector& blockIndices); 64 | 65 | 66 | /** 67 | * compute the marginal cov for the given block indices, write the result in spinv). 68 | */ 69 | void computeCovariance(SparseBlockMatrix& spinv, const std::vector& rowBlockIndices, const std::vector< std::pair >& blockIndices); 70 | 71 | 72 | /** 73 | * set the CCS representation of the cholesky factor along with the inverse permutation used to reduce the fill-in. 74 | * permInv might be 0, will then not permute the entries. 75 | * 76 | * The pointers provided by the user need to be still valid when calling computeCovariance(). The pointers 77 | * are owned by the caller, MarginalCovarianceCholesky does not free the pointers. 78 | */ 79 | void setCholeskyFactor(int n, int* Lp, int* Li, double* Lx, int* permInv); 80 | 81 | protected: 82 | // information about the cholesky factor (lower triangle) 83 | int _n; ///< L is an n X n matrix 84 | int* _Ap; ///< column pointer of the CCS storage 85 | int* _Ai; ///< row indices of the CCS storage 86 | double* _Ax; ///< values of the cholesky factor 87 | int* _perm; ///< permutation of the cholesky factor. Variable re-ordering for better fill-in 88 | 89 | LookupMap _map; ///< hash look up table for the already computed entries 90 | std::vector _diag; ///< cache 1 / H_ii to avoid recalculations 91 | 92 | //! compute the index used for hashing 93 | int computeIndex(int r, int c) const { /*assert(r <= c);*/ return r*_n + c;} 94 | /** 95 | * compute one entry in the covariance, r and c are values after applying the permutation, and upper triangular. 96 | * May issue recursive calls to itself to compute the missing values. 97 | */ 98 | double computeEntry(int r, int c); 99 | }; 100 | 101 | } 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/Thirdparty/g2o/g2o/core/optimization_algorithm.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_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_H 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "../stuff/property.h" 35 | 36 | #include "hyper_graph.h" 37 | #include "sparse_block_matrix.h" 38 | 39 | namespace g2o { 40 | 41 | class SparseOptimizer; 42 | 43 | /** 44 | * \brief Generic interface for a non-linear solver operating on a graph 45 | */ 46 | class OptimizationAlgorithm 47 | { 48 | public: 49 | enum SolverResult {Terminate=2, OK=1, Fail=-1}; 50 | OptimizationAlgorithm(); 51 | virtual ~OptimizationAlgorithm(); 52 | 53 | /** 54 | * initialize the solver, called once before the first call to solve() 55 | */ 56 | virtual bool init(bool online = false) = 0; 57 | 58 | /** 59 | * Solve one iteration. The SparseOptimizer running on-top will call this 60 | * for the given number of iterations. 61 | * @param iteration indicates the current iteration 62 | */ 63 | virtual SolverResult solve(int iteration, bool online = false) = 0; 64 | 65 | /** 66 | * computes the block diagonal elements of the pattern specified in the input 67 | * and stores them in given SparseBlockMatrix. 68 | * If your solver does not support computing the marginals, return false. 69 | */ 70 | virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices) = 0; 71 | 72 | /** 73 | * update the structures for online processing 74 | */ 75 | virtual bool updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges) = 0; 76 | 77 | /** 78 | * called by the optimizer if verbose. re-implement, if you want to print something 79 | */ 80 | virtual void printVerbose(std::ostream& os) const {(void) os;}; 81 | 82 | public: 83 | //! return the optimizer operating on 84 | const SparseOptimizer* optimizer() const { return _optimizer;} 85 | SparseOptimizer* optimizer() { return _optimizer;} 86 | 87 | /** 88 | * specify on which optimizer the solver should work on 89 | */ 90 | void setOptimizer(SparseOptimizer* optimizer); 91 | 92 | //! return the properties of the solver 93 | const PropertyMap& properties() const { return _properties;} 94 | 95 | /** 96 | * update the properties from a string, see PropertyMap::updateMapFromString() 97 | */ 98 | bool updatePropertiesFromString(const std::string& propString); 99 | 100 | /** 101 | * print the properties to a stream in a human readable fashion 102 | */ 103 | void printProperties(std::ostream& os) const; 104 | 105 | protected: 106 | SparseOptimizer* _optimizer; ///< the optimizer the solver is working on 107 | PropertyMap _properties; ///< the properties of your solver, use this to store the parameters of your solver 108 | 109 | private: 110 | // Disable the copy constructor and assignment operator 111 | OptimizationAlgorithm(const OptimizationAlgorithm&) { } 112 | OptimizationAlgorithm& operator= (const OptimizationAlgorithm&) { return *this; } 113 | }; 114 | 115 | } // end namespace 116 | 117 | #endif 118 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | // ADD BY wangjing 64 | double preChi2 = _optimizer->activeRobustChi2(); 65 | // END ADD 66 | 67 | if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure 68 | ok = _solver->buildStructure(); 69 | if (! ok) { 70 | cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl; 71 | return OptimizationAlgorithm::Fail; 72 | } 73 | } 74 | 75 | t=get_monotonic_time(); 76 | _solver->buildSystem(); 77 | if (globalStats) { 78 | globalStats->timeQuadraticForm = get_monotonic_time()-t; 79 | t=get_monotonic_time(); 80 | } 81 | 82 | ok = _solver->solve(); 83 | if (globalStats) { 84 | globalStats->timeLinearSolution = get_monotonic_time()-t; 85 | t=get_monotonic_time(); 86 | } 87 | 88 | _optimizer->update(_solver->x()); 89 | if (globalStats) { 90 | globalStats->timeUpdate = get_monotonic_time()-t; 91 | } 92 | 93 | // ADD BY wangjing 94 | _optimizer->computeActiveErrors(); 95 | double afterChi2 = _optimizer->activeRobustChi2(); 96 | 97 | if(fabs(preChi2 - afterChi2)<1e-3/*1e-6*/) 98 | return Terminate; 99 | // END ADD 100 | 101 | if (ok) 102 | return OK; 103 | else 104 | return Fail; 105 | } 106 | 107 | void OptimizationAlgorithmGaussNewton::printVerbose(std::ostream& os) const 108 | { 109 | os 110 | << "\t schur= " << _solver->schur(); 111 | } 112 | 113 | } // end namespace 114 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/Thirdparty/g2o/g2o/core/parameter_container.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_container.h" 28 | 29 | #include 30 | 31 | #include "factory.h" 32 | #include "parameter.h" 33 | 34 | #include "../stuff/macros.h" 35 | #include "../stuff/color_macros.h" 36 | #include "../stuff/string_tools.h" 37 | 38 | namespace g2o { 39 | 40 | using namespace std; 41 | 42 | ParameterContainer::ParameterContainer(bool isMainStorage_) : 43 | _isMainStorage(isMainStorage_) 44 | { 45 | } 46 | 47 | void ParameterContainer::clear() { 48 | if (!_isMainStorage) 49 | return; 50 | for (iterator it = begin(); it!=end(); it++){ 51 | delete it->second; 52 | } 53 | BaseClass::clear(); 54 | } 55 | 56 | ParameterContainer::~ParameterContainer(){ 57 | clear(); 58 | } 59 | 60 | bool ParameterContainer::addParameter(Parameter* p){ 61 | if (p->id()<0) 62 | return false; 63 | iterator it=find(p->id()); 64 | if (it!=end()) 65 | return false; 66 | insert(make_pair(p->id(), p)); 67 | return true; 68 | } 69 | 70 | Parameter* ParameterContainer::getParameter(int id) { 71 | iterator it=find(id); 72 | if (it==end()) 73 | return 0; 74 | return it->second; 75 | } 76 | 77 | Parameter* ParameterContainer::detachParameter(int id){ 78 | iterator it=find(id); 79 | if (it==end()) 80 | return 0; 81 | Parameter* p=it->second; 82 | erase(it); 83 | return p; 84 | } 85 | 86 | bool ParameterContainer::write(std::ostream& os) const{ 87 | Factory* factory = Factory::instance(); 88 | for (const_iterator it=begin(); it!=end(); it++){ 89 | os << factory->tag(it->second) << " "; 90 | os << it->second->id() << " "; 91 | it->second->write(os); 92 | os << endl; 93 | } 94 | return true; 95 | } 96 | 97 | bool ParameterContainer::read(std::istream& is, const std::map* _renamedTypesLookup){ 98 | stringstream currentLine; 99 | string token; 100 | 101 | Factory* factory = Factory::instance(); 102 | HyperGraph::GraphElemBitset elemBitset; 103 | elemBitset[HyperGraph::HGET_PARAMETER] = 1; 104 | 105 | while (1) { 106 | int bytesRead = readLine(is, currentLine); 107 | if (bytesRead == -1) 108 | break; 109 | currentLine >> token; 110 | if (bytesRead == 0 || token.size() == 0 || token[0] == '#') 111 | continue; 112 | if (_renamedTypesLookup && _renamedTypesLookup->size()>0){ 113 | map::const_iterator foundIt = _renamedTypesLookup->find(token); 114 | if (foundIt != _renamedTypesLookup->end()) { 115 | token = foundIt->second; 116 | } 117 | } 118 | 119 | HyperGraph::HyperGraphElement* element = factory->construct(token, elemBitset); 120 | if (! element) // not a parameter or otherwise unknown tag 121 | continue; 122 | assert(element->elementType() == HyperGraph::HGET_PARAMETER && "Should be a param"); 123 | 124 | Parameter* p = static_cast(element); 125 | int pid; 126 | currentLine >> pid; 127 | p->setId(pid); 128 | bool r = p->read(currentLine); 129 | if (! r) { 130 | cerr << __PRETTY_FUNCTION__ << ": Error reading data " << token << " for parameter " << pid << endl; 131 | delete p; 132 | } else { 133 | if (! addParameter(p) ){ 134 | cerr << __PRETTY_FUNCTION__ << ": Parameter of type:" << token << " id:" << pid << " already defined" << endl; 135 | } 136 | } 137 | } // while read line 138 | 139 | return true; 140 | } 141 | 142 | } // end namespace 143 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/Thirdparty/g2o/g2o/core/sparse_block_matrix_diagonal.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_BLOCK_MATRIX_DIAGONAL_H 28 | #define G2O_SPARSE_BLOCK_MATRIX_DIAGONAL_H 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "../../config.h" 35 | #include "matrix_operations.h" 36 | 37 | namespace g2o { 38 | 39 | /** 40 | * \brief Sparse matrix which uses blocks on the diagonal 41 | * 42 | * This class is used as a const view on a SparseBlockMatrix 43 | * which allows a faster iteration over the elements of the 44 | * matrix. 45 | */ 46 | template 47 | class SparseBlockMatrixDiagonal 48 | { 49 | public: 50 | //! this is the type of the elementary block, it is an Eigen::Matrix. 51 | typedef MatrixType SparseMatrixBlock; 52 | 53 | //! columns of the matrix 54 | int cols() const {return _blockIndices.size() ? _blockIndices.back() : 0;} 55 | //! rows of the matrix 56 | int rows() const {return _blockIndices.size() ? _blockIndices.back() : 0;} 57 | 58 | typedef std::vector > DiagonalVector; 59 | 60 | SparseBlockMatrixDiagonal(const std::vector& blockIndices) : 61 | _blockIndices(blockIndices) 62 | {} 63 | 64 | //! how many rows/cols does the block at block-row / block-column r has? 65 | inline int dimOfBlock(int r) const { return r ? _blockIndices[r] - _blockIndices[r-1] : _blockIndices[0] ; } 66 | 67 | //! where does the row /col at block-row / block-column r starts? 68 | inline int baseOfBlock(int r) const { return r ? _blockIndices[r-1] : 0 ; } 69 | 70 | //! the block matrices per block-column 71 | const DiagonalVector& diagonal() const { return _diagonal;} 72 | DiagonalVector& diagonal() { return _diagonal;} 73 | 74 | //! indices of the row blocks 75 | const std::vector& blockIndices() const { return _blockIndices;} 76 | 77 | void multiply(double*& dest, const double* src) const 78 | { 79 | int destSize=cols(); 80 | if (! dest) { 81 | dest=new double[destSize]; 82 | memset(dest,0, destSize*sizeof(double)); 83 | } 84 | 85 | // map the memory by Eigen 86 | Eigen::Map destVec(dest, destSize); 87 | Eigen::Map srcVec(src, rows()); 88 | 89 | # ifdef G2O_OPENMP 90 | # pragma omp parallel for default (shared) schedule(dynamic, 10) 91 | # endif 92 | for (int i=0; i < static_cast(_diagonal.size()); ++i){ 93 | int destOffset = baseOfBlock(i); 94 | int srcOffset = destOffset; 95 | const SparseMatrixBlock& A = _diagonal[i]; 96 | // destVec += *A.transpose() * srcVec (according to the sub-vector parts) 97 | internal::axpy(A, srcVec, srcOffset, destVec, destOffset); 98 | } 99 | } 100 | 101 | protected: 102 | const std::vector& _blockIndices; ///< vector of the indices of the blocks along the diagonal 103 | DiagonalVector _diagonal; 104 | }; 105 | 106 | } //end namespace 107 | 108 | #endif 109 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/Thirdparty/g2o/g2o/solvers/linear_solver_dense.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 H. Strasdat 3 | // Copyright (C) 2012 R. Kümmerle 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 | #ifndef G2O_LINEAR_SOLVER_DENSE_H 29 | #define G2O_LINEAR_SOLVER_DENSE_H 30 | 31 | #include "../core/linear_solver.h" 32 | #include "../core/batch_stats.h" 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | 40 | namespace g2o { 41 | 42 | /** 43 | * \brief linear solver using dense cholesky decomposition 44 | */ 45 | template 46 | class LinearSolverDense : public LinearSolver 47 | { 48 | public: 49 | LinearSolverDense() : 50 | LinearSolver(), 51 | _reset(true) 52 | { 53 | } 54 | 55 | virtual ~LinearSolverDense() 56 | { 57 | } 58 | 59 | virtual bool init() 60 | { 61 | _reset = true; 62 | return true; 63 | } 64 | 65 | bool solve(const SparseBlockMatrix& A, double* x, double* b) 66 | { 67 | int n = A.cols(); 68 | int m = A.cols(); 69 | 70 | Eigen::MatrixXd& H = _H; 71 | if (H.cols() != n) { 72 | H.resize(n, m); 73 | _reset = true; 74 | } 75 | if (_reset) { 76 | _reset = false; 77 | H.setZero(); 78 | } 79 | 80 | // copy the sparse block matrix into a dense matrix 81 | int c_idx = 0; 82 | for (size_t i = 0; i < A.blockCols().size(); ++i) { 83 | int c_size = A.colsOfBlock(i); 84 | assert(c_idx == A.colBaseOfBlock(i) && "mismatch in block indices"); 85 | 86 | const typename SparseBlockMatrix::IntBlockMap& col = A.blockCols()[i]; 87 | if (col.size() > 0) { 88 | typename SparseBlockMatrix::IntBlockMap::const_iterator it; 89 | for (it = col.begin(); it != col.end(); ++it) { 90 | int r_idx = A.rowBaseOfBlock(it->first); 91 | // only the upper triangular block is processed 92 | if (it->first <= (int)i) { 93 | int r_size = A.rowsOfBlock(it->first); 94 | H.block(r_idx, c_idx, r_size, c_size) = *(it->second); 95 | if (r_idx != c_idx) // write the lower triangular block 96 | H.block(c_idx, r_idx, c_size, r_size) = it->second->transpose(); 97 | } 98 | } 99 | } 100 | 101 | c_idx += c_size; 102 | } 103 | 104 | // solving via Cholesky decomposition 105 | Eigen::VectorXd::MapType xvec(x, m); 106 | Eigen::VectorXd::ConstMapType bvec(b, n); 107 | _cholesky.compute(H); 108 | if (_cholesky.isPositive()) { 109 | xvec = _cholesky.solve(bvec); 110 | return true; 111 | } 112 | return false; 113 | } 114 | 115 | protected: 116 | bool _reset; 117 | Eigen::MatrixXd _H; 118 | Eigen::LDLT _cholesky; 119 | 120 | }; 121 | 122 | 123 | }// end namespace 124 | 125 | #endif 126 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/Thirdparty/g2o/g2o/stuff/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_MACROS_H 28 | #define G2O_MACROS_H 29 | 30 | #ifndef DEG2RAD 31 | #define DEG2RAD(x) ((x) * 0.01745329251994329575) 32 | #endif 33 | 34 | #ifndef RAD2DEG 35 | #define RAD2DEG(x) ((x) * 57.29577951308232087721) 36 | #endif 37 | 38 | // GCC the one and only 39 | #if defined(__GNUC__) 40 | # define G2O_ATTRIBUTE_CONSTRUCTOR(func) \ 41 | static void func(void)__attribute__ ((constructor)); \ 42 | static void func(void) 43 | 44 | # define G2O_ATTRIBUTE_UNUSED __attribute__((unused)) 45 | # define G2O_ATTRIBUTE_FORMAT12 __attribute__ ((format (printf, 1, 2))) 46 | # define G2O_ATTRIBUTE_FORMAT23 __attribute__ ((format (printf, 2, 3))) 47 | # define G2O_ATTRIBUTE_WARNING(func) func __attribute__((warning)) 48 | # define G2O_ATTRIBUTE_DEPRECATED(func) func __attribute__((deprecated)) 49 | 50 | #ifdef ANDROID 51 | # define g2o_isnan(x) isnan(x) 52 | # define g2o_isinf(x) isinf(x) 53 | # define g2o_isfinite(x) isfinite(x) 54 | #else 55 | # define g2o_isnan(x) std::isnan(x) 56 | # define g2o_isinf(x) std::isinf(x) 57 | # define g2o_isfinite(x) std::isfinite(x) 58 | #endif 59 | 60 | // MSVC on Windows 61 | #elif defined _MSC_VER 62 | # define __PRETTY_FUNCTION__ __FUNCTION__ 63 | 64 | /** 65 | Modified by Mark Pupilli from: 66 | 67 | "Initializer/finalizer sample for MSVC and GCC. 68 | 2010 Joe Lowe. Released into the public domain." 69 | 70 | "For MSVC, places a ptr to the function in the user initializer section (.CRT$XCU), basically the same thing the compiler does for the constructor calls for static C++ objects. For GCC, uses a constructor attribute." 71 | 72 | (As posted on Stack OVerflow) 73 | */ 74 | # define G2O_ATTRIBUTE_CONSTRUCTOR(f) \ 75 | __pragma(section(".CRT$XCU",read)) \ 76 | static void __cdecl f(void); \ 77 | __declspec(allocate(".CRT$XCU")) void (__cdecl*f##_)(void) = f; \ 78 | static void __cdecl f(void) 79 | 80 | # define G2O_ATTRIBUTE_UNUSED 81 | # define G2O_ATTRIBUTE_FORMAT12 82 | # define G2O_ATTRIBUTE_FORMAT23 83 | # define G2O_ATTRIBUTE_WARNING(func) func 84 | # define G2O_ATTRIBUTE_DEPRECATED(func) func 85 | 86 | #include 87 | 88 | # define g2o_isnan(x) _isnan(x) 89 | # define g2o_isinf(x) (_finite(x) == 0) 90 | # define g2o_isfinite(x) (_finite(x) != 0) 91 | 92 | // unknown compiler 93 | #else 94 | # ifndef __PRETTY_FUNCTION__ 95 | # define __PRETTY_FUNCTION__ "" 96 | # endif 97 | # define G2O_ATTRIBUTE_CONSTRUCTOR(func) func 98 | # define G2O_ATTRIBUTE_UNUSED 99 | # define G2O_ATTRIBUTE_FORMAT12 100 | # define G2O_ATTRIBUTE_FORMAT23 101 | # define G2O_ATTRIBUTE_WARNING(func) func 102 | # define G2O_ATTRIBUTE_DEPRECATED(func) func 103 | 104 | #include 105 | #define g2o_isnan(x) isnan(x) 106 | #define g2o_isinf(x) isinf(x) 107 | #define g2o_isfinite(x) isfinite(x) 108 | 109 | #endif 110 | 111 | // some macros that are only useful for c++ 112 | #ifdef __cplusplus 113 | 114 | #define G2O_FSKIP_LINE(f) \ 115 | {char c=' ';while(c != '\n' && f.good() && !(f).eof()) (f).get(c);} 116 | 117 | #ifndef PVAR 118 | #define PVAR(s) \ 119 | #s << " = " << (s) << std::flush 120 | #endif 121 | 122 | #ifndef PVARA 123 | #define PVARA(s) \ 124 | #s << " = " << RAD2DEG(s) << "deg" << std::flush 125 | #endif 126 | 127 | #ifndef FIXED 128 | #define FIXED(s) \ 129 | std::fixed << s << std::resetiosflags(std::ios_base::fixed) 130 | #endif 131 | 132 | #endif // __cplusplus 133 | 134 | #endif 135 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/Thirdparty/g2o/g2o/stuff/timeutil.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 "timeutil.h" 28 | #include 29 | 30 | #ifdef _WINDOWS 31 | #include 32 | #include 33 | #endif 34 | 35 | #ifdef UNIX 36 | #include 37 | #endif 38 | 39 | namespace g2o { 40 | 41 | #ifdef _WINDOWS 42 | #if defined(_MSC_VER) || defined(_MSC_EXTENSIONS) 43 | #define DELTA_EPOCH_IN_MICROSECS 11644473600000000Ui64 44 | #else 45 | #define DELTA_EPOCH_IN_MICROSECS 11644473600000000ULL 46 | #endif 47 | 48 | struct timezone 49 | { 50 | int tz_minuteswest; /* minutes W of Greenwich */ 51 | int tz_dsttime; /* type of dst correction */ 52 | }; 53 | 54 | int gettimeofday(struct timeval *tv, struct timezone *tz) 55 | { 56 | // Define a structure to receive the current Windows filetime 57 | FILETIME ft; 58 | 59 | // Initialize the present time to 0 and the timezone to UTC 60 | unsigned __int64 tmpres = 0; 61 | static int tzflag = 0; 62 | 63 | if (NULL != tv) 64 | { 65 | GetSystemTimeAsFileTime(&ft); 66 | 67 | // The GetSystemTimeAsFileTime returns the number of 100 nanosecond 68 | // intervals since Jan 1, 1601 in a structure. Copy the high bits to 69 | // the 64 bit tmpres, shift it left by 32 then or in the low 32 bits. 70 | tmpres |= ft.dwHighDateTime; 71 | tmpres <<= 32; 72 | tmpres |= ft.dwLowDateTime; 73 | 74 | // Convert to microseconds by dividing by 10 75 | tmpres /= 10; 76 | 77 | // The Unix epoch starts on Jan 1 1970. Need to subtract the difference 78 | // in seconds from Jan 1 1601. 79 | tmpres -= DELTA_EPOCH_IN_MICROSECS; 80 | 81 | // Finally change microseconds to seconds and place in the seconds value. 82 | // The modulus picks up the microseconds. 83 | tv->tv_sec = (long)(tmpres / 1000000UL); 84 | tv->tv_usec = (long)(tmpres % 1000000UL); 85 | } 86 | 87 | if (NULL != tz) { 88 | if (!tzflag) { 89 | _tzset(); 90 | tzflag++; 91 | } 92 | 93 | long sec; 94 | int hours; 95 | _get_timezone(&sec); 96 | _get_daylight(&hours); 97 | 98 | // Adjust for the timezone west of Greenwich 99 | tz->tz_minuteswest = sec / 60; 100 | tz->tz_dsttime = hours; 101 | } 102 | 103 | return 0; 104 | } 105 | #endif 106 | 107 | ScopeTime::ScopeTime(const char* title) : _title(title), _startTime(get_monotonic_time()) {} 108 | 109 | ScopeTime::~ScopeTime() { 110 | std::cerr << _title<<" took "<<1000*(get_monotonic_time()-_startTime)<<"ms.\n"; 111 | } 112 | 113 | double get_monotonic_time() 114 | { 115 | #if (defined(_POSIX_TIMERS) && (_POSIX_TIMERS+0 >= 0) && defined(_POSIX_MONOTONIC_CLOCK)) 116 | struct timespec ts; 117 | clock_gettime(CLOCK_MONOTONIC, &ts); 118 | return ts.tv_sec + ts.tv_nsec*1e-9; 119 | #else 120 | return get_time(); 121 | #endif 122 | } 123 | 124 | } // end namespace 125 | -------------------------------------------------------------------------------- /g2o/Thirdparty/g2o/g2o/stuff/timeutil.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_TIMEUTIL_H 28 | #define G2O_TIMEUTIL_H 29 | 30 | #ifdef _WINDOWS 31 | #include 32 | #else 33 | #include 34 | #endif 35 | 36 | #include 37 | 38 | 39 | /** @addtogroup utils **/ 40 | // @{ 41 | 42 | /** \file timeutil.h 43 | * \brief utility functions for handling time related stuff 44 | */ 45 | 46 | /// Executes code, only if secs are gone since last exec. 47 | /// extended version, in which the current time is given, e.g., timestamp of IPC message 48 | #ifndef DO_EVERY_TS 49 | #define DO_EVERY_TS(secs, currentTime, code) \ 50 | if (1) {\ 51 | static double s_lastDone_ = (currentTime); \ 52 | double s_now_ = (currentTime); \ 53 | if (s_lastDone_ > s_now_) \ 54 | s_lastDone_ = s_now_; \ 55 | if (s_now_ - s_lastDone_ > (secs)) { \ 56 | code; \ 57 | s_lastDone_ = s_now_; \ 58 | }\ 59 | } else \ 60 | (void)0 61 | #endif 62 | 63 | /// Executes code, only if secs are gone since last exec. 64 | #ifndef DO_EVERY 65 | #define DO_EVERY(secs, code) DO_EVERY_TS(secs, g2o::get_time(), code) 66 | #endif 67 | 68 | #ifndef MEASURE_TIME 69 | #define MEASURE_TIME(text, code) \ 70 | if(1) { \ 71 | double _start_time_ = g2o::get_time(); \ 72 | code; \ 73 | fprintf(stderr, "%s took %f sec\n", text, g2o::get_time() - _start_time_); \ 74 | } else \ 75 | (void) 0 76 | #endif 77 | 78 | namespace g2o { 79 | 80 | #ifdef _WINDOWS 81 | typedef struct timeval { 82 | long tv_sec; 83 | long tv_usec; 84 | } timeval; 85 | int gettimeofday(struct timeval *tv, struct timezone *tz); 86 | #endif 87 | 88 | /** 89 | * return the current time in seconds since 1. Jan 1970 90 | */ 91 | inline double get_time() 92 | { 93 | struct timeval ts; 94 | gettimeofday(&ts,0); 95 | return ts.tv_sec + ts.tv_usec*1e-6; 96 | } 97 | 98 | /** 99 | * return a monotonic increasing time which basically does not need to 100 | * have a reference point. Consider this for measuring how long some 101 | * code fragments required to execute. 102 | * 103 | * On Linux we call clock_gettime() on other systems we currently 104 | * call get_time(). 105 | */ 106 | double get_monotonic_time(); 107 | 108 | /** 109 | * \brief Class to measure the time spent in a scope 110 | * 111 | * To use this class, e.g. to measure the time spent in a function, 112 | * just create and instance at the beginning of the function. 113 | */ 114 | class ScopeTime { 115 | public: 116 | ScopeTime(const char* title); 117 | ~ScopeTime(); 118 | private: 119 | std::string _title; 120 | double _startTime; 121 | }; 122 | 123 | } // end namespace 124 | 125 | #ifndef MEASURE_FUNCTION_TIME 126 | #define MEASURE_FUNCTION_TIME \ 127 | g2o::ScopeTime scopeTime(__PRETTY_FUNCTION__) 128 | #endif 129 | 130 | 131 | // @} 132 | #endif 133 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /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(SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig 79 | PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 80 | if (SUITESPARSECONFIG_LIBRARY) 81 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /g2o/include/IMU_Data.h: -------------------------------------------------------------------------------- 1 | #ifndef IMU_PREINTEGRATION_IMUDATA_H 2 | #define IMU_PREINTEGRATION_IMUDATA_H 3 | 4 | #include 5 | 6 | // IMU model can find in IMG/IMU_Modelling.png 7 | 8 | /** 9 | * IMU model https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model 10 | * the web IMU Parameter[sigma_g, sigma_gw, sigma_a, sigma_aw] 11 | * For EuRoc dataset, according to V1_01_easy/imu0/sensor.yaml 12 | * The params: 13 | * sigma_g: 1.6968e-4 rad / s / sqrt(Hz) n_g(gyroscope_noise_density) // 连续时间gyr白噪声 14 | * sigma_gw: 1.9393e-5 rad / s^2 / sqrt(Hz) n_bg(gyroscope_random_walk) // 连续时间gyr随机游走 15 | * sigma_a: 2.0e-3 m / s^2 / sqrt(Hz) na(accelerometer_noise_density) // 连续时间acc白噪声 16 | * sigma_aw: 3.0e-3 m / s^3 / sqrt(Hz) n_ba(accelerometer_random_walk) // 连续时间acc随机游走 17 | */ 18 | 19 | // 保存IMU传感器参数和原始数据 20 | class IMUData 21 | { 22 | 23 | public: 24 | // 成员变量中包含Eigen对象时，需要使用以下宏重载new保证指针对齐 25 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 26 | 27 | IMUData() = default; 28 | 29 | IMUData(const double &gx, const double &gy, const double &gz, 30 | const double &ax, const double &ay, const double &az, 31 | const double &t); 32 | 33 | public: 34 | // IMU原始数据 35 | Eigen::Vector3d gyr_; 36 | Eigen::Vector3d acc_; 37 | // 数据时间戳 38 | double timestamp_; 39 | 40 | 41 | /**IMU传感器参数，无需多次设置 **/ 42 | // IMU传感器连续高斯白噪声 43 | static double sigma_gyr_; 44 | static double sigma_acc_; 45 | 46 | // IMU传感器连续随机游走 47 | static double sigma_gyr_walk_; 48 | static double sigma_acc_walk_; 49 | 50 | // IMU传感器数据采集间隔 51 | static double delta_t; 52 | 53 | 54 | // 离散参数和连续参数对应关系见IMU模型介绍 55 | // IMU传感器的离散随机游走的平方 56 | static double gyr_noise_rw2_; 57 | static double acc_noise_rw2_; 58 | 59 | // IMU传感器的离散噪声的平方 60 | static double gyr_bias_rw2_; 61 | static double acc_bias_rw2_; 62 | 63 | 64 | // IMU传感器的离散高斯白噪声矩阵，认为三轴各向同性 65 | static Eigen::Matrix3d gyr_meas_cov_; 66 | static Eigen::Matrix3d acc_meas_cov_; 67 | 68 | // IMU传感器的离散随机游走矩阵，认为三轴各向同性 69 | static Eigen::Matrix3d gyr_bias_rw_cov_; 70 | static Eigen::Matrix3d acc_bias_rw_cov_; 71 | 72 | }; 73 | 74 | 75 | 76 | 77 | #endif //IMU_PREINTEGRATION_IMUDATA_H 78 | -------------------------------------------------------------------------------- /g2o/include/so3.h: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2011 Hauke Strasdat (Imperial College London) 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #ifndef SOPHUS_SO3_H 24 | #define SOPHUS_SO3_H 25 | 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | 32 | namespace Sophus 33 | { 34 | 35 | using namespace Eigen; 36 | const double SMALL_EPS = 1e-10; 37 | 38 | class SO3 39 | { 40 | public: 41 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 42 | 43 | // Jr, right jacobian of SO3 44 | static Matrix3d JacobianR(const Vector3d &w); 45 | 46 | // Jr inverse 47 | static Matrix3d JacobianRInv(const Vector3d &w); 48 | 49 | // Jl, left jacobian of SO3, Jl(x) = Jr(-x) 50 | static Matrix3d JacobianL(const Vector3d &w); 51 | 52 | // Jl inverse 53 | static Matrix3d JacobianLInv(const Vector3d &w); 54 | 55 | 56 | /*******************/ 57 | SO3(); 58 | 59 | SO3(const SO3 &other); 60 | 61 | explicit SO3(const Matrix3d &_R); 62 | 63 | 64 | explicit SO3(const Quaterniond &unit_quaternion); 65 | 66 | 67 | SO3(double rot_x, double rot_y, double rot_z); 68 | 69 | void operator=(const SO3 &so3); 70 | 71 | SO3 operator*(const SO3 &so3) const; 72 | 73 | void operator*=(const SO3 &so3); 74 | 75 | Vector3d operator*(const Vector3d &xyz) const; 76 | 77 | SO3 inverse() const; 78 | 79 | Matrix3d matrix() const; 80 | 81 | Matrix3d Adj() const; 82 | 83 | Matrix3d generator(int i); 84 | 85 | Vector3d log() const; 86 | 87 | static SO3 exp(const Vector3d &omega); 88 | 89 | static SO3 expAndTheta(const Vector3d &omega, double *theta); 90 | 91 | static Vector3d log(const SO3 &so3); 92 | 93 | static Vector3d logAndTheta(const SO3 &so3, double *theta); 94 | 95 | static Matrix3d hat(const Vector3d &omega); // 向量=>反对称矩阵 96 | 97 | static Vector3d vee(const Matrix3d &Omega); // 反对称矩阵=>向量 98 | 99 | static Vector3d lieBracket(const Vector3d &omega1, const Vector3d &omega2); 100 | 101 | static Matrix3d d_lieBracketab_by_d_a(const Vector3d &b); 102 | 103 | void setQuaternion(const Quaterniond &quaternion); 104 | 105 | const Quaterniond &unit_quaternion() const 106 | { 107 | return unit_quaternion_; 108 | } 109 | 110 | static const int DoF = 3; 111 | 112 | protected: 113 | 114 | Quaterniond unit_quaternion_; 115 | 116 | 117 | }; 118 | 119 | inline std::ostream &operator<<(std::ostream &out_str, const SO3 &so3) 120 | { 121 | out_str << so3.log().transpose() << std::endl; 122 | return out_str; 123 | 124 | } 125 | 126 | 127 | } 128 | 129 | #endif 130 | 131 | 132 | -------------------------------------------------------------------------------- /g2o/src/IMU_Data.cpp: -------------------------------------------------------------------------------- 1 | #include "IMU_Data.h" 2 | 3 | // 初始化static 成员变量 4 | // 设置IMU传感器参数，continue parameter（默认使用EUROC数据集） 5 | double IMUData::sigma_gyr_ = 1.6968e-4; 6 | double IMUData::sigma_acc_ = 2.0e-3; 7 | 8 | double IMUData::sigma_gyr_walk_ = 1.9393e-5; 9 | double IMUData::sigma_acc_walk_ = 3.0e-3; 10 | 11 | double IMUData::delta_t = 0.005; 12 | 13 | 14 | // 计算IMU离散传感器参数 15 | double IMUData::gyr_noise_rw2_ = IMUData::sigma_gyr_ * IMUData::sigma_gyr_ / IMUData::delta_t; 16 | double IMUData::acc_noise_rw2_ = IMUData::sigma_acc_ * IMUData::sigma_acc_ / IMUData::delta_t; 17 | 18 | Eigen::Matrix3d IMUData::gyr_meas_cov_ = Eigen::Matrix3d::Identity() * IMUData::gyr_noise_rw2_; 19 | Eigen::Matrix3d IMUData::acc_meas_cov_ = Eigen::Matrix3d::Identity() * IMUData::acc_noise_rw2_; 20 | 21 | double IMUData::gyr_bias_rw2_ = IMUData::sigma_gyr_walk_ * IMUData::sigma_gyr_walk_ * IMUData::delta_t; 22 | double IMUData::acc_bias_rw2_ = IMUData::sigma_acc_walk_ * IMUData::sigma_acc_walk_ * IMUData::delta_t; 23 | 24 | Eigen::Matrix3d IMUData::gyr_bias_rw_cov_ = Eigen::Matrix3d::Identity() * IMUData::gyr_bias_rw2_; 25 | Eigen::Matrix3d IMUData::acc_bias_rw_cov_ = Eigen::Matrix3d::Identity() * IMUData::acc_bias_rw2_; 26 | 27 | 28 | IMUData::IMUData(const double &gx, const double &gy, const double &gz, const double &ax, const double &ay, 29 | const double &az, const double &t) : gyr_(gx, gy, gz), acc_(ax, ay, az), timestamp_(t) 30 | { 31 | 32 | } 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /g2o/src/IMU_Preintegrator.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by mc on 19-5-8. 3 | // 4 | 5 | #include "IMU_Preintegrator.h" 6 | #include 7 | 8 | IMUPreintegrator::IMUPreintegrator() 9 | { 10 | delta_p_.setZero(); 11 | delta_v_.setZero(); 12 | delta_rot_.setIdentity(); 13 | 14 | jacobian_p_bias_gyr_.setZero(); 15 | jacobian_p_bias_acc_.setZero(); 16 | 17 | jacobian_v_bias_gyr_.setZero(); 18 | jacobian_v_bias_acc_.setZero(); 19 | 20 | jacobian_rot_bias_gyr_.setZero(); 21 | 22 | cov_p_v_rot_.setZero(); 23 | 24 | delta_time_ = 0.0; 25 | 26 | } 27 | 28 | 29 | 30 | IMUPreintegrator::IMUPreintegrator(const IMUPreintegrator &preintegrator) : 31 | delta_p_(preintegrator.delta_p_), delta_v_(preintegrator.delta_v_), delta_rot_(preintegrator.delta_rot_), 32 | jacobian_p_bias_gyr_(preintegrator.jacobian_p_bias_gyr_), 33 | jacobian_p_bias_acc_(preintegrator.jacobian_p_bias_acc_), 34 | jacobian_v_bias_gyr_(preintegrator.jacobian_v_bias_gyr_), 35 | jacobian_v_bias_acc_(preintegrator.jacobian_v_bias_acc_), 36 | jacobian_rot_bias_gyr_(preintegrator.jacobian_rot_bias_gyr_), 37 | cov_p_v_rot_(preintegrator.cov_p_v_rot_), delta_time_(preintegrator.delta_time_) 38 | { 39 | 40 | } 41 | 42 | 43 | 44 | void IMUPreintegrator::Update(const Eigen::Vector3d &omega, const Eigen::Vector3d &acc, const double &delta_t) 45 | { 46 | double dt2 = delta_t*delta_t; 47 | 48 | // 步骤1 计算IMU预积分旋转增量和Jacobian 49 | Eigen::Matrix3d dR = Sophus::SO3::exp(omega*delta_t).matrix(); 50 | Eigen::Matrix3d Jr_R = Sophus::SO3::JacobianR(omega*delta_t); 51 | 52 | 53 | // 步骤2 计算IMU预积分观测方程噪声误差的不确定度方差 54 | // 参考文献1补充材料的公式A.7-A.9, 文中误差项顺序为RVP, 代码中为PVR 55 | Eigen::Matrix3d I3x3; 56 | I3x3.setIdentity(); 57 | 58 | Matrix9d A; 59 | A.setIdentity(); 60 | // block大小是3x3，起始位置(6,6) 61 | A.block<3,3>(6,6) = dR.transpose(); 62 | 63 | A.block<3,3>(3,6) = -delta_rot_ * Sophus::SO3::hat(acc) * delta_t; 64 | 65 | A.block<3,3>(0,6) = -0.5 * delta_rot_ * Sophus::SO3::hat(acc) * dt2; 66 | 67 | A.block<3,3>(0,3) = I3x3 * delta_t; 68 | 69 | // 将论文中的B拆成两部分 70 | Eigen::Matrix Bg; 71 | Bg.setZero(); 72 | Bg.block<3,3>(6,0) = Jr_R * delta_t; 73 | 74 | Eigen::Matrix Ca; 75 | Ca.setZero(); 76 | Ca.block<3,3>(3,0) = delta_rot_ * delta_t; 77 | Ca.block<3,3>(0,0) = 0.5*delta_rot_*dt2; 78 | 79 | 80 | cov_p_v_rot_ = A * cov_p_v_rot_ * A.transpose() + 81 | Bg * IMUData::gyr_meas_cov_ * Bg.transpose() + 82 | Ca * IMUData::acc_meas_cov_ * Ca.transpose(); 83 | 84 | // 步骤3 计算Jacobian, 用于矫正bias更新后的IMU预积分测量值 85 | // 补充材料的A.20,计算为IMU帧间数据增量信息 86 | 87 | // 在补充材料公式基础上进行递推 88 | jacobian_p_bias_acc_ += jacobian_v_bias_acc_ * delta_t - 0.5 * delta_rot_ * dt2; 89 | jacobian_p_bias_gyr_ += jacobian_v_bias_gyr_ * delta_t - 0.5 * delta_rot_ * Sophus::SO3::hat(acc) * jacobian_rot_bias_gyr_ * dt2; 90 | 91 | jacobian_v_bias_acc_ += - delta_rot_ * delta_t; 92 | jacobian_v_bias_gyr_ += -delta_rot_ * Sophus::SO3::hat(acc) * jacobian_rot_bias_gyr_ * delta_t; 93 | 94 | jacobian_rot_bias_gyr_ = dR.transpose() * jacobian_rot_bias_gyr_ - Jr_R * delta_t; // 在补充材料公式基础上进行递推 95 | 96 | 97 | 98 | 99 | // 步骤4 计算IMU预积分测量值 100 | delta_p_ += delta_v_*delta_t + 0.5 * delta_rot_ * acc *dt2; 101 | delta_v_ += delta_rot_ * acc *delta_t; 102 | delta_rot_ = NormalizeRotationM(delta_rot_ * dR); 103 | delta_time_ += delta_t; 104 | 105 | 106 | } 107 | 108 | 109 | 110 | void IMUPreintegrator::reset() 111 | { 112 | delta_p_.setZero(); 113 | delta_v_.setZero(); 114 | delta_rot_.setZero(); 115 | 116 | jacobian_p_bias_gyr_.setZero(); 117 | jacobian_p_bias_acc_.setZero(); 118 | 119 | jacobian_v_bias_gyr_.setZero(); 120 | jacobian_p_bias_acc_.setZero(); 121 | 122 | jacobian_rot_bias_gyr_.setZero(); 123 | 124 | cov_p_v_rot_.setZero(); 125 | 126 | delta_time_ = 0.0; 127 | } 128 | 129 | 130 | 131 | -------------------------------------------------------------------------------- /g2o/src/main.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by mc on 19-5-9. 3 | // 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | 13 | void LoadEuRocIMUData(const std::string &strImuPath, std::vector &imu_datas); 14 | 15 | 16 | int main(int argc, char **argv) 17 | { 18 | 19 | std::vector imu_datas; 20 | 21 | std::string imu_file_path(argv[1]); 22 | LoadEuRocIMUData(imu_file_path, imu_datas); 23 | 24 | IMUPreintegrator IMUPreintegrator; 25 | 26 | std::ofstream output("../../../../log/new_pre.txt"); 27 | 28 | for(int i=0; i<20; ++i) 29 | { 30 | IMUData imu_data = imu_datas[i]; 31 | double dt = imu_datas[i+1].timestamp_ - imu_data.timestamp_; 32 | 33 | // 由于没有其他传感器融合，此处的IMU数据没有扣除bias 34 | IMUPreintegrator.Update(imu_data.gyr_, imu_data.acc_, dt); 35 | 36 | 37 | output << "Pre P " << i << std::endl; 38 | output << IMUPreintegrator.GetDeltaP() << std::endl; 39 | output << "Pre V " << i << std::endl; 40 | output << IMUPreintegrator.GetDeltaV() << std::endl; 41 | output << "Pre R " << i << std::endl; 42 | output << IMUPreintegrator.GetDeltaRot() << std::endl; 43 | output << "Pre Cov " << i << std::endl; 44 | output << IMUPreintegrator.GetCovPVRot() << std::endl; 45 | 46 | output << "Pre P jacobian gyr " << i << std::endl; 47 | output << IMUPreintegrator.GetPJacoBiasgyr() << std::endl; 48 | output << "Pre P jacobian acc " << i << std::endl; 49 | output << IMUPreintegrator.GetPJacoBiasacc() << std::endl; 50 | output << "Pre V jacobian gyr " << i << std::endl; 51 | output << IMUPreintegrator.GetVJacoBiasgyr() << std::endl; 52 | output << "Pre V jacobian acc " << i << std::endl; 53 | output << IMUPreintegrator.GetVJacoBiasacc() << std::endl; 54 | output << "Pre Rot jacobian acc " << i << std::endl; 55 | output << IMUPreintegrator.GetRotJacoBiasgyr() << std::endl; 56 | 57 | } 58 | 59 | 60 | return 0; 61 | 62 | } 63 | 64 | 65 | 66 | void LoadEuRocIMUData(const std::string &strImuPath, std::vector &imu_datas) 67 | { 68 | 69 | std::ifstream fImus; 70 | fImus.open(strImuPath.c_str()); 71 | imu_datas.reserve(30000); 72 | //int testcnt = 10; 73 | 74 | 75 | while (!fImus.eof()) 76 | { 77 | std::string s; 78 | getline(fImus, s); 79 | if (!s.empty()) 80 | { 81 | char c = s.at(0); 82 | if (c < '0' || c > '9') 83 | continue; 84 | 85 | std::stringstream ss; 86 | ss << s; 87 | double tmpd; 88 | int cnt = 0; 89 | double data[10]; // timestamp, wx,wy,wz, ax,ay,az 90 | while (ss >> tmpd) 91 | { 92 | data[cnt] = tmpd; 93 | cnt++; 94 | if (cnt == 7) 95 | break; 96 | if (ss.peek() == ',' || ss.peek() == ' ') 97 | ss.ignore(); 98 | } 99 | // data[0]是时间戳。 100 | data[0] *= 1e-9; 101 | IMUData imudata(data[1], data[2], data[3], 102 | data[4], data[5], data[6], data[0]); 103 | imu_datas.push_back(imudata); 104 | 105 | } 106 | } 107 | } --------------------------------------------------------------------------------