├── .gitattributes ├── .gitignore ├── CMakeLists.txt ├── Dependencies.md ├── Dockerfile ├── LICENSE-GPL.txt ├── LICENSE.txt ├── README.md ├── build.sh ├── cmake_modules ├── FindCSparse.cmake └── FindEigen3.cmake ├── command.txt ├── dependencies └── g2o │ ├── CMakeLists.txt │ ├── README.txt │ ├── cmake_modules │ ├── FindBLAS.cmake │ ├── FindCSparse.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 │ │ ├── g2o_core_api.h │ │ ├── hyper_dijkstra.cpp │ │ ├── hyper_dijkstra.h │ │ ├── hyper_graph.cpp │ │ ├── hyper_graph.h │ │ ├── hyper_graph_action.cpp │ │ ├── hyper_graph_action.h │ │ ├── jacobian_workspace.cpp │ │ ├── jacobian_workspace.h │ │ ├── linear_solver.h │ │ ├── marginal_covariance_cholesky.cpp │ │ ├── marginal_covariance_cholesky.h │ │ ├── matrix_operations.h │ │ ├── matrix_structure.cpp │ │ ├── matrix_structure.h │ │ ├── openmp_mutex.h │ │ ├── optimizable_graph.cpp │ │ ├── optimizable_graph.h │ │ ├── optimization_algorithm.cpp │ │ ├── optimization_algorithm.h │ │ ├── optimization_algorithm_dogleg.cpp │ │ ├── optimization_algorithm_dogleg.h │ │ ├── optimization_algorithm_factory.cpp │ │ ├── optimization_algorithm_factory.h │ │ ├── optimization_algorithm_gauss_newton.cpp │ │ ├── optimization_algorithm_gauss_newton.h │ │ ├── optimization_algorithm_levenberg.cpp │ │ ├── optimization_algorithm_levenberg.h │ │ ├── optimization_algorithm_property.h │ │ ├── optimization_algorithm_with_hessian.cpp │ │ ├── optimization_algorithm_with_hessian.h │ │ ├── parameter.cpp │ │ ├── parameter.h │ │ ├── parameter_container.cpp │ │ ├── parameter_container.h │ │ ├── robust_kernel.cpp │ │ ├── robust_kernel.h │ │ ├── robust_kernel_factory.cpp │ │ ├── robust_kernel_factory.h │ │ ├── robust_kernel_impl.cpp │ │ ├── robust_kernel_impl.h │ │ ├── solver.cpp │ │ ├── solver.h │ │ ├── sparse_block_matrix.h │ │ ├── sparse_block_matrix.hpp │ │ ├── sparse_block_matrix_ccs.h │ │ ├── sparse_block_matrix_diagonal.h │ │ ├── sparse_block_matrix_test.cpp │ │ ├── sparse_optimizer.cpp │ │ ├── sparse_optimizer.h │ │ ├── sparse_optimizer_terminate_action.cpp │ │ └── sparse_optimizer_terminate_action.h │ ├── solvers │ │ ├── csparse_extension.cpp │ │ ├── csparse_extension.h │ │ ├── csparse_helper.cpp │ │ ├── csparse_helper.h │ │ ├── g2o_csparse_api.h │ │ ├── g2o_csparse_extension_api.h │ │ ├── linear_solver_csparse.h │ │ ├── linear_solver_dense.h │ │ ├── linear_solver_eigen.h │ │ └── solver_csparse.cpp │ ├── stuff │ │ ├── color_macros.h │ │ ├── macros.h │ │ ├── misc.h │ │ ├── opengl_primitives.cpp │ │ ├── opengl_primitives.h │ │ ├── opengl_wrapper.h │ │ ├── os_specific.c │ │ ├── os_specific.h │ │ ├── property.cpp │ │ ├── property.h │ │ ├── string_tools.cpp │ │ ├── string_tools.h │ │ ├── timeutil.cpp │ │ └── timeutil.h │ └── types │ │ ├── dquat2mat.cpp │ │ ├── dquat2mat.h │ │ ├── dquat2mat_maxima_generated.cpp │ │ ├── edge_se3.cpp │ │ ├── edge_se3.h │ │ ├── edge_se3_offset.cpp │ │ ├── edge_se3_offset.h │ │ ├── edge_se3_pointxyz.cpp │ │ ├── edge_se3_pointxyz.h │ │ ├── edge_se3_prior.cpp │ │ ├── edge_se3_prior.h │ │ ├── edge_xyz_prior.cpp │ │ ├── edge_xyz_prior.h │ │ ├── eigen_types_new.h │ │ ├── g2o_types_slam3d_api.h │ │ ├── isometry3d_gradients.cpp │ │ ├── isometry3d_gradients.h │ │ ├── isometry3d_mappings.cpp │ │ ├── isometry3d_mappings.h │ │ ├── parameter_se3_offset.cpp │ │ ├── parameter_se3_offset.h │ │ ├── se3_ops.h │ │ ├── se3_ops.hpp │ │ ├── se3quat.h │ │ ├── sim3.h │ │ ├── types_dyn_slam3d.cpp │ │ ├── types_dyn_slam3d.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 │ │ ├── types_slam3d.cpp │ │ ├── types_slam3d.h │ │ ├── vertex_point_xy.cpp │ │ ├── vertex_point_xy.h │ │ ├── vertex_pointxyz.cpp │ │ ├── vertex_pointxyz.h │ │ ├── vertex_se3.cpp │ │ └── vertex_se3.h │ └── license-bsd.txt ├── example ├── kitti-0000-0013.yaml ├── kitti-0018-0020.yaml ├── omd.yaml └── vdo_slam.cc ├── include ├── Converter.h ├── Frame.h ├── Map.h ├── ORBextractor.h ├── Optimizer.h ├── System.h ├── Tracking.h └── cvplot │ ├── color.h │ ├── cvplot.h │ ├── figure.h │ ├── highgui.h │ └── window.h ├── sourcecode_guidance.md ├── src ├── Converter.cc ├── Frame.cc ├── Map.cc ├── ORBextractor.cc ├── Optimizer.cc ├── System.cc ├── Tracking.cc └── cvplot │ ├── color.cc │ ├── figure.cc │ ├── highgui.cc │ ├── internal.h │ └── window.cc └── tools ├── cube_segmentation.m ├── example.mask └── kitti_mask_sem2gt.cpp /.gitattributes: -------------------------------------------------------------------------------- 1 | *.tar.gz filter=lfs diff=lfs merge=lfs -text 2 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(ObjSLAM) 3 | 4 | IF(NOT CMAKE_BUILD_TYPE) 5 | SET(CMAKE_BUILD_TYPE Release) 6 | ENDIF() 7 | 8 | MESSAGE("Build type: " ${CMAKE_BUILD_TYPE}) 9 | 10 | # ====== start c++ compile ====== 11 | 12 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 13 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 14 | 15 | # Check C++11 or C++0x support 16 | include(CheckCXXCompilerFlag) 17 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 18 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 19 | if(COMPILER_SUPPORTS_CXX11) 20 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 21 | add_definitions(-DCOMPILEDWITHC11) 22 | message(STATUS "Using flag -std=c++11.") 23 | elseif(COMPILER_SUPPORTS_CXX0X) 24 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 25 | add_definitions(-DCOMPILEDWITHC0X) 26 | message(STATUS "Using flag -std=c++0x.") 27 | else() 28 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 29 | endif() 30 | 31 | LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) 32 | 33 | 34 | find_package(OpenCV 3.0 QUIET) 35 | if(NOT OpenCV_FOUND) 36 | find_package(OpenCV 2.4.3 QUIET) 37 | if(NOT OpenCV_FOUND) 38 | message(FATAL_ERROR "OpenCV > 2.4.3 not found.") 39 | endif() 40 | endif() 41 | 42 | find_package(Eigen3 3.1.0 REQUIRED) 43 | find_package(CSparse REQUIRED) 44 | 45 | include_directories( 46 | ${PROJECT_SOURCE_DIR} 47 | ${PROJECT_SOURCE_DIR}/include 48 | ${EIGEN3_INCLUDE_DIR} 49 | ${CSPARSE_INCLUDE_DIR} 50 | ) 51 | 52 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) 53 | 54 | add_library(${PROJECT_NAME} SHARED 55 | src/System.cc 56 | src/Tracking.cc 57 | src/ORBextractor.cc 58 | src/Converter.cc 59 | src/Map.cc 60 | src/Optimizer.cc 61 | src/Frame.cc 62 | 63 | src/cvplot/color.cc 64 | src/cvplot/figure.cc 65 | src/cvplot/highgui.cc 66 | src/cvplot/window.cc 67 | src/cvplot/internal.h 68 | ) 69 | 70 | target_link_libraries( 71 | ${PROJECT_NAME} 72 | ${OpenCV_LIBS} 73 | ${EIGEN3_LIBS} 74 | ${CSPARSE_LIBRARY} 75 | # for mac os (default) 76 | #${PROJECT_SOURCE_DIR}/dependencies/g2o/lib/libg2o.dylib 77 | # for linux (ubuntu) 78 | ${PROJECT_SOURCE_DIR}/dependencies/g2o/lib/libg2o.so 79 | ) 80 | 81 | 82 | # Build example 83 | 84 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/example) 85 | 86 | add_executable(vdo_slam 87 | example/vdo_slam.cc) 88 | target_link_libraries(vdo_slam ${PROJECT_NAME}) 89 | 90 | 91 | -------------------------------------------------------------------------------- /Dependencies.md: -------------------------------------------------------------------------------- 1 | ##List of Known Dependencies 2 | ###VDO-SLAM version 1.0 3 | 4 | In this document we list all the pieces of code included by VDO-SLAM and linked libraries which are not property of the authors of VDO-SLAM. 5 | 6 | 7 | #####Code in **src** and **include** folders 8 | 9 | * *ORBextractor.cc*. 10 | This is a modified version of orb.cpp of OpenCV library. The original code is BSD licensed. 11 | 12 | 13 | #####Code in ***dependencies*** folder 14 | 15 | * All code in **g2o** folder. 16 | This is a modified version of [g2o](https://github.com/RainerKuemmerle/g2o). All files included are BSD licensed. 17 | 18 | #####Library dependencies 19 | 20 | * **OpenCV**. 21 | BSD license. 22 | 23 | * **cvplot**. 24 | MIT license. 25 | 26 | * **Eigen3**. 27 | For versions greater than 3.1.1 is MPL2, earlier versions are LGPLv3. 28 | 29 | * **CSPARSE**. 30 | GNU Lesser General Public License. 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:16.04 2 | 3 | RUN mkdir /app 4 | WORKDIR /app 5 | 6 | RUN apt-get update \ 7 | && DEBIAN_FRONTEND=noninteractive apt-get install -y --no-install-recommends software-properties-common \ 8 | && add-apt-repository ppa:ubuntu-toolchain-r/test 9 | 10 | # Install GCC-9 11 | RUN apt-get update \ 12 | && DEBIAN_FRONTEND=noninteractive apt-get install -y --no-install-recommends \ 13 | build-essential \ 14 | gcc-9 \ 15 | g++-9 \ 16 | gcc-9-multilib \ 17 | g++-9-multilib \ 18 | xutils-dev \ 19 | patch \ 20 | git \ 21 | python3 \ 22 | python3-pip \ 23 | libpulse-dev \ 24 | && apt-get clean \ 25 | && rm -rf /var/lib/apt/lists/* 26 | 27 | RUN update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-9 50 \ 28 | && update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-9 50 29 | 30 | # Install OpenCV 3.4 31 | 32 | RUN apt-get update 33 | RUN apt-get install -y cmake git libgtk2.0-dev pkg-config libavcodec-dev \ 34 | libavformat-dev libswscale-dev python-dev python-numpy libtbb2 libtbb-dev \ 35 | libjpeg-dev libpng-dev libtiff-dev libjasper-dev libdc1394-22-dev unzip 36 | 37 | RUN apt-get install -y wget 38 | RUN apt-get install -y vim 39 | 40 | # get and build OpenCV 3.4 41 | RUN cd \ 42 | && wget https://github.com/opencv/opencv/archive/3.4.0.zip \ 43 | && unzip 3.4.0.zip \ 44 | && cd opencv-3.4.0 \ 45 | && mkdir build \ 46 | && cd build \ 47 | && cmake .. \ 48 | && make -j8 \ 49 | && make install \ 50 | && cd \ 51 | && rm 3.4.0.zip 52 | 53 | # install and build opencv_contrib 54 | RUN cd \ 55 | && wget https://github.com/opencv/opencv_contrib/archive/3.4.0.zip \ 56 | && unzip 3.4.0.zip \ 57 | && cd opencv-3.4.0/build \ 58 | && cmake -DOPENCV_EXTRA_MODULES_PATH=../../opencv_contrib-3.4.0/modules/ .. \ 59 | && make -j8 \ 60 | && make install \ 61 | && cd ../.. \ 62 | && rm 3.4.0.zip 63 | 64 | 65 | # Install Eigen3 66 | RUN apt install libeigen3-dev 67 | 68 | # install CSparse 69 | RUN DEBIAN_FRONTEND=noninteractive apt install -y libsuitesparse-dev 70 | 71 | COPY . . 72 | 73 | # Build the Library 74 | RUN chmod +x build.sh 75 | RUN ./build.sh 76 | 77 | CMD ["/bin/bash"] 78 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | VDO-SLAM is released under a GPLv3 license (see LICENSE-GPL.txt). 2 | Please see Dependencies.md for a list of all included code and library dependencies which are not property of the authors of VDO-SLAM. 3 | 4 | For a closed-source version of VDO-SLAM for commercial purposes, please contact the authors. 5 | 6 | If you use VDO-SLAM in an academic work, please cite the most relevant publication associated by visiting: 7 | https://github.com/halajun/VDO_SLAM 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /build.sh: -------------------------------------------------------------------------------- 1 | echo "Configuring and building g2o ..." 2 | 3 | cd dependencies/g2o 4 | 5 | mkdir build 6 | cd build 7 | cmake .. -DCMAKE_BUILD_TYPE=Release 8 | make -j 9 | 10 | cd ../../../ 11 | 12 | echo "Configuring and building VDO-SLAM ..." 13 | 14 | mkdir build 15 | cd build 16 | cmake .. -DCMAKE_BUILD_TYPE=Release 17 | make -j 18 | -------------------------------------------------------------------------------- /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 | PATH_SUFFIXES 14 | suitesparse 15 | ) 16 | 17 | find_library(CSPARSE_LIBRARY NAMES cxsparse libcxsparse 18 | PATHS 19 | /usr/lib 20 | /usr/local/lib 21 | /opt/local/lib 22 | /sw/lib 23 | ) 24 | 25 | include(FindPackageHandleStandardArgs) 26 | find_package_handle_standard_args(CSPARSE DEFAULT_MSG 27 | CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY) 28 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /command.txt: -------------------------------------------------------------------------------- 1 | 2 | ./example/vdo_slam /Users/steed/work/kitti_ms/0000-0013.yaml /Users/steed/work/kitti_ms/0003 3 | 4 | ./example/vdo_slam /Users/steed/work/omd/swinging_4_unconstrained/omd.yaml /Users/steed/work/omd/swinging_4_unconstrained 5 | 6 | ./example/vdo_slam example/kitti-0000-0013.yaml PATH_TO_KITTI_SEQUENCE_DATA_FOLDER 7 | 8 | ./example/vdo_slam example/omd.yaml PATH_TO_OMD_SEQUENCE_DATA_FOLDER 9 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | PATH_SUFFIXES 14 | suitesparse 15 | ) 16 | 17 | find_library(CSPARSE_LIBRARY NAMES cxsparse libcxsparse 18 | PATHS 19 | /usr/lib 20 | /usr/local/lib 21 | /opt/local/lib 22 | /sw/lib 23 | ) 24 | 25 | include(FindPackageHandleStandardArgs) 26 | find_package_handle_standard_args(CSPARSE DEFAULT_MSG 27 | CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY) 28 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | /* #undef G2O_SINGLE_PRECISION_MATH */ 14 | #ifdef G2O_SINGLE_PRECISION_MATH 15 | #define G2O_NUMBER_FORMAT_STR "%g" 16 | 17 | #ifdef __cplusplus 18 | using number_t = float; 19 | #else 20 | typedef float number_t; 21 | #endif 22 | #else 23 | #define G2O_NUMBER_FORMAT_STR "%lg" 24 | 25 | #ifdef __cplusplus 26 | using number_t = double; 27 | #else 28 | typedef double number_t; 29 | #endif 30 | #endif 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /dependencies/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 | #cmakedefine G2O_SINGLE_PRECISION_MATH 14 | #ifdef G2O_SINGLE_PRECISION_MATH 15 | #define G2O_NUMBER_FORMAT_STR "%g" 16 | 17 | #ifdef __cplusplus 18 | using number_t = float; 19 | #else 20 | typedef float number_t; 21 | #endif 22 | #else 23 | #define G2O_NUMBER_FORMAT_STR "%lg" 24 | 25 | #ifdef __cplusplus 26 | using number_t = double; 27 | #else 28 | typedef double number_t; 29 | #endif 30 | #endif 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/core/g2o_core_api.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Description: import/export macros for creating DLLS with Microsoft 3 | * compiler. Any exported function needs to be declared with the 4 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 5 | * for each DLL (arrrrrgh!!!) 6 | * 7 | * 17 Jan 2012 8 | * Email: pupilli@cs.bris.ac.uk 9 | ****************************************************************************/ 10 | #ifndef G2O_CORE_API_H 11 | #define G2O_CORE_API_H 12 | 13 | #include "../../config.h" 14 | 15 | #ifdef _MSC_VER 16 | // We are using a Microsoft compiler: 17 | #ifdef G2O_SHARED_LIBS 18 | #ifdef core_EXPORTS 19 | #define G2O_CORE_API __declspec(dllexport) 20 | #else 21 | #define G2O_CORE_API __declspec(dllimport) 22 | #endif 23 | #else 24 | #define G2O_CORE_API 25 | #endif 26 | 27 | #else 28 | // Not Microsoft compiler so set empty definition: 29 | #define G2O_CORE_API 30 | #endif 31 | 32 | #endif // G2O_CORE_API_H 33 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/core/optimization_algorithm_gauss_newton.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "optimization_algorithm_gauss_newton.h" 28 | 29 | #include 30 | 31 | #include "../stuff/timeutil.h" 32 | #include "../stuff/macros.h" 33 | 34 | #include "solver.h" 35 | #include "batch_stats.h" 36 | #include "sparse_optimizer.h" 37 | using namespace std; 38 | 39 | namespace g2o { 40 | 41 | OptimizationAlgorithmGaussNewton::OptimizationAlgorithmGaussNewton(Solver* solver) : 42 | OptimizationAlgorithmWithHessian(solver) 43 | { 44 | } 45 | 46 | OptimizationAlgorithmGaussNewton::~OptimizationAlgorithmGaussNewton() 47 | { 48 | } 49 | 50 | OptimizationAlgorithm::SolverResult OptimizationAlgorithmGaussNewton::solve(int iteration, bool online) 51 | { 52 | assert(_optimizer && "_optimizer not set"); 53 | assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph"); 54 | bool ok = true; 55 | 56 | //here so that correct component for max-mixtures can be computed before the build structure 57 | double t=get_monotonic_time(); 58 | _optimizer->computeActiveErrors(); 59 | G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats(); 60 | if (globalStats) { 61 | globalStats->timeResiduals = get_monotonic_time()-t; 62 | } 63 | 64 | if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure 65 | ok = _solver->buildStructure(); 66 | if (! ok) { 67 | cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl; 68 | return OptimizationAlgorithm::Fail; 69 | } 70 | } 71 | 72 | t=get_monotonic_time(); 73 | _solver->buildSystem(); 74 | if (globalStats) { 75 | globalStats->timeQuadraticForm = get_monotonic_time()-t; 76 | t=get_monotonic_time(); 77 | } 78 | 79 | ok = _solver->solve(); 80 | if (globalStats) { 81 | globalStats->timeLinearSolution = get_monotonic_time()-t; 82 | t=get_monotonic_time(); 83 | } 84 | 85 | _optimizer->update(_solver->x()); 86 | if (globalStats) { 87 | globalStats->timeUpdate = get_monotonic_time()-t; 88 | } 89 | if (ok) 90 | return OK; 91 | else 92 | return Fail; 93 | } 94 | 95 | void OptimizationAlgorithmGaussNewton::printVerbose(std::ostream& os) const 96 | { 97 | os 98 | << "\t schur= " << _solver->schur(); 99 | } 100 | 101 | } // end namespace 102 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | #if defined(_MSC_VER) || defined(__APPLE__) 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::shared_ptr RobustKernelPtr; 78 | 79 | } // end namespace g2o 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/core/sparse_optimizer_terminate_action.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 SPARSE_OPTIMIZER_TERMINATE_ACTION_H 28 | #define SPARSE_OPTIMIZER_TERMINATE_ACTION_H 29 | 30 | #include "g2o_core_api.h" 31 | #include "hyper_graph_action.h" 32 | 33 | namespace g2o { 34 | 35 | class SparseOptimizer; 36 | 37 | /** 38 | * \brief stop iterating based on the gain which is (oldChi - currentChi) / currentChi. 39 | * 40 | * stop iterating based on the gain which is (oldChi - currentChi) / currentChi. 41 | * If the gain is larger than zero and below the threshold, then the optimizer is stopped. 42 | * Typically usage of this action includes adding it as a postIteration action, by calling 43 | * addPostIterationAction on a sparse optimizer. 44 | */ 45 | class G2O_CORE_API SparseOptimizerTerminateAction : public HyperGraphAction 46 | { 47 | public: 48 | SparseOptimizerTerminateAction(); 49 | virtual HyperGraphAction* operator()(const HyperGraph* graph, Parameters* parameters = 0); 50 | 51 | number_t gainThreshold() const { return _gainThreshold;} 52 | void setGainThreshold(number_t gainThreshold); 53 | 54 | int maxIterations() const { return _maxIterations;} 55 | void setMaxIterations(int maxit); 56 | 57 | protected: 58 | number_t _gainThreshold; 59 | number_t _lastChi; 60 | bool _auxTerminateFlag; 61 | int _maxIterations; 62 | 63 | void setOptimizerStopFlag(const SparseOptimizer* optimizer, bool stop); 64 | }; 65 | 66 | } // end namespace 67 | 68 | #endif 69 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/solvers/csparse_extension.h: -------------------------------------------------------------------------------- 1 | // CSparse: a Concise Sparse matrix package. 2 | // Copyright (c) 2006, Timothy A. Davis. 3 | // http://www.cise.ufl.edu/research/sparse/CSparse 4 | // 5 | // -------------------------------------------------------------------------------- 6 | // 7 | // CSparse is free software; you can redistribute it and/or 8 | // modify it under the terms of the GNU Lesser General Public 9 | // License as published by the Free Software Foundation; either 10 | // version 2.1 of the License, or (at your option) any later version. 11 | // 12 | // CSparse is distributed in the hope that it will be useful, 13 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | // Lesser General Public License for more details. 16 | // 17 | // You should have received a copy of the GNU Lesser General Public 18 | // License along with this Module; if not, write to the Free Software 19 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 20 | 21 | #ifndef G2O_CSPARSE_EXTENSION_H 22 | #define G2O_CSPARSE_EXTENSION_H 23 | 24 | #ifndef NCOMPLEX 25 | #define NCOMPLEX 26 | #endif 27 | #include 28 | 29 | #include "g2o_csparse_extension_api.h" 30 | 31 | namespace g2o { 32 | 33 | namespace csparse_extension { 34 | 35 | // our extensions to csparse 36 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 37 | /** 38 | * Originally from CSparse, avoid memory re-allocations by giving workspace pointers 39 | * CSparse: Copyright (c) 2006-2011, Timothy A. Davis. 40 | */ 41 | G2O_CSPARSE_EXTENSION_API csn* cs_chol_workspace (const cs *A, const css *S, int* cin, number_t* xin); 42 | G2O_CSPARSE_EXTENSION_API int cs_cholsolsymb(const cs *A, number_t *b, const css* S, number_t* workspace, int* work); 43 | 44 | } // end namespace 45 | } // end namespace 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/solvers/csparse_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_CSPARSE_HELPER_H 28 | #define G2O_CSPARSE_HELPER_H 29 | 30 | #include "csparse_extension.h" 31 | 32 | #include "g2o_csparse_extension_api.h" 33 | 34 | namespace g2o { 35 | 36 | namespace csparse_extension { 37 | 38 | /** 39 | * write the sparse matrix to a file loadable with ocatve 40 | */ 41 | G2O_CSPARSE_EXTENSION_API bool writeCs2Octave(const char* filename, const cs* A, bool upperTriangular = true); 42 | 43 | } // end namespace 44 | } // end namespace 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/solvers/g2o_csparse_api.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 | /*************************************************************************** 28 | * Description: import/export macros for creating DLLS with Microsoft 29 | * compiler. Any exported function needs to be declared with the 30 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 31 | * for each DLL (arrrrrgh!!!) 32 | * 33 | * 17 Jan 2012 34 | * Email: pupilli@cs.bris.ac.uk 35 | ****************************************************************************/ 36 | #ifndef G2O_SOLVER_CSPARSE_API_H 37 | #define G2O_SOLVER_CSPARSE_API_H 38 | 39 | #include "../../config.h" 40 | 41 | #ifdef _MSC_VER 42 | // We are using a Microsoft compiler: 43 | #ifdef G2O_SHARED_LIBS 44 | #ifdef solver_csparse_EXPORTS 45 | #define G2O_SOLVER_CSPARSE_API __declspec(dllexport) 46 | #else 47 | #define G2O_SOLVER_CSPARSE_API __declspec(dllimport) 48 | #endif 49 | #else 50 | #define G2O_SOLVER_CSPARSE_API 51 | #endif 52 | 53 | #else 54 | // Not Microsoft compiler so set empty definition: 55 | #define G2O_SOLVER_CSPARSE_API 56 | #endif 57 | 58 | #endif // G2O_CSPARSE_API_H 59 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/solvers/g2o_csparse_extension_api.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2012 Rainer Kuemmerle 3 | // 4 | // g2o is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU Lesser General Public License as published 6 | // by the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // g2o is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU Lesser General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU Lesser General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef G2O_CSPARSE_EXTENSION_API_H 18 | #define G2O_CSPARSE_EXTENSION_API_H 19 | 20 | #include "../../config.h" 21 | 22 | #ifdef _MSC_VER 23 | // We are using a Microsoft compiler: 24 | #ifdef G2O_LGPL_SHARED_LIBS 25 | #ifdef csparse_extension_EXPORTS 26 | #define G2O_CSPARSE_EXTENSION_API __declspec(dllexport) 27 | #else 28 | #define G2O_CSPARSE_EXTENSION_API __declspec(dllimport) 29 | #endif 30 | #else 31 | #define G2O_CSPARSE_EXTENSION_API 32 | #endif 33 | 34 | #else 35 | // Not Microsoft compiler so set empty definition: 36 | #define G2O_CSPARSE_EXTENSION_API 37 | #endif 38 | 39 | #endif // G2O_CSPARSE_API_H 40 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/stuff/opengl_wrapper.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_OPENGL_WRAPPER_H 28 | #define G2O_OPENGL_WRAPPER_H 29 | 30 | #include "g2o/config.h" 31 | 32 | #ifdef WINDOWS 33 | #include 34 | #endif 35 | 36 | #ifdef G2O_OPENGL_FOUND 37 | # ifdef __APPLE__ 38 | # include 39 | # else 40 | # include 41 | # endif 42 | #endif 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/types/dquat2mat.cpp: -------------------------------------------------------------------------------- 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 | #include "dquat2mat.h" 28 | #include 29 | namespace g2o { 30 | namespace internal { 31 | using namespace std; 32 | 33 | #include "dquat2mat_maxima_generated.cpp" 34 | 35 | int _q2m(number_t& S, number_t& qw, const number_t& r00 , const number_t& r10 , const number_t& r20 , const number_t& r01 , const number_t& r11 , const number_t& r21 , const number_t& r02 , const number_t& r12 , const number_t& r22 ){ 36 | number_t tr=r00 + r11 + r22; 37 | if (tr > 0) { 38 | S = sqrt(tr + 1.0) * 2; // S=4*qw 39 | qw = 0.25 * S; 40 | // qx = (r21 - r12) / S; 41 | // qy = (r02 - r20) / S; 42 | // qz = (r10 - r01) / S; 43 | return 0; 44 | } else if ((r00 > r11)&(r00 > r22)) { 45 | S = sqrt(1.0 + r00 - r11 - r22) * 2; // S=4*qx 46 | qw = (r21 - r12) / S; 47 | // qx = 0.25 * S; 48 | // qy = (r01 + r10) / S; 49 | // qz = (r02 + r20) / S; 50 | return 1; 51 | } else if (r11 > r22) { 52 | S = sqrt(1.0 + r11 - r00 - r22) * 2; // S=4*qy 53 | qw = (r02 - r20) / S; 54 | // qx = (r01 + r10) / S; 55 | // qy = 0.25 * S; 56 | return 2; 57 | } else { 58 | S = sqrt(1.0 + r22 - r00 - r11) * 2; // S=4*qz 59 | qw = (r10 - r01) / S; 60 | // qx = (r02 + r20) / S; 61 | // qy = (r12 + r21) / S; 62 | // qz = 0.25 * S; 63 | return 3; 64 | } 65 | } 66 | 67 | void compute_dq_dR ( Eigen::Matrix& dq_dR , const number_t& r11 , const number_t& r21 , const number_t& r31 , const number_t& r12 , const number_t& r22 , const number_t& r32 , const number_t& r13 , const number_t& r23 , const number_t& r33 ){ 68 | number_t qw; 69 | number_t S; 70 | int whichCase=_q2m( S, qw, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 ); 71 | S*=.25; 72 | switch(whichCase){ 73 | case 0: compute_dq_dR_w(dq_dR, S, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 ); 74 | break; 75 | case 1: compute_dq_dR_x(dq_dR, S, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 ); 76 | break; 77 | case 2: compute_dq_dR_y(dq_dR, S, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 ); 78 | break; 79 | case 3: compute_dq_dR_z(dq_dR, S, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 ); 80 | break; 81 | } 82 | if (qw<=0) 83 | dq_dR *= -1; 84 | } 85 | } 86 | } 87 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/types/dquat2mat.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 _DQUAT2MAT_H_ 28 | #define _DQUAT2MAT_H_ 29 | #include 30 | #include "g2o_types_slam3d_api.h" 31 | 32 | namespace g2o { 33 | namespace internal { 34 | 35 | void G2O_TYPES_SLAM3D_API compute_dq_dR ( Eigen::Matrix& dq_dR , const number_t& r11 , const number_t& r21 , const number_t& r31 , const number_t& r12 , const number_t& r22 , const number_t& r32 , const number_t& r13 , const number_t& r23 , const number_t& r33 ); 36 | 37 | void G2O_TYPES_SLAM3D_API compute_dR_dq ( Eigen::Matrix& dR_dq , const number_t& qx , const number_t& qy , const number_t& qz , const number_t& qw ) ; 38 | } 39 | } 40 | #endif 41 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/types/edge_se3_offset.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_EDGE_SE3_OFFSET_H_ 28 | #define G2O_EDGE_SE3_OFFSET_H_ 29 | 30 | 31 | #include "edge_se3.h" 32 | #include "g2o_types_slam3d_api.h" 33 | 34 | namespace g2o { 35 | class ParameterSE3Offset; 36 | class CacheSE3Offset; 37 | 38 | /** 39 | * \brief Offset edge 40 | */ 41 | // first two args are the measurement type, second two the connection classes 42 | class G2O_TYPES_SLAM3D_API EdgeSE3Offset : public EdgeSE3 { 43 | public: 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 45 | EdgeSE3Offset(); 46 | virtual bool read(std::istream& is); 47 | virtual bool write(std::ostream& os) const; 48 | 49 | void computeError(); 50 | 51 | 52 | void linearizeOplus(); 53 | 54 | virtual bool setMeasurementFromState() ; 55 | 56 | virtual number_t initialEstimatePossible(const OptimizableGraph::VertexSet& /*from*/, 57 | OptimizableGraph::Vertex* /*to*/) { 58 | return 1.; 59 | } 60 | 61 | virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to); 62 | 63 | protected: 64 | virtual bool resolveCaches(); 65 | ParameterSE3Offset *_offsetFrom, *_offsetTo; 66 | CacheSE3Offset *_cacheFrom, *_cacheTo; 67 | }; 68 | 69 | } // end namespace 70 | #endif 71 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/types/edge_se3_pointxyz.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_EDGE_SE3_POINT_XYZ_H_ 28 | #define G2O_EDGE_SE3_POINT_XYZ_H_ 29 | 30 | #include "../core/base_binary_edge.h" 31 | 32 | #include "vertex_se3.h" 33 | #include "vertex_pointxyz.h" 34 | #include "parameter_se3_offset.h" 35 | #include "g2o_types_slam3d_api.h" 36 | 37 | namespace g2o { 38 | 39 | /*! \class EdgeSE3PointXYZ 40 | * \brief g2o edge from a track to a point node 41 | */ 42 | // first two args are the measurement type, second two the connection classes 43 | class G2O_TYPES_SLAM3D_API EdgeSE3PointXYZ : public BaseBinaryEdge<3, Vector3, VertexSE3, VertexPointXYZ> { 44 | public: 45 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 46 | EdgeSE3PointXYZ(); 47 | virtual bool read(std::istream& is); 48 | virtual bool write(std::ostream& os) const; 49 | 50 | // return the error estimate as a 3-vector 51 | void computeError(); 52 | // jacobian 53 | virtual void linearizeOplus(); 54 | 55 | 56 | virtual void setMeasurement(const Vector3& m){ 57 | _measurement = m; 58 | } 59 | 60 | virtual bool setMeasurementData(const number_t* d){ 61 | Eigen::Map v(d); 62 | _measurement = v; 63 | return true; 64 | } 65 | 66 | virtual bool getMeasurementData(number_t* d) const{ 67 | Eigen::Map v(d); 68 | v=_measurement; 69 | return true; 70 | } 71 | 72 | virtual int measurementDimension() const {return 3;} 73 | 74 | virtual bool setMeasurementFromState() ; 75 | 76 | virtual number_t initialEstimatePossible(const OptimizableGraph::VertexSet& from, 77 | OptimizableGraph::Vertex* to) { 78 | (void) to; 79 | return (from.count(_vertices[0]) == 1 ? 1.0 : -1.0); 80 | } 81 | 82 | virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to); 83 | 84 | const ParameterSE3Offset* offsetParameter() { return offsetParam; } 85 | private: 86 | Eigen::Matrix J; // jacobian before projection 87 | ParameterSE3Offset* offsetParam; 88 | CacheSE3Offset* cache; 89 | virtual bool resolveCaches(); 90 | }; 91 | 92 | #ifdef G2O_HAVE_OPENGL 93 | class EdgeSE3PointXYZDrawAction: public DrawAction{ 94 | public: 95 | EdgeSE3PointXYZDrawAction(); 96 | virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, 97 | HyperGraphElementAction::Parameters* params_); 98 | }; 99 | #endif 100 | 101 | } 102 | #endif 103 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/types/edge_se3_prior.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_EDGE_SE3_PRIOR_H_ 28 | #define G2O_EDGE_SE3_PRIOR_H_ 29 | 30 | #include "vertex_se3.h" 31 | #include "../core/base_unary_edge.h" 32 | #include "parameter_se3_offset.h" 33 | #include "g2o_types_slam3d_api.h" 34 | namespace g2o { 35 | /** 36 | * \brief prior for an SE3 element 37 | * 38 | * Provides a prior for a 3d pose vertex. Again the measurement is represented by an 39 | * Isometry3 matrix. 40 | */ 41 | class G2O_TYPES_SLAM3D_API EdgeSE3Prior : public BaseUnaryEdge<6, Isometry3, VertexSE3> { 42 | public: 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 44 | EdgeSE3Prior(); 45 | virtual bool read(std::istream& is); 46 | virtual bool write(std::ostream& os) const; 47 | 48 | // return the error estimate as a 3-vector 49 | void computeError(); 50 | 51 | // jacobian 52 | virtual void linearizeOplus(); 53 | 54 | virtual void setMeasurement(const Isometry3& m){ 55 | _measurement = m; 56 | _inverseMeasurement = m.inverse(); 57 | } 58 | 59 | virtual bool setMeasurementData(const number_t* d){ 60 | Eigen::Map v(d); 61 | _measurement = internal::fromVectorQT(v); 62 | _inverseMeasurement = _measurement.inverse(); 63 | return true; 64 | } 65 | 66 | virtual bool getMeasurementData(number_t* d) const{ 67 | Eigen::Map v(d); 68 | v = internal::toVectorQT(_measurement); 69 | return true; 70 | } 71 | 72 | virtual int measurementDimension() const {return 7;} 73 | 74 | virtual bool setMeasurementFromState() ; 75 | 76 | virtual number_t initialEstimatePossible(const OptimizableGraph::VertexSet& /*from*/, 77 | OptimizableGraph::Vertex* /*to*/) { 78 | return 1.; 79 | } 80 | 81 | virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to); 82 | protected: 83 | Isometry3 _inverseMeasurement; 84 | virtual bool resolveCaches(); 85 | ParameterSE3Offset* _offsetParam; 86 | CacheSE3Offset* _cache; 87 | }; 88 | 89 | } 90 | #endif 91 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/types/edge_xyz_prior.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 "edge_xyz_prior.h" 28 | #include 29 | 30 | namespace g2o { 31 | using namespace std; 32 | 33 | EdgeXYZPrior::EdgeXYZPrior() : BaseUnaryEdge<3, Vector3, VertexPointXYZ>() { 34 | information().setIdentity(); 35 | } 36 | 37 | bool EdgeXYZPrior::read(std::istream& is) { 38 | // read measurement 39 | Vector3 meas; 40 | for (int i=0; i<3; i++) is >> meas[i]; 41 | setMeasurement(meas); 42 | // read covariance matrix (upper triangle) 43 | if (is.good()) { 44 | for ( int i=0; i> information()(i,j); 47 | if (i!=j) 48 | information()(j,i)=information()(i,j); 49 | } 50 | } 51 | return !is.fail(); 52 | } 53 | 54 | bool EdgeXYZPrior::write(std::ostream& os) const { 55 | for (int i = 0; i<3; i++) os << measurement()[i] << " "; 56 | for (int i=0; i(_vertices[0]); 65 | _error = v->estimate() - _measurement; 66 | } 67 | 68 | void EdgeXYZPrior::linearizeOplus(){ 69 | _jacobianOplusXi = Matrix3::Identity(); 70 | } 71 | 72 | bool EdgeXYZPrior::setMeasurementFromState(){ 73 | const VertexPointXYZ* v = static_cast(_vertices[0]); 74 | _measurement = v->estimate(); 75 | return true; 76 | } 77 | } 78 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/types/edge_xyz_prior.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_EDGE_XYZ_PRIOR_H_ 28 | #define G2O_EDGE_XYZ_PRIOR_H_ 29 | 30 | #include "../core/base_unary_edge.h" 31 | #include "g2o_types_slam3d_api.h" 32 | #include "vertex_pointxyz.h" 33 | 34 | namespace g2o { 35 | /** 36 | * \brief prior for an XYZ vertex (VertexPointXYZ) 37 | * 38 | * Provides a prior for a 3d point vertex. The measurement is represented by a 39 | * Vector3 with a corresponding 3x3 upper triangle covariance matrix (upper triangle only). 40 | */ 41 | class G2O_TYPES_SLAM3D_API EdgeXYZPrior : public BaseUnaryEdge<3, Vector3, VertexPointXYZ> { 42 | public: 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 44 | EdgeXYZPrior(); 45 | virtual bool read(std::istream& is); 46 | virtual bool write(std::ostream& os) const; 47 | 48 | void computeError(); 49 | 50 | // jacobian 51 | virtual void linearizeOplus(); 52 | 53 | virtual void setMeasurement(const Vector3& m){ 54 | _measurement = m; 55 | } 56 | 57 | virtual bool setMeasurementData(const number_t* d){ 58 | Eigen::Map v(d); 59 | _measurement = v; 60 | return true; 61 | } 62 | 63 | virtual bool getMeasurementData(number_t* d) const{ 64 | Eigen::Map v(d); 65 | v = _measurement; 66 | return true; 67 | } 68 | 69 | virtual int measurementDimension() const { return 3; } 70 | 71 | virtual bool setMeasurementFromState() ; 72 | 73 | virtual number_t initialEstimatePossible(const OptimizableGraph::VertexSet& /*from*/, 74 | OptimizableGraph::Vertex* /*to*/) { 75 | return 0; 76 | } 77 | }; 78 | 79 | } 80 | #endif 81 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/types/eigen_types_new.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_NEW_H 28 | #define G2O_EIGEN_TYPES_NEW_H 29 | 30 | #include 31 | #include 32 | 33 | #include "../../config.h" 34 | 35 | namespace g2o { 36 | 37 | template 38 | using VectorN = Eigen::Matrix; 39 | using Vector2 = VectorN<2>; 40 | using Vector3 = VectorN<3>; 41 | using Vector4 = VectorN<4>; 42 | using Vector6 = VectorN<6>; 43 | using Vector7 = VectorN<7>; 44 | using VectorX = VectorN; 45 | 46 | template 47 | using MatrixN = Eigen::Matrix; 48 | using Matrix2 = MatrixN<2>; 49 | using Matrix3 = MatrixN<3>; 50 | using Matrix4 = MatrixN<4>; 51 | using MatrixX = MatrixN; 52 | 53 | typedef Eigen::Transform Isometry2; 54 | typedef Eigen::Transform Isometry3; 55 | 56 | typedef Eigen::Transform Affine2; 57 | typedef Eigen::Transform Affine3; 58 | 59 | typedef Eigen::Rotation2D Rotation2D; 60 | 61 | typedef Eigen::Quaternion Quaternion; 62 | 63 | typedef Eigen::AngleAxis AngleAxis; 64 | 65 | } // end namespace g2o 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/types/g2o_types_slam3d_api.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 | /*************************************************************************** 28 | * Description: import/export macros for creating DLLS with Microsoft 29 | * compiler. Any exported function needs to be declared with the 30 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 31 | * for each DLL (arrrrrgh!!!) 32 | * 33 | * 17 Jan 2012 34 | * Email: pupilli@cs.bris.ac.uk 35 | ****************************************************************************/ 36 | #ifndef G2O_TYPES_SLAM3D_API_H 37 | #define G2O_TYPES_SLAM3D_API_H 38 | 39 | #include "../../config.h" 40 | 41 | #ifdef _MSC_VER 42 | // We are using a Microsoft compiler: 43 | #ifdef G2O_SHARED_LIBS 44 | #ifdef types_slam3d_EXPORTS 45 | #define G2O_TYPES_SLAM3D_API __declspec(dllexport) 46 | #else 47 | #define G2O_TYPES_SLAM3D_API __declspec(dllimport) 48 | #endif 49 | #else 50 | #define G2O_TYPES_SLAM3D_API 51 | #endif 52 | 53 | #else 54 | // Not Microsoft compiler so set empty definition: 55 | #define G2O_TYPES_SLAM3D_API 56 | #endif 57 | 58 | #endif // G2O_TYPES_SLAM3D_API_H 59 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/types/isometry3d_gradients.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 "isometry3d_gradients.h" 28 | #include 29 | namespace g2o { 30 | using namespace std; 31 | using namespace Eigen; 32 | 33 | namespace internal { 34 | #include "dquat2mat.cpp" 35 | } // end namespace internal 36 | 37 | } // end namespace 38 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/types/types_dyn_slam3d.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of VDO-SLAM. 3 | * 4 | * Copyright (C) 2019-2020 Jun Zhang (The Australian National University) 5 | * For more information see 6 | * 7 | **/ 8 | 9 | #include "types_dyn_slam3d.h" 10 | 11 | #include "../core/factory.h" 12 | #include "../stuff/macros.h" 13 | 14 | namespace g2o { 15 | 16 | using namespace std; 17 | using namespace Eigen; 18 | 19 | // ************************************************************************************************ 20 | 21 | LandmarkMotionTernaryEdge::LandmarkMotionTernaryEdge() : BaseMultiEdge<3,Vector3>() 22 | { 23 | resize(3); 24 | J.fill(0); 25 | J.block<3,3>(0,0) = Matrix3::Identity(); 26 | } 27 | 28 | bool LandmarkMotionTernaryEdge::read(std::istream& is) 29 | { 30 | Vector3d meas; 31 | for (int i=0; i<3; i++) is >> meas[i]; 32 | setMeasurement(meas); 33 | 34 | for (int i=0; i<3; i++) 35 | for (int j=i; j<3; j++) { 36 | is >> information()(i,j); 37 | if (i!=j) 38 | information()(j,i) = information()(i,j); 39 | } 40 | return true; 41 | 42 | } 43 | 44 | bool LandmarkMotionTernaryEdge::write(std::ostream& os) const 45 | { 46 | for (int i=0; i<3; i++) os << measurement()[i] << " "; 47 | for (int i=0; i<3; ++i) 48 | for (int j=i; j<3; ++j) 49 | os << information()(i, j) << " " ; 50 | return os.good(); 51 | } 52 | 53 | void LandmarkMotionTernaryEdge::computeError() 54 | { 55 | VertexPointXYZ* point1 = dynamic_cast(_vertices[0]); 56 | VertexPointXYZ* point2 = dynamic_cast(_vertices[1]); 57 | VertexSE3* H = static_cast(_vertices[2]); 58 | 59 | Vector3 expected = point1->estimate() - H->estimate().inverse()*point2->estimate(); 60 | _error = expected - _measurement; 61 | } 62 | 63 | void LandmarkMotionTernaryEdge::linearizeOplus() 64 | { 65 | 66 | //VertexPointXYZ* point1 = dynamic_cast(_vertices[0]); 67 | VertexPointXYZ* point2 = dynamic_cast(_vertices[1]); 68 | VertexSE3* H = static_cast(_vertices[2]); 69 | 70 | Vector3 invHl2 = H->estimate().inverse()*point2->estimate(); 71 | 72 | // jacobian wrt H 73 | J(0,4) = invHl2(2); 74 | J(0,5) = -invHl2(1); 75 | J(1,3) = -invHl2(2); 76 | J(1,5) = invHl2(0); 77 | J(2,3) = invHl2(1); 78 | J(2,4) = -invHl2(0); 79 | 80 | Eigen::Matrix Jhom = J; 81 | 82 | _jacobianOplus[0] = Matrix3::Identity(); 83 | _jacobianOplus[1] = -H->estimate().inverse().rotation(); 84 | _jacobianOplus[2] = Jhom; 85 | } 86 | 87 | // ************************************************************************************************ 88 | 89 | EdgeSE3Altitude::EdgeSE3Altitude() : BaseUnaryEdge<1, double, VertexSE3>() 90 | { 91 | resize(1); 92 | J.fill(0); 93 | J[1] = 1; 94 | } 95 | 96 | bool EdgeSE3Altitude::read(std::istream& is) 97 | { 98 | double meas; 99 | is >> meas; 100 | setMeasurement(meas); 101 | is >> information()(0); 102 | return true; 103 | } 104 | 105 | bool EdgeSE3Altitude::write(std::ostream& os) const 106 | { 107 | os << measurement() << " "; 108 | os << information()(0) << " "; 109 | return os.good(); 110 | } 111 | 112 | void EdgeSE3Altitude::computeError() 113 | { 114 | VertexSE3* v = static_cast(_vertices[0]); 115 | _error << v->estimate().translation()[1] - _measurement; 116 | } 117 | 118 | void EdgeSE3Altitude::linearizeOplus() 119 | { 120 | Eigen::Matrix Jhom = J; 121 | _jacobianOplusXi = Jhom; 122 | } 123 | 124 | // ************************************************************************************************ 125 | 126 | } // end namespace 127 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/types/types_dyn_slam3d.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of VDO-SLAM. 3 | * 4 | * Copyright (C) 2019-2020 Jun Zhang (The Australian National University) 5 | * For more information see 6 | * 7 | **/ 8 | 9 | #ifndef G2O_DYNAMIC_SLAM3D 10 | #define G2O_DYNAMIC_SLAM3D 11 | 12 | #include "vertex_se3.h" 13 | #include "edge_se3.h" 14 | #include "vertex_pointxyz.h" 15 | #include "../core/base_multi_edge.h" 16 | #include "../core/base_unary_edge.h" 17 | 18 | namespace g2o { 19 | 20 | namespace types_dyn_slam3d { 21 | void init(); 22 | } 23 | 24 | class LandmarkMotionTernaryEdge: public BaseMultiEdge<3,Vector3> 25 | { 26 | public: 27 | LandmarkMotionTernaryEdge(); 28 | 29 | virtual bool read(std::istream& is); 30 | virtual bool write(std::ostream& os) const; 31 | void computeError(); 32 | void linearizeOplus(); 33 | 34 | virtual void setMeasurement(const Vector3& m){ 35 | _measurement = m; 36 | } 37 | 38 | private: 39 | Eigen::Matrix J; 40 | 41 | }; 42 | 43 | 44 | class EdgeSE3Altitude: public BaseUnaryEdge<1, double, VertexSE3> 45 | { 46 | public: 47 | EdgeSE3Altitude(); 48 | 49 | virtual bool read(std::istream& is); 50 | virtual bool write(std::ostream& os) const; 51 | void computeError(); 52 | void linearizeOplus(); 53 | 54 | virtual void setMeasurement(const double& m){ 55 | _measurement = m; 56 | } 57 | 58 | private: 59 | Eigen::Matrix J; 60 | }; 61 | 62 | 63 | } // end namespace 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /dependencies/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 | VertexSBADepth::VertexSBADepth() : BaseVertex<1, Matrix >() 57 | { 58 | } 59 | 60 | bool VertexSBADepth::read(std::istream& is) 61 | { 62 | Matrix lv; 63 | for (int i=0; i<1; i++) 64 | is >> _estimate[i]; 65 | return true; 66 | } 67 | 68 | bool VertexSBADepth::write(std::ostream& os) const 69 | { 70 | Matrix lv=estimate(); 71 | for (int i=0; i<1; i++){ 72 | os << lv[i] << " "; 73 | } 74 | return os.good(); 75 | } 76 | 77 | VertexSBAFlow::VertexSBAFlow() : BaseVertex<2, Vector2d>() 78 | { 79 | } 80 | 81 | bool VertexSBAFlow::read(std::istream& is) 82 | { 83 | Vector2d lv; 84 | for (int i=0; i<2; i++) 85 | is >> _estimate[i]; 86 | return true; 87 | } 88 | 89 | bool VertexSBAFlow::write(std::ostream& os) const 90 | { 91 | Vector2d lv=estimate(); 92 | for (int i=0; i<2; i++){ 93 | os << lv[i] << " "; 94 | } 95 | return os.good(); 96 | } 97 | 98 | VertexSBAFlowDepth::VertexSBAFlowDepth() : BaseVertex<3, Vector3d>() 99 | { 100 | } 101 | 102 | bool VertexSBAFlowDepth::read(std::istream& is) 103 | { 104 | Vector3d lv; 105 | for (int i=0; i<3; i++) 106 | is >> _estimate[i]; 107 | return true; 108 | } 109 | 110 | bool VertexSBAFlowDepth::write(std::ostream& os) const 111 | { 112 | Vector3d lv=estimate(); 113 | for (int i=0; i<3; i++){ 114 | os << lv[i] << " "; 115 | } 116 | return os.good(); 117 | } 118 | 119 | } // end namespace 120 | -------------------------------------------------------------------------------- /dependencies/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 | class VertexSBADepth : public BaseVertex<1, Matrix > 60 | { 61 | public: 62 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 63 | VertexSBADepth(); 64 | virtual bool read(std::istream& is); 65 | virtual bool write(std::ostream& os) const; 66 | 67 | virtual void setToOriginImpl() { 68 | _estimate.fill(0.); 69 | } 70 | 71 | virtual void oplusImpl(const double* update) 72 | { 73 | Eigen::Map > v(update); 74 | _estimate += v; 75 | } 76 | }; 77 | 78 | class VertexSBAFlow : public BaseVertex<2, Vector2d> 79 | { 80 | public: 81 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 82 | VertexSBAFlow(); 83 | virtual bool read(std::istream& is); 84 | virtual bool write(std::ostream& os) const; 85 | 86 | virtual void setToOriginImpl() { 87 | _estimate.fill(0.); 88 | } 89 | 90 | virtual void oplusImpl(const double* update) 91 | { 92 | Eigen::Map v(update); 93 | _estimate += v; 94 | } 95 | }; 96 | 97 | class VertexSBAFlowDepth : public BaseVertex<3, Vector3d> 98 | { 99 | public: 100 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 101 | VertexSBAFlowDepth(); 102 | virtual bool read(std::istream& is); 103 | virtual bool write(std::ostream& os) const; 104 | 105 | virtual void setToOriginImpl() { 106 | _estimate.fill(0.); 107 | } 108 | 109 | virtual void oplusImpl(const double* update) 110 | { 111 | Eigen::Map v(update); 112 | _estimate += v; 113 | } 114 | }; 115 | 116 | } // end namespace 117 | 118 | #endif // SBA_TYPES 119 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/types/types_slam3d.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 "types_slam3d.h" 28 | #include "../core/factory.h" 29 | #include "../stuff/macros.h" 30 | 31 | #include 32 | 33 | namespace g2o { 34 | 35 | G2O_REGISTER_TYPE_GROUP(slam3d); 36 | 37 | G2O_REGISTER_TYPE(VERTEX_SE3:QUAT, VertexSE3); 38 | G2O_REGISTER_TYPE(EDGE_SE3:QUAT, EdgeSE3); 39 | G2O_REGISTER_TYPE(VERTEX_TRACKXYZ, VertexPointXYZ); 40 | G2O_REGISTER_TYPE(EDGE_SE3_MOTION, LandmarkMotionTernaryEdge); 41 | // G2O_REGISTER_TYPE(EDGE_SE3_ALTITUDE, EdgeSE3Altitude); 42 | 43 | G2O_REGISTER_TYPE(PARAMS_SE3OFFSET, ParameterSE3Offset); 44 | G2O_REGISTER_TYPE(EDGE_SE3_TRACKXYZ, EdgeSE3PointXYZ); 45 | G2O_REGISTER_TYPE(EDGE_SE3_PRIOR, EdgeSE3Prior); 46 | // G2O_REGISTER_TYPE(EDGE_SE3_XYZ_PRIOR, EdgeSE3XYZPrior); 47 | G2O_REGISTER_TYPE(CACHE_SE3_OFFSET, CacheSE3Offset); 48 | G2O_REGISTER_TYPE(EDGE_SE3_OFFSET, EdgeSE3Offset); 49 | 50 | // G2O_REGISTER_TYPE(PARAMS_CAMERACALIB, ParameterCamera); 51 | // G2O_REGISTER_TYPE(PARAMS_STEREOCAMERACALIB, ParameterStereoCamera); 52 | // G2O_REGISTER_TYPE(CACHE_CAMERA, CacheCamera); 53 | // G2O_REGISTER_TYPE(EDGE_PROJECT_DISPARITY, EdgeSE3PointXYZDisparity); 54 | // G2O_REGISTER_TYPE(EDGE_PROJECT_DEPTH, EdgeSE3PointXYZDepth); 55 | 56 | // G2O_REGISTER_TYPE(EDGE_POINTXYZ, EdgePointXYZ); 57 | G2O_REGISTER_TYPE(EDGE_POINTXYZ_PRIOR, EdgeXYZPrior); 58 | 59 | // G2O_REGISTER_TYPE(EDGE_SE3_LOTSOF_XYZ, EdgeSE3LotsOfXYZ) 60 | 61 | /*********** ACTIONS ************/ 62 | G2O_REGISTER_ACTION(VertexSE3WriteGnuplotAction); 63 | G2O_REGISTER_ACTION(VertexPointXYZWriteGnuplotAction); 64 | G2O_REGISTER_ACTION(EdgeSE3WriteGnuplotAction); 65 | 66 | #ifdef G2O_HAVE_OPENGL 67 | G2O_REGISTER_ACTION(VertexPointXYZDrawAction); 68 | G2O_REGISTER_ACTION(VertexSE3DrawAction); 69 | G2O_REGISTER_ACTION(EdgeSE3DrawAction); 70 | G2O_REGISTER_ACTION(EdgeSE3PointXYZDrawAction); 71 | G2O_REGISTER_ACTION(EdgeProjectDisparityDrawAction); 72 | G2O_REGISTER_ACTION(CacheCameraDrawAction); 73 | G2O_REGISTER_ACTION(CacheSE3OffsetDrawAction); 74 | #endif 75 | 76 | } // end namespace 77 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/types/types_slam3d.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_TYPES_SLAM3D_ 28 | #define G2O_TYPES_SLAM3D_ 29 | 30 | #include "../../config.h" 31 | #include "../core/base_vertex.h" 32 | #include "../core/base_binary_edge.h" 33 | #include "../core/hyper_graph_action.h" 34 | 35 | #define THREE_D_TYPES_ANALYTIC_JACOBIAN 36 | 37 | #include "vertex_se3.h" 38 | #include "edge_se3.h" 39 | #include "vertex_pointxyz.h" 40 | 41 | #include "parameter_se3_offset.h" 42 | #include "edge_se3_pointxyz.h" 43 | #include "edge_se3_offset.h" 44 | 45 | #include "types_dyn_slam3d.h" 46 | // #include "edge_se3_altitude.h" 47 | 48 | // #include "parameter_camera.h" 49 | // #include "parameter_stereo_camera.h" 50 | // #include "edge_se3_pointxyz_disparity.h" 51 | // #include "edge_se3_pointxyz_depth.h" 52 | #include "edge_se3_prior.h" 53 | // #include "edge_se3_xyzprior.h" 54 | 55 | // #include "edge_pointxyz.h" 56 | #include "edge_xyz_prior.h" 57 | 58 | // #include "edge_se3_lotsofxyz.h" 59 | #endif 60 | -------------------------------------------------------------------------------- /dependencies/g2o/g2o/types/vertex_point_xy.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_VERTEX_POINT_XY_H 28 | #define G2O_VERTEX_POINT_XY_H 29 | 30 | #include "g2o_types_slam2d_api.h" 31 | #include "g2o/config.h" 32 | #include "g2o/core/base_vertex.h" 33 | #include "g2o/core/hyper_graph_action.h" 34 | 35 | #include 36 | 37 | namespace g2o { 38 | 39 | class G2O_TYPES_SLAM2D_API VertexPointXY : public BaseVertex<2, Vector2> 40 | { 41 | public: 42 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 43 | VertexPointXY(); 44 | 45 | virtual void setToOriginImpl() { 46 | _estimate.setZero(); 47 | } 48 | 49 | virtual bool setEstimateDataImpl(const number_t* est){ 50 | _estimate[0] = est[0]; 51 | _estimate[1] = est[1]; 52 | return true; 53 | } 54 | 55 | virtual bool getEstimateData(number_t* est) const{ 56 | est[0] = _estimate[0]; 57 | est[1] = _estimate[1]; 58 | return true; 59 | } 60 | 61 | virtual int estimateDimension() const { 62 | return 2; 63 | } 64 | 65 | virtual bool setMinimalEstimateDataImpl(const number_t* est){ 66 | return setEstimateData(est); 67 | } 68 | 69 | virtual bool getMinimalEstimateData(number_t* est) const{ 70 | return getEstimateData(est); 71 | } 72 | 73 | virtual int minimalEstimateDimension() const { 74 | return 2; 75 | } 76 | 77 | virtual void oplusImpl(const number_t* update) 78 | { 79 | _estimate[0] += update[0]; 80 | _estimate[1] += update[1]; 81 | } 82 | 83 | virtual bool read(std::istream& is); 84 | virtual bool write(std::ostream& os) const; 85 | 86 | }; 87 | 88 | class G2O_TYPES_SLAM2D_API VertexPointXYWriteGnuplotAction: public WriteGnuplotAction { 89 | public: 90 | VertexPointXYWriteGnuplotAction(); 91 | virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, 92 | HyperGraphElementAction::Parameters* params_); 93 | }; 94 | 95 | #ifdef G2O_HAVE_OPENGL 96 | class G2O_TYPES_SLAM2D_API VertexPointXYDrawAction: public DrawAction{ 97 | public: 98 | VertexPointXYDrawAction(); 99 | virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, 100 | HyperGraphElementAction::Parameters* params_); 101 | protected: 102 | FloatProperty *_pointSize; 103 | virtual bool refreshPropertyPtrs(HyperGraphElementAction::Parameters* params_); 104 | }; 105 | #endif 106 | 107 | } 108 | 109 | #endif 110 | -------------------------------------------------------------------------------- /dependencies/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 | -------------------------------------------------------------------------------- /example/kitti-0000-0013.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 721.5377 9 | Camera.fy: 721.5377 10 | Camera.cx: 609.5593 11 | Camera.cy: 172.8540 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | Camera.k3: 0.0 18 | 19 | Camera.width: 1242 20 | Camera.height: 375 21 | 22 | # Camera frames per second 23 | Camera.fps: 10.0 24 | 25 | # IR projector baseline times fx (aprox.) 26 | Camera.bf: 387.5744 27 | 28 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 29 | Camera.RGB: 1 30 | 31 | #-------------------------------------------------------------------------------------------- 32 | # System Parameters 33 | #-------------------------------------------------------------------------------------------- 34 | 35 | # Switch for Dataset (1 for Oxford MultiMotion Dataset, 2 for KITTI dataset, 3...) 36 | ChooseData: 2 37 | 38 | # Deptmap values factor 39 | DepthMapFactor: 256.0 40 | 41 | # Close/Far Depth threshold 42 | ThDepthBG: 40.0 43 | ThDepthOBJ: 25.0 44 | 45 | # Max Tracking Points on Background and Object in each frame 46 | MaxTrackPointBG: 1200 # 1200 1500 2000 47 | MaxTrackPointOBJ: 800 # 800 48 | 49 | # Scene Flow Magnitude and Distribution Threshold 50 | SFMgThres: 0.12 # 0.05 51 | SFDsThres: 0.3 # 0.99 52 | 53 | # Window Size and Overlapping Size for Local Batch Optimization 54 | WINDOW_SIZE: 20 55 | OVERLAP_SIZE: 4 56 | 57 | # Use sampled feature or detected feature for background (1: sampled, 0: detected) 58 | UseSampleFeature: 0 59 | 60 | #-------------------------------------------------------------------------------------------- 61 | # ORB Parameters (NOTE: only FAST corners are used in this system.) 62 | #-------------------------------------------------------------------------------------------- 63 | 64 | # ORB Extractor: Number of features per image 65 | ORBextractor.nFeatures: 2500 66 | 67 | # ORB Extractor: Scale factor between levels in the scale pyramid 68 | ORBextractor.scaleFactor: 1.2 69 | 70 | # ORB Extractor: Number of levels in the scale pyramid 71 | ORBextractor.nLevels: 8 72 | 73 | # ORB Extractor: Fast threshold 74 | ORBextractor.iniThFAST: 20 75 | ORBextractor.minThFAST: 7 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /example/kitti-0018-0020.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 718.8560 9 | Camera.fy: 718.8560 10 | Camera.cx: 607.1928 11 | Camera.cy: 185.2157 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 1242 19 | Camera.height: 375 20 | 21 | # Camera frames per second 22 | Camera.fps: 10.0 23 | 24 | # stereo baseline times fx 25 | Camera.bf: 388.1822 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | #-------------------------------------------------------------------------------------------- 31 | # System Parameters 32 | #-------------------------------------------------------------------------------------------- 33 | 34 | # Switch for Dataset (1 for Oxford MultiMotion Dataset, 2 for KITTI dataset, 3...) 35 | ChooseData: 2 36 | 37 | # Deptmap values factor 38 | DepthMapFactor: 256.0 39 | 40 | # Close/Far Depth threshold 41 | ThDepthBG: 40.0 42 | ThDepthOBJ: 25.0 43 | 44 | # Max Tracking Points on Background and Object in each frame 45 | MaxTrackPointBG: 1200 # 1200 46 | MaxTrackPointOBJ: 800 # 800 47 | 48 | # Scene Flow Magnitude and Distribution Threshold 49 | SFMgThres: 0.12 # 0.05 50 | SFDsThres: 0.3 # 0.99 51 | 52 | # Window Size and Overlapping Size for Local Batch Optimization 53 | WINDOW_SIZE: 20 54 | OVERLAP_SIZE: 4 55 | 56 | # Use sampled feature or detected feature for background (1: sampled, 0: detected) 57 | UseSampleFeature: 0 58 | 59 | #-------------------------------------------------------------------------------------------- 60 | # ORB Parameters (NOTE: only FAST corners are used in this system.) 61 | #-------------------------------------------------------------------------------------------- 62 | 63 | # ORB Extractor: Number of features per image 64 | ORBextractor.nFeatures: 2500 65 | 66 | # ORB Extractor: Scale factor between levels in the scale pyramid 67 | ORBextractor.scaleFactor: 1.2 68 | 69 | # ORB Extractor: Number of levels in the scale pyramid 70 | ORBextractor.nLevels: 8 71 | 72 | # ORB Extractor: Fast threshold 73 | ORBextractor.iniThFAST: 20 74 | ORBextractor.minThFAST: 7 75 | 76 | -------------------------------------------------------------------------------- /example/omd.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 618.3587036132812 # 618.3587036132812 569.031286352279 9 | Camera.fy: 618.5924072265625 # 618.5924072265625 568.822872802803 10 | Camera.cx: 328.9866333007812 # 328.9866333007812 338.766074706977 11 | Camera.cy: 237.7507629394531 # 237.7507629394531 221.664002362162 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | Camera.k3: 0.0 18 | 19 | Camera.width: 640 20 | Camera.height: 480 21 | 22 | # Camera frames per second 23 | Camera.fps: 30.0 24 | 25 | # IR projector baseline times fx (aprox.) 26 | Camera.bf: 387.5744 27 | 28 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 29 | Camera.RGB: 1 30 | 31 | #-------------------------------------------------------------------------------------------- 32 | # System Parameters 33 | #-------------------------------------------------------------------------------------------- 34 | 35 | # Switch for Dataset (1 for Oxford MultiMotion Dataset, 2 for KITTI dataset, 3...) 36 | ChooseData: 1 37 | 38 | # Deptmap values factor 39 | DepthMapFactor: 1000.0 40 | 41 | # Close/Far Depth threshold 42 | ThDepthBG: 40.0 43 | ThDepthOBJ: 25.0 44 | 45 | # Max Tracking Points on Background and Object in each frame 46 | MaxTrackPointBG: 1200 47 | MaxTrackPointOBJ: 800 48 | 49 | # Scene Flow Magnitude and Distribution Threshold 50 | SFMgThres: 0.02 51 | SFDsThres: 0.99 52 | 53 | # Window Size and Overlapping Size for Local Batch Optimization 54 | WINDOW_SIZE: 20 55 | OVERLAP_SIZE: 4 56 | 57 | # Use sampled feature or detected feature for background (1: sampled, 0: detected) 58 | UseSampleFeature: 1 59 | 60 | #-------------------------------------------------------------------------------------------- 61 | # ORB Parameters (NOTE: only FAST corners are used in this system.) 62 | #-------------------------------------------------------------------------------------------- 63 | 64 | # ORB Extractor: Number of features per image 65 | ORBextractor.nFeatures: 3000 66 | 67 | # ORB Extractor: Scale factor between levels in the scale pyramid 68 | ORBextractor.scaleFactor: 1.2 69 | 70 | # ORB Extractor: Number of levels in the scale pyramid 71 | ORBextractor.nLevels: 8 72 | 73 | # ORB Extractor: Fast threshold 74 | ORBextractor.iniThFAST: 20 75 | ORBextractor.minThFAST: 7 76 | 77 | -------------------------------------------------------------------------------- /include/Converter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of VDO-SLAM. 3 | * 4 | * Copyright (C) 2019-2020 Jun Zhang (The Australian National University) 5 | * For more information see 6 | * 7 | **/ 8 | 9 | #ifndef CONVERTER_H 10 | #define CONVERTER_H 11 | 12 | #include 13 | 14 | #include 15 | #include"dependencies/g2o/g2o/types/types_six_dof_expmap.h" 16 | #include"dependencies/g2o/g2o/types/types_seven_dof_expmap.h" 17 | 18 | namespace VDO_SLAM 19 | { 20 | 21 | class Converter 22 | { 23 | public: 24 | static std::vector toDescriptorVector(const cv::Mat &Descriptors); 25 | 26 | static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT); 27 | static g2o::SE3Quat toSE3Quat(const g2o::Sim3 &gSim3); 28 | 29 | static cv::Mat toCvMat(const g2o::SE3Quat &SE3); 30 | static cv::Mat toCvMat(const g2o::Sim3 &Sim3); 31 | static cv::Mat toCvMat(const Eigen::Matrix &m); 32 | static cv::Mat toCvMat(const Eigen::Matrix3d &m); 33 | static cv::Mat toCvMat(const Eigen::Matrix &m); 34 | static cv::Mat toCvSE3(const Eigen::Matrix &R, const Eigen::Matrix &t); 35 | 36 | static Eigen::Matrix toVector3d(const cv::Mat &cvVector); 37 | static Eigen::Matrix toVector3d(const cv::Point3f &cvPoint); 38 | static Eigen::Matrix toMatrix3d(const cv::Mat &cvMat3); 39 | static Eigen::Matrix toMatrix4d(const cv::Mat &cvMat4); 40 | 41 | static std::vector toQuaternion(const cv::Mat &M); 42 | static cv::Mat toInvMatrix(const cv::Mat &T); 43 | }; 44 | 45 | }// namespace VDO_SLAM 46 | 47 | #endif // CONVERTER_H 48 | -------------------------------------------------------------------------------- /include/Map.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of VDO-SLAM. 3 | * 4 | * Copyright (C) 2019-2020 Jun Zhang (The Australian National University) 5 | * For more information see 6 | * 7 | **/ 8 | 9 | #ifndef MAP_H 10 | #define MAP_H 11 | 12 | #include 13 | 14 | #include 15 | 16 | 17 | namespace VDO_SLAM 18 | { 19 | 20 | class Map 21 | { 22 | public: 23 | Map(); 24 | 25 | // ========================================================== 26 | // ============= output for evaluating results ============== 27 | 28 | // ========================================================== 29 | // ========================================================== 30 | 31 | // <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> 32 | // <<<<<<<<<<<<<<<<<<<< output for graph structure >>>>>>>>>>>>>>>>>>> 33 | 34 | // static features and depths detected in image plane. (k*n) 35 | std::vector > vpFeatSta; 36 | std::vector > vfDepSta; 37 | std::vector > vp3DPointSta; 38 | // index of temporal matching. (k-1)*n 39 | std::vector > vnAssoSta; 40 | // feature tracklets: pair.first = frameID; pair.second = featureID; 41 | std::vector > > TrackletSta; 42 | 43 | 44 | // dynamic feature correspondences and depths detected in image plane. k*n 45 | std::vector > vpFeatDyn; 46 | std::vector > vfDepDyn; 47 | std::vector > vp3DPointDyn; 48 | // index of temporal matching. (k-1)*n 49 | std::vector > vnAssoDyn; 50 | // label indicating which object the feature (3D point) belongs to. (k-1)*n 51 | std::vector > vnFeatLabel; 52 | // feature tracklets: pair.first = frameID; pair.second = featureID; 53 | std::vector > > TrackletDyn; 54 | std::vector nObjID; 55 | 56 | 57 | // absolute camera pose of each frame, starting from 1st frame. (k*1) 58 | std::vector vmCameraPose; 59 | std::vector vmCameraPose_RF; // refine result 60 | std::vector vmCameraPose_GT; // ground truth result 61 | // rigid motion of camera and dynamic points. (k-1)*m 62 | std::vector > vmRigidCentre; // ground truth object center 63 | std::vector > vmRigidMotion; 64 | std::vector > vmObjPosePre; // for new metric 26 Feb 2020 65 | std::vector > vmRigidMotion_RF; // refine result 66 | std::vector > vmRigidMotion_GT; // ground truth result 67 | std::vector > vfAllSpeed_GT; // camera and object speeds 68 | // rigid motion label in each frame (k-1)*m 69 | // 0 stands for camera motion; 1,...,l stands for rigid motions. 70 | std::vector > vnRMLabel; // tracking label 71 | std::vector > vnSMLabel; // semantic label 72 | std::vector > vnSMLabelGT; 73 | // object status (10 Jan 2020) 74 | std::vector > vbObjStat; 75 | // object tracking times (10 Jan 2020) 76 | std::vector > vnObjTraTime; 77 | std::vector nObjTraCount; 78 | std::vector nObjTraCountGT; 79 | std::vector nObjTraSemLab; 80 | 81 | // time analysis 82 | std::vector fLBA_time; 83 | // (0) frame updating (1) camera estimation (2) object tracking (3) object estimation (4) map updating; 84 | std::vector > vfAll_time; 85 | 86 | 87 | // <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> 88 | // <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> 89 | 90 | protected: 91 | 92 | long unsigned int mnMaxKFid; 93 | 94 | // Index related to a big change in the map 95 | int mnBigChangeIdx; 96 | 97 | }; 98 | 99 | } //namespace VDO_SLAM 100 | 101 | #endif // MAP_H 102 | -------------------------------------------------------------------------------- /include/ORBextractor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of VDO-SLAM. 3 | * 4 | * Copyright (C) 2019-2020 Jun Zhang (The Australian National University) 5 | * For more information see 6 | * 7 | **/ 8 | 9 | #ifndef ORBEXTRACTOR_H 10 | #define ORBEXTRACTOR_H 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | 17 | namespace VDO_SLAM 18 | { 19 | 20 | class ExtractorNode 21 | { 22 | public: 23 | ExtractorNode():bNoMore(false){} 24 | 25 | void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4); 26 | 27 | std::vector vKeys; 28 | cv::Point2i UL, UR, BL, BR; 29 | std::list::iterator lit; 30 | bool bNoMore; 31 | }; 32 | 33 | class ORBextractor 34 | { 35 | public: 36 | 37 | enum {HARRIS_SCORE=0, FAST_SCORE=1 }; 38 | 39 | ORBextractor(int nfeatures, float scaleFactor, int nlevels, 40 | int iniThFAST, int minThFAST); 41 | 42 | ~ORBextractor(){} 43 | 44 | // Compute the ORB features and descriptors on an image. 45 | // ORB are dispersed on the image using an octree. 46 | // Mask is ignored in the current implementation. 47 | void operator()( cv::InputArray image, cv::InputArray mask, 48 | std::vector& keypoints, 49 | cv::OutputArray descriptors); 50 | 51 | int inline GetLevels(){ 52 | return nlevels;} 53 | 54 | float inline GetScaleFactor(){ 55 | return scaleFactor;} 56 | 57 | std::vector inline GetScaleFactors(){ 58 | return mvScaleFactor; 59 | } 60 | 61 | std::vector inline GetInverseScaleFactors(){ 62 | return mvInvScaleFactor; 63 | } 64 | 65 | std::vector inline GetScaleSigmaSquares(){ 66 | return mvLevelSigma2; 67 | } 68 | 69 | std::vector inline GetInverseScaleSigmaSquares(){ 70 | return mvInvLevelSigma2; 71 | } 72 | 73 | std::vector mvImagePyramid; 74 | 75 | protected: 76 | 77 | void ComputePyramid(cv::Mat image); 78 | void ComputeKeyPointsOctTree(std::vector >& allKeypoints); 79 | std::vector DistributeOctTree(const std::vector& vToDistributeKeys, const int &minX, 80 | const int &maxX, const int &minY, const int &maxY, const int &nFeatures, const int &level); 81 | 82 | void ComputeKeyPointsOld(std::vector >& allKeypoints); 83 | std::vector pattern; 84 | 85 | int nfeatures; 86 | double scaleFactor; 87 | int nlevels; 88 | int iniThFAST; 89 | int minThFAST; 90 | 91 | std::vector mnFeaturesPerLevel; 92 | 93 | std::vector umax; 94 | 95 | std::vector mvScaleFactor; 96 | std::vector mvInvScaleFactor; 97 | std::vector mvLevelSigma2; 98 | std::vector mvInvLevelSigma2; 99 | }; 100 | 101 | } //namespace VDO_SLAM 102 | 103 | #endif 104 | 105 | -------------------------------------------------------------------------------- /include/Optimizer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of VDO-SLAM. 3 | * 4 | * Copyright (C) 2019-2020 Jun Zhang (The Australian National University) 5 | * For more information see 6 | * 7 | **/ 8 | 9 | #ifndef OPTIMIZER_H 10 | #define OPTIMIZER_H 11 | 12 | #include "Map.h" 13 | #include "Frame.h" 14 | #include "dependencies/g2o/g2o/types/types_six_dof_expmap.h" 15 | 16 | namespace VDO_SLAM 17 | { 18 | 19 | using namespace std; 20 | 21 | class Optimizer 22 | { 23 | public: 24 | 25 | int static PoseOptimizationNew(Frame *pCurFrame, Frame *pLastFrame, vector &TemperalMatch); 26 | int static PoseOptimizationFlow2Cam(Frame *pCurFrame, Frame *pLastFrame, vector &TemperalMatch); 27 | cv::Mat static PoseOptimizationObjMot(Frame *pCurFrame, Frame *pLastFrame, const vector &ObjId, std::vector &InlierID); 28 | cv::Mat static PoseOptimizationFlow2(Frame *pCurFrame, Frame *pLastFrame, const vector &ObjId, std::vector &InlierID); 29 | void static FullBatchOptimization(Map* pMap, const cv::Mat Calib_K); 30 | void static PartialBatchOptimization(Map* pMap, const cv::Mat Calib_K, const int WINDOW_SIZE); 31 | cv::Mat static Get3DinWorld(const cv::KeyPoint &Feats2d, const float &Dpts, const cv::Mat &Calib_K, const cv::Mat &CameraPose); 32 | cv::Mat static Get3DinCamera(const cv::KeyPoint &Feats2d, const float &Dpts, const cv::Mat &Calib_K); 33 | 34 | }; 35 | 36 | } //namespace VDO_SLAM 37 | 38 | #endif // OPTIMIZER_H 39 | -------------------------------------------------------------------------------- /include/System.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of VDO-SLAM. 3 | * 4 | * Copyright (C) 2019-2020 Jun Zhang (The Australian National University) 5 | * For more information see 6 | * 7 | **/ 8 | 9 | 10 | #ifndef SYSTEM_H 11 | #define SYSTEM_H 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "Tracking.h" 18 | #include "Map.h" 19 | 20 | namespace VDO_SLAM 21 | { 22 | 23 | using namespace std; 24 | 25 | class Map; 26 | class Tracking; 27 | 28 | class System 29 | { 30 | public: 31 | 32 | // Input sensor 33 | enum eSensor{ 34 | MONOCULAR=0, 35 | STEREO=1, 36 | RGBD=2 37 | }; 38 | 39 | public: 40 | 41 | // Initialize the SLAM system. 42 | System(const string &strSettingsFile, const eSensor sensor); 43 | 44 | 45 | // Process the given rgbd frame. Depthmap must be registered to the RGB frame. 46 | // Input image: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. 47 | // Input depthmap: Float (CV_32F). 48 | // Returns the camera pose (empty if tracking fails). 49 | cv::Mat TrackRGBD(const cv::Mat &im, cv::Mat &depthmap, const cv::Mat &flowmap, const cv::Mat &masksem, 50 | const cv::Mat &mTcw_gt, const vector > &vObjPose_gt, const double ×tamp, 51 | cv::Mat &imTraj, const int &nImage); 52 | 53 | void SaveResults(const string &filename); 54 | 55 | private: 56 | 57 | // Input sensor 58 | eSensor mSensor; 59 | 60 | // Map structure. 61 | Map* mpMap; 62 | 63 | // Tracker. It receives a frame and computes the associated camera pose. 64 | Tracking* mpTracker; 65 | 66 | }; 67 | 68 | }// namespace VDO_SLAM 69 | 70 | #endif // SYSTEM_H 71 | -------------------------------------------------------------------------------- /include/cvplot/color.h: -------------------------------------------------------------------------------- 1 | #ifndef CVPLOT_COLOR_H 2 | #define CVPLOT_COLOR_H 3 | 4 | #include 5 | 6 | namespace cvplot { 7 | 8 | struct Color { 9 | uint8_t r, g, b, a; 10 | Color(uint8_t r, uint8_t g, uint8_t b, uint8_t a = 255) 11 | : r(r), g(g), b(b), a(a) {} 12 | Color(const uint8_t *rgb, uint8_t a = 255) 13 | : Color(rgb[0], rgb[1], rgb[2], a) {} 14 | Color() : Color(0, 0, 0) {} 15 | 16 | Color alpha(uint8_t alpha) const; 17 | Color gamma(float gamma) const; 18 | float hue() const; 19 | 20 | static Color gray(uint8_t v); 21 | static Color hue(float hue); 22 | static Color cos(float hue); 23 | static Color index(uint8_t index, uint8_t density = 16, float avoid = 2.f, 24 | float range = 2.f); 25 | static Color hash(const std::string &seed); 26 | static Color uniq(const std::string &name); 27 | }; 28 | 29 | static const Color Red = Color::hue(0.f); 30 | static const Color Orange = Color::hue(.5f); 31 | static const Color Yellow = Color::hue(1.f); 32 | static const Color Lawn = Color::hue(1.5f); 33 | static const Color Green = Color::hue(2.f); 34 | static const Color Aqua = Color::hue(2.5f); 35 | static const Color Cyan = Color::hue(3.f); 36 | static const Color Sky = Color::hue(3.5f); 37 | static const Color Blue = Color::hue(4.f); 38 | static const Color Purple = Color::hue(4.5f); 39 | static const Color Magenta = Color::hue(5.f); 40 | static const Color Pink = Color::hue(5.5f); 41 | static const Color Black = Color::gray(0); 42 | static const Color Dark = Color::gray(32); 43 | static const Color Gray = Color::gray(128); 44 | static const Color Light = Color::gray(223); 45 | static const Color White = Color::gray(255); 46 | 47 | } // namespace cvplot 48 | 49 | #endif // CVPLOT_COLOR_H 50 | -------------------------------------------------------------------------------- /include/cvplot/cvplot.h: -------------------------------------------------------------------------------- 1 | #ifndef CVPLOT_H 2 | #define CVPLOT_H 3 | 4 | #include "color.h" 5 | #include "figure.h" 6 | #include "highgui.h" 7 | #include "window.h" 8 | 9 | #endif // CVPLOT_H 10 | -------------------------------------------------------------------------------- /include/cvplot/highgui.h: -------------------------------------------------------------------------------- 1 | #ifndef CVPLOT_HIGHGUI_H 2 | #define CVPLOT_HIGHGUI_H 3 | 4 | #include 5 | #include 6 | 7 | #include "window.h" 8 | 9 | namespace cvplot { 10 | 11 | int createTrackbar(const std::string &trackbarname, const std::string &winname, 12 | int *value, int count, TrackbarCallback onChange = 0, 13 | void *userdata = 0); 14 | void destroyAllWindows(); 15 | void destroyWindow(const std::string &view); 16 | int getMouseWheelDelta(int flags); 17 | int getTrackbarPos(const std::string &trackbarname, const std::string &winname); 18 | double getWindowProperty(const std::string &winname, int prop_id); 19 | void imshow(const std::string &view, void *img); 20 | void moveWindow(const std::string &view, int x, int y); 21 | void namedWindow(const std::string &view, int flags = 0); 22 | void resizeWindow(const std::string &view, int width, int height); 23 | void resizeWindow(const std::string &view, const Size &size); 24 | Rect selectROI(const std::string &windowName, void *img, 25 | bool showCrosshair = true, bool fromCenter = false); 26 | Rect selectROI(void *img, bool showCrosshair = true, bool fromCenter = false); 27 | void selectROIs(const std::string &windowName, void *img, 28 | std::vector &boundingBoxes, bool showCrosshair = true, 29 | bool fromCenter = false); 30 | void setMouseCallback(const std::string &view, MouseCallback onMouse, 31 | void *userdata = 0); 32 | void setTrackbarMax(const std::string &trackbarname, const std::string &winname, 33 | int maxval); 34 | void setTrackbarMin(const std::string &trackbarname, const std::string &winname, 35 | int minval); 36 | void setTrackbarPos(const std::string &trackbarname, const std::string &winname, 37 | int pos); 38 | void setWindowProperty(const std::string &winname, int prop_id, 39 | double prop_value); 40 | void setWindowTitle(const std::string &view, const std::string &title); 41 | int startWindowThread(); 42 | int waitKey(int delay = 0); 43 | int waitKeyEx(int delay = 0); 44 | 45 | } // namespace cvplot 46 | 47 | #endif // CVPLOT_HIGHGUI_H 48 | -------------------------------------------------------------------------------- /include/cvplot/window.h: -------------------------------------------------------------------------------- 1 | #ifndef CVPLOT_WINDOW_H 2 | #define CVPLOT_WINDOW_H 3 | 4 | #include "color.h" 5 | 6 | #include 7 | #include 8 | 9 | namespace cvplot { 10 | 11 | struct Rect { 12 | int x, y, width, height; 13 | Rect(int x, int y, int width, int height) 14 | : x(x), y(y), width(width), height(height) {} 15 | }; 16 | 17 | struct Size { 18 | int width, height; 19 | Size(int width, int height) : width(width), height(height) {} 20 | }; 21 | 22 | struct Offset { 23 | int x, y; 24 | Offset(int x, int y) : x(x), y(y) {} 25 | }; 26 | 27 | typedef void (*MouseCallback)(int event, int x, int y, int flags, void *param); 28 | typedef void (*TrackbarCallback)(int pos, void *param); 29 | 30 | class Window; 31 | 32 | class View { 33 | public: 34 | View(Window &window, const std::string &title = "", Size size = {300, 300}) 35 | : window_(window), 36 | title_(title), 37 | rect_(0, 0, size.width, size.height), 38 | frameless_(false), 39 | background_color_(Black), 40 | frame_color_(Green), 41 | text_color_(Black), 42 | mouse_callback_(NULL), 43 | mouse_param_(NULL) {} 44 | View &resize(Rect rect); 45 | View &size(Size size); 46 | View &offset(Offset offset); 47 | View &autosize(); 48 | View &title(const std::string &title); 49 | View &alpha(int alpha); 50 | View &backgroundColor(Color color); 51 | View &frameColor(Color color); 52 | View &textColor(Color color); 53 | View &mouse(MouseCallback callback, void *param = NULL); 54 | void onmouse(int event, int x, int y, int flags); 55 | 56 | Color backgroundColor(); 57 | Color frameColor(); 58 | Color textColor(); 59 | std::string &title(); 60 | bool has(Offset offset); 61 | 62 | void drawRect(Rect rect, Color color); 63 | void drawFill(Color background = White); 64 | void drawImage(const void *image, int alpha = 255); 65 | void drawText(const std::string &text, Offset offset, Color color) const; 66 | void drawFrame(const std::string &title) const; 67 | void *buffer(Rect &rect); 68 | void finish(); 69 | void flush(); 70 | void hide(bool hidden = true); 71 | 72 | View &operator=(const View &) = delete; 73 | 74 | protected: 75 | Rect rect_; 76 | std::string title_; 77 | bool frameless_; 78 | Window &window_; 79 | Color background_color_; 80 | Color frame_color_; 81 | Color text_color_; 82 | MouseCallback mouse_callback_; 83 | void *mouse_param_; 84 | bool hidden_; 85 | }; 86 | 87 | class Window { 88 | public: 89 | Window(const std::string &title = ""); 90 | Window &resize(Rect rect); 91 | Window &size(Size size); 92 | Window &offset(Offset offset); 93 | Window &title(const std::string &title); 94 | Window &fps(float fps); 95 | Window &ensure(Rect rect); 96 | Window &cursor(bool cursor); 97 | void *buffer(); 98 | void flush(); 99 | View &view(const std::string &name, Size size = {300, 300}); 100 | void dirty(); 101 | void tick(); 102 | void hide(bool hidden = true); 103 | void onmouse(int event, int x, int y, int flags); 104 | 105 | Window &operator=(const Window &) = delete; 106 | 107 | static Window ¤t(); 108 | static void current(Window &window); 109 | static Window ¤t(const std::string &title); 110 | 111 | protected: 112 | Offset offset_; 113 | void *buffer_; 114 | std::string title_; 115 | std::string name_; 116 | std::map views_; 117 | bool dirty_; 118 | float flush_time_; 119 | float fps_; 120 | bool hidden_; 121 | bool show_cursor_; 122 | Offset cursor_; 123 | }; 124 | 125 | class Util { 126 | public: 127 | static void sleep(float seconds = 0); 128 | static char key(float timeout = 0); 129 | static std::string line(float timeout = 0); 130 | }; 131 | 132 | } // namespace cvplot 133 | 134 | #endif // CVPLOT_WINDOW_H 135 | -------------------------------------------------------------------------------- /sourcecode_guidance.md: -------------------------------------------------------------------------------- 1 | # VDO-SLAM 2 | **Authors:** [Jun Zhang](https://halajun.github.io/), [Mina Henein](https://minahenein.github.io/), [Robert Mahony](https://users.cecs.anu.edu.au/~Robert.Mahony/) and [Viorela Ila](http://viorelaila.net/) 3 | 4 | VDO-SLAM is a Visual Object-aware Dynamic SLAM library for **RGB-D** cameras that is able to track dynamic objects, estimate the camera poses along with the static and dynamic structure, the full SE(3) pose change of every rigid object in the scene, extract velocity information, and be demonstrable in real-world outdoor scenarios. We provide examples to run the SLAM system in the [KITTI Tracking Dataset](http://cvlibs.net/datasets/kitti/eval_tracking.php), and in the [Oxford Multi-motion Dataset](https://robotic-esp.com/datasets/omd/). 5 | 6 | 7 | # 1. Prerequisites 8 | We have tested the library in **Mac OS X 10.14** and **Ubuntu 16.04**, but it should be easy to compile in other platforms. 9 | 10 | ## c++11, gcc and clang 11 | We use some functionalities of c++11, and the tested gcc version is 9.2.1 (ubuntu), the tested clang version is 1000.11.45.5 (Mac). 12 | 13 | ## OpenCV 14 | We use [OpenCV](http://opencv.org) to manipulate images and features. Download and install instructions can be found at: http://opencv.org. **Required at least 3.0. Tested with OpenCV 3.4**. 15 | 16 | ## Eigen3 17 | Required by g2o (see below). Download and install instructions can be found at: http://eigen.tuxfamily.org. **Required at least 3.1.0**. 18 | 19 | ## g2o (Included in dependencies folder) 20 | We use modified versions of [g2o](https://github.com/RainerKuemmerle/g2o) library to perform non-linear optimizations. The modified libraries (which are BSD) are included in the *dependencies* folder. 21 | 22 | 23 | # 2. Building Library 24 | 25 | In the source code folder, we provide a script `build.sh` to build the *dependencies* libraries and *VDO-SLAM*. 26 | Please make sure you have installed all required dependencies (see Section 1). 27 | Please also change the library file suffix, i.e., '.dylib' for Mac (default) or '.so' for Ubuntu, in the main CMakeLists.txt. 28 | Then Execute: 29 | ``` 30 | cd VDO-SLAM 31 | chmod +x build.sh 32 | ./build.sh 33 | ``` 34 | 35 | This will create 36 | 37 | 1. **libObjSLAM.dylib (Mac)** or **libObjSLAM.so (Ubuntu)** at *lib* folder, 38 | 39 | 2. **libg2o.dylib (Mac)** or **libg2o.so (Ubuntu)** at */dependencies/g2o/lib* folder, 40 | 41 | 3. and the executable **vdo_slam** in *example* folder. 42 | 43 | # 3. Running Examples 44 | 45 | ## KITTI Tracking Dataset 46 | 47 | 1. Download the demo sequence: [kitti_demo](https://drive.google.com/file/d/1LpjIdh6xL_UtWOkiJng0CKSmP7qAQhGu/view?usp=sharing), and uncompress it. 48 | 49 | 2. Execute the following command. 50 | ``` 51 | ./example/vdo_slam example/kitti-0000-0013.yaml PATH_TO_KITTI_SEQUENCE_DATA_FOLDER 52 | ``` 53 | 54 | ## Oxford Multi-motion Dataset 55 | 56 | 1. Download the demo sequence: [omd_demo](https://drive.google.com/file/d/1t4rG685a_7r0bHuW0bPKNbOhyiugnJK7/view?usp=sharing), and uncompress it. 57 | 58 | 2. Execute the following command. 59 | ``` 60 | ./example/vdo_slam example/omd.yaml PATH_TO_OMD_SEQUENCE_DATA_FOLDER 61 | ``` 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /src/Map.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of VDO-SLAM. 3 | * 4 | * Copyright (C) 2019-2020 Jun Zhang (The Australian National University) 5 | * For more information see 6 | * 7 | **/ 8 | 9 | #include "Map.h" 10 | 11 | namespace VDO_SLAM 12 | { 13 | 14 | Map::Map():mnMaxKFid(0),mnBigChangeIdx(0) 15 | { 16 | } 17 | 18 | } //namespace VDO_SLAM 19 | -------------------------------------------------------------------------------- /src/cvplot/color.cc: -------------------------------------------------------------------------------- 1 | #include "cvplot/color.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace cvplot { 7 | 8 | namespace { 9 | std::map color_counter; 10 | } 11 | 12 | Color Color::alpha(uint8_t alpha) const { return Color(r, g, b, alpha); } 13 | 14 | Color Color::gamma(float gamma) const { 15 | return Color(pow(r / 255.f, 1 / gamma) * 255, pow(g / 255.f, 1 / gamma) * 255, 16 | pow(b / 255.f, 1 / gamma) * 255, a); 17 | } 18 | 19 | Color Color::gray(uint8_t v) { return Color(v, v, v); } 20 | 21 | Color Color::index(uint8_t index, uint8_t density, float avoid, 22 | float range) { // avoid greens by default 23 | if (avoid > 0) { 24 | auto step = density / (6 - range); 25 | auto offset = (avoid + range / 2) * step; 26 | index = offset + index % density; 27 | density += step * range; 28 | } 29 | auto hue = index % density * 6.f / density; 30 | return Color::cos(hue); 31 | } 32 | 33 | Color Color::hash(const std::string &seed) { 34 | return Color::index(std::hash{}(seed)); 35 | } 36 | 37 | Color Color::uniq(const std::string &name) { 38 | if (color_counter.count(name) == 0) { 39 | color_counter[name] = color_counter.size(); 40 | } 41 | return Color::index(color_counter[name]); 42 | } 43 | 44 | Color Color::hue(float hue) { 45 | Color color; 46 | auto i = (int)hue; 47 | auto f = (hue - i) * 255; 48 | switch (i % 6) { 49 | case 0: 50 | return Color(255, f, 0); 51 | case 1: 52 | return Color(255 - f, 255, 0); 53 | case 2: 54 | return Color(0, 255, f); 55 | case 3: 56 | return Color(0, 255 - f, 255); 57 | case 4: 58 | return Color(f, 0, 255); 59 | case 5: 60 | return Color(255, 0, 255 - f); 61 | } 62 | return Color(); 63 | } 64 | 65 | Color Color::cos(float hue) { 66 | return Color((std::cos(hue * 1.047f) + 1) * 127.9f, 67 | (std::cos((hue - 2) * 1.047f) + 1) * 127.9f, 68 | (std::cos((hue - 4) * 1.047f) + 1) * 127.9f); 69 | } 70 | 71 | float Color::hue() const { 72 | auto min = std::min(std::min(r, g), b); 73 | auto max = std::max(std::max(r, g), b); 74 | if (min == max) { 75 | return 0; 76 | } 77 | auto hue = 0.f; 78 | if (r == max) { 79 | hue = (g - b) / (float)(max - min); 80 | } else if (g == max) { 81 | hue = 2.f + (b - r) / (float)(max - min); 82 | } else { 83 | hue = 4.f + (r - g) / (float)(max - min); 84 | } 85 | if (hue < 0) { 86 | hue += 6; 87 | } 88 | return hue; 89 | } 90 | 91 | } // namespace cvplot 92 | -------------------------------------------------------------------------------- /src/cvplot/internal.h: -------------------------------------------------------------------------------- 1 | #ifndef CVPLOT_INTERNAL_H 2 | #define CVPLOT_INTERNAL_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #define EXPECT_EQ(a__, b__) \ 10 | do { \ 11 | if ((a__) != (b__)) { \ 12 | std::cerr << "Incorrect " << #a__ << " (" << (a__) << "), should equal " \ 13 | << (b__) << std::endl; \ 14 | exit(-1); \ 15 | } \ 16 | } while (0) 17 | 18 | namespace cvplot { 19 | 20 | static const int paleness = 32; 21 | 22 | static uint8_t channel2pale(uint8_t c) { 23 | return c * (255 - 2 * paleness) / 255 + paleness; 24 | } 25 | 26 | static cv::Scalar color2scalar(const Color &color) { 27 | return cv::Scalar(channel2pale(color.b), channel2pale(color.g), 28 | channel2pale(color.r)); 29 | } 30 | 31 | static float value2snap(float value) { 32 | return std::max({pow(10, floor(log10(value))), 33 | pow(10, floor(log10(value / 2))) * 2, 34 | pow(10, floor(log10(value / 5))) * 5}); 35 | } 36 | 37 | class Trans { 38 | public: 39 | Trans(void *buffer) : Trans(*(cv::Mat *)buffer) {} 40 | 41 | Trans(cv::Mat &buffer) : original_(buffer), alpha_(0), interim_(NULL) {} 42 | 43 | Trans(cv::Mat &buffer, int alpha) : Trans(buffer) { setup(alpha); } 44 | 45 | ~Trans() { flush(); } 46 | 47 | cv::Mat &get() const { return (interim_ != NULL ? *interim_ : original_); } 48 | 49 | void setup(int alpha) { 50 | bool transparent = (alpha != 255); 51 | if (transparent) { 52 | interim_ = new cv::Mat(); 53 | original_.copyTo(*interim_); 54 | } 55 | alpha_ = alpha; 56 | } 57 | 58 | void flush() { 59 | if (interim_) { 60 | // std::cerr << "blending " << alpha_ << std::endl; 61 | auto weight = alpha_ / 255.f; 62 | cv::addWeighted(*interim_, weight, original_, 1 - weight, 0, original_); 63 | delete interim_; 64 | interim_ = NULL; 65 | } 66 | } 67 | 68 | cv::Mat &with(int alpha) { 69 | if (alpha != alpha_) { 70 | flush(); 71 | setup(alpha); 72 | } 73 | return get(); 74 | } 75 | 76 | cv::Mat &with(const Color &color) { return with(color.a); } 77 | 78 | protected: 79 | int alpha_; 80 | cv::Mat &original_; 81 | cv::Mat *interim_; 82 | }; 83 | 84 | } // namespace cvplot 85 | 86 | #endif // CVPLOT_INTERNAL_H 87 | -------------------------------------------------------------------------------- /tools/cube_segmentation.m: -------------------------------------------------------------------------------- 1 | %% script for color cubes segmentation in Oxford Multi-motion Dataset (https://robotic-esp.com/datasets/omd/) 2 | 3 | % setthings 4 | dataset = 'swinging_4_unconstrained'; 5 | display = 1; 6 | saveOutput = 1; 7 | 8 | % path 9 | folder_path = '/omd/swinging_4_unconstrained_stereo/'; 10 | stereo_path = strcat(folder_path, '/image_0/'); 11 | segmentation_path = strcat(folder_path, '/mask/'); 12 | 13 | file_pattern = fullfile(stereo_path, '*.png'); 14 | rgb_files = dir(file_pattern); 15 | figure; 16 | for i = 1:length(rgb_files) 17 | i 18 | base_frame_name = rgb_files(i).name; 19 | full_frame_name = fullfile(stereo_path, base_frame_name); 20 | 21 | %read rgb image 22 | I_rgb = imread(strcat(full_frame_name)); 23 | 24 | %foreground-background segmentation -Otsu's method 25 | I_gray = rgb2gray(I_rgb); 26 | T = graythresh(I_gray); 27 | mask_fore_back = I_gray>0.78*T.*255; 28 | 29 | %hsv color segmentation 30 | I = I_rgb.*repmat(uint8(mask_fore_back),[1,1,3]); 31 | I_hsv = rgb2hsv(I); 32 | mask_hsv = I_hsv(:,:,2)>0.28; 33 | 34 | masked_I = I_rgb.*repmat(uint8(mask_hsv),[1,1,3]); 35 | 36 | se = strel('cube',9); 37 | eroded_mask_hsv = imerode(mask_hsv,se); 38 | 39 | se = strel('cube',38); 40 | dilated_mask_hsv = imdilate(eroded_mask_hsv,se); 41 | 42 | %post process - will give great mask, multiple labels per mask 43 | %final_mask = dilated_mask_hsv & mask_hsv; 44 | %labels = bwlabel(final_mask); 45 | 46 | %post process - will give great mask, multiple labels per mask 47 | labels = bwlabel(dilated_mask_hsv); 48 | 49 | uv = unique(labels); 50 | counts = histc(reshape(labels,1,size(labels,1)*size(labels,2))',uv); 51 | 52 | for k=1:size(uv,1) 53 | if (counts(k)<10000) 54 | labels(labels == uv(k)) = 0; 55 | end 56 | end 57 | 58 | % uv_new = unique(labels)' 59 | 60 | if saveOutput 61 | dlmwrite(strcat(segmentation_path,base_frame_name(1:end-4),'_labels.txt'),labels,'delimiter','\t','newline','pc'); 62 | end 63 | 64 | if display 65 | labelled_I = labeloverlay(double(mask_hsv),labels); 66 | % figure; imshow(I_rgb); title('original RGB image') 67 | % figure; imshow(mask_fore_back); title('foreground-background segmentation - Otsu Method') 68 | % figure; imshow(mask_hsv); title('cube segmentation - HSV color thresholding') 69 | % figure; imshow(masked_I); title('segmented image') 70 | % figure; imshow(eroded_mask_hsv); title('eroded mask') 71 | % figure; imshow(dilated_mask_hsv); title('dilated mask') 72 | imshow(labelled_I); title('labelled mask') 73 | end 74 | end --------------------------------------------------------------------------------