├── CMakeLists.txt ├── CMakeModules └── FindEigen.cmake ├── LICENCE ├── README.md ├── build.sh ├── include └── hso │ ├── CoarseTracker.h │ ├── ImageReader.h │ ├── IndexThreadReduce.h │ ├── MatrixAccumulator.h │ ├── bundle_adjustment.h │ ├── camera.h │ ├── config.h │ ├── depth_filter.h │ ├── feature.h │ ├── feature_alignment.h │ ├── feature_detection.h │ ├── frame.h │ ├── frame_handler_base.h │ ├── frame_handler_mono.h │ ├── global.h │ ├── initialization.h │ ├── map.h │ ├── matcher.h │ ├── point.h │ ├── pose_optimizer.h │ ├── reprojector.h │ ├── viewer.h │ └── vikit │ ├── aligned_mem.h │ ├── homography.h │ ├── math_utils.h │ ├── patch_score.h │ ├── performance_monitor.h │ ├── ringbuffer.h │ ├── robust_cost.h │ ├── timer.h │ └── vision.h ├── src ├── CoarseTracker.cpp ├── ImageReader.cpp ├── bundle_adjustment.cpp ├── camera.cpp ├── config.cpp ├── depth_filter.cpp ├── feature_alignment.cpp ├── feature_detection.cpp ├── frame.cpp ├── frame_handler_base.cpp ├── frame_handler_mono.cpp ├── initialization.cpp ├── map.cpp ├── matcher.cpp ├── point.cpp ├── pose_optimizer.cpp ├── reprojector.cpp ├── viewer.cpp └── vikit │ ├── homography.cpp │ ├── math_utils.cpp │ ├── performance_monitor.cpp │ ├── robust_cost.cpp │ └── vision.cpp ├── test ├── cameras │ ├── euroc.txt │ ├── icl-nuim.txt │ ├── tum_mono_vo_narrow.txt │ └── tum_mono_vo_wide.txt ├── euroc_batch.sh ├── icl-nuim_batch.sh ├── result │ └── KeyFrameTrajectory.txt ├── single_sequence.sh ├── test_dataset.cpp ├── timestamp │ ├── L0.txt │ ├── L1.txt │ ├── L2.txt │ ├── L3.txt │ ├── MH01.txt │ ├── MH02.txt │ ├── MH03.txt │ ├── MH04.txt │ ├── MH05.txt │ ├── O0.txt │ ├── O1.txt │ ├── O2.txt │ ├── O3.txt │ ├── V101.txt │ ├── V102.txt │ ├── V103.txt │ ├── V201.txt │ ├── V202.txt │ └── V203.txt └── tum_monoVO_batch.sh └── thirdparty ├── Pangolin-master.zip ├── Sophus ├── CMakeLists.txt ├── README ├── SophusConfig.cmake.in ├── cmake_modules │ └── FindEigen3.cmake └── sophus │ ├── scso3.cpp │ ├── scso3.h │ ├── se2.cpp │ ├── se2.h │ ├── se3.cpp │ ├── se3.h │ ├── sim3.cpp │ ├── sim3.h │ ├── so2.cpp │ ├── so2.h │ ├── so3.cpp │ ├── so3.h │ ├── test_scso3.cpp │ ├── test_se2.cpp │ ├── test_se3.cpp │ ├── test_sim3.cpp │ ├── test_so2.cpp │ └── test_so3.cpp ├── fast ├── CMakeLists.txt ├── LICENSE ├── README.md ├── fastConfig.cmake.in ├── include │ └── fast │ │ ├── corner_10.h │ │ ├── corner_9.h │ │ ├── fast.h │ │ └── faster_corner_utilities.h ├── package.xml └── src │ ├── fast_10.cpp │ ├── fast_10_score.cpp │ ├── fast_12_detect.cpp │ ├── fast_12_score.cpp │ ├── fast_7_detect.cpp │ ├── fast_7_score.cpp │ ├── fast_8_detect.cpp │ ├── fast_8_score.cpp │ ├── fast_9.cpp │ ├── fast_9_score.cpp │ ├── faster_corner_10_sse.cpp │ ├── faster_corner_9_sse.cpp │ └── nonmax_3x3.cpp └── g2o ├── CMakeLists.txt ├── README.txt ├── cmake_modules ├── FindBLAS.cmake ├── FindEigen3.cmake └── FindLAPACK.cmake ├── config.h ├── config.h.in ├── g2o ├── core │ ├── base_binary_edge.h │ ├── base_binary_edge.hpp │ ├── base_edge.h │ ├── base_multi_edge.h │ ├── base_multi_edge.hpp │ ├── base_unary_edge.h │ ├── base_unary_edge.hpp │ ├── base_vertex.h │ ├── base_vertex.hpp │ ├── batch_stats.cpp │ ├── batch_stats.h │ ├── block_solver.h │ ├── block_solver.hpp │ ├── cache.cpp │ ├── cache.h │ ├── creators.h │ ├── eigen_types.h │ ├── estimate_propagator.cpp │ ├── estimate_propagator.h │ ├── factory.cpp │ ├── factory.h │ ├── hyper_dijkstra.cpp │ ├── hyper_dijkstra.h │ ├── hyper_graph.cpp │ ├── hyper_graph.h │ ├── hyper_graph_action.cpp │ ├── hyper_graph_action.h │ ├── jacobian_workspace.cpp │ ├── jacobian_workspace.h │ ├── linear_solver.h │ ├── marginal_covariance_cholesky.cpp │ ├── marginal_covariance_cholesky.h │ ├── matrix_operations.h │ ├── matrix_structure.cpp │ ├── matrix_structure.h │ ├── openmp_mutex.h │ ├── optimizable_graph.cpp │ ├── optimizable_graph.h │ ├── optimization_algorithm.cpp │ ├── optimization_algorithm.h │ ├── optimization_algorithm_dogleg.cpp │ ├── optimization_algorithm_dogleg.h │ ├── optimization_algorithm_factory.cpp │ ├── optimization_algorithm_factory.h │ ├── optimization_algorithm_gauss_newton.cpp │ ├── optimization_algorithm_gauss_newton.h │ ├── optimization_algorithm_levenberg.cpp │ ├── optimization_algorithm_levenberg.h │ ├── optimization_algorithm_property.h │ ├── optimization_algorithm_with_hessian.cpp │ ├── optimization_algorithm_with_hessian.h │ ├── parameter.cpp │ ├── parameter.h │ ├── parameter_container.cpp │ ├── parameter_container.h │ ├── robust_kernel.cpp │ ├── robust_kernel.h │ ├── robust_kernel_factory.cpp │ ├── robust_kernel_factory.h │ ├── robust_kernel_impl.cpp │ ├── robust_kernel_impl.h │ ├── solver.cpp │ ├── solver.h │ ├── sparse_block_matrix.h │ ├── sparse_block_matrix.hpp │ ├── sparse_block_matrix_ccs.h │ ├── sparse_block_matrix_diagonal.h │ ├── sparse_block_matrix_test.cpp │ ├── sparse_optimizer.cpp │ └── sparse_optimizer.h ├── solvers │ ├── linear_solver_dense.h │ └── linear_solver_eigen.h ├── stuff │ ├── color_macros.h │ ├── g2o_stuff_api.h │ ├── macros.h │ ├── misc.h │ ├── os_specific.c │ ├── os_specific.h │ ├── property.cpp │ ├── property.h │ ├── sparse_helper.cpp │ ├── sparse_helper.h │ ├── string_tools.cpp │ ├── string_tools.h │ ├── timeutil.cpp │ └── timeutil.h └── types │ ├── se3_ops.h │ ├── se3_ops.hpp │ ├── se3quat.h │ ├── sim3.h │ ├── types_sba.cpp │ ├── types_sba.h │ ├── types_seven_dof_expmap.cpp │ ├── types_seven_dof_expmap.h │ ├── types_six_dof_expmap.cpp │ └── types_six_dof_expmap.h └── license-bsd.txt /CMakeModules/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # 3 | # CMake script for finding the Eigen library. 4 | # 5 | # http://eigen.tuxfamily.org/index.php?title=Main_Page 6 | # 7 | # Copyright (c) 2006, 2007 Montel Laurent, 8 | # Copyright (c) 2008, 2009 Gael Guennebaud, 9 | # Copyright (c) 2009 Benoit Jacob 10 | # Redistribution and use is allowed according to the terms of the 2-clause BSD 11 | # license. 12 | # 13 | # 14 | # Input variables: 15 | # 16 | # - Eigen_ROOT_DIR (optional): When specified, header files and libraries 17 | # will be searched for in `${Eigen_ROOT_DIR}/include` and 18 | # `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order 19 | # will be ignored. When unspecified, the default CMake search order is used. 20 | # This variable can be specified either as a CMake or environment variable. 21 | # If both are set, preference is given to the CMake variable. 22 | # Use this variable for finding packages installed in a nonstandard location, 23 | # or for enforcing that one of multiple package installations is picked up. 24 | # 25 | # Cache variables (not intended to be used in CMakeLists.txt files) 26 | # 27 | # - Eigen_INCLUDE_DIR: Absolute path to package headers. 28 | # 29 | # 30 | # Output variables: 31 | # 32 | # - Eigen_FOUND: Boolean that indicates if the package was found 33 | # - Eigen_INCLUDE_DIRS: Paths to the necessary header files 34 | # - Eigen_VERSION: Version of Eigen library found 35 | # - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen 36 | # 37 | # 38 | # Example usage: 39 | # 40 | # # Passing the version means Eigen_FOUND will only be TRUE if a 41 | # # version >= the provided version is found. 42 | # find_package(Eigen 3.1.2) 43 | # if(NOT Eigen_FOUND) 44 | # # Error handling 45 | # endif() 46 | # ... 47 | # add_definitions(${Eigen_DEFINITIONS}) 48 | # ... 49 | # include_directories(${Eigen_INCLUDE_DIRS} ...) 50 | # 51 | ############################################################################### 52 | 53 | find_package(PkgConfig) 54 | pkg_check_modules(PC_EIGEN eigen3) 55 | set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) 56 | 57 | 58 | find_path(EIGEN_INCLUDE_DIR Eigen/Core 59 | HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} 60 | "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" 61 | "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility 62 | PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" 63 | "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" 64 | PATH_SUFFIXES eigen3 include/eigen3 include) 65 | 66 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 67 | 68 | include(FindPackageHandleStandardArgs) 69 | find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) 70 | 71 | mark_as_advanced(EIGEN_INCLUDE_DIR) 72 | 73 | if(EIGEN_FOUND) 74 | message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") 75 | endif(EIGEN_FOUND) 76 | 77 | 78 | set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 79 | set(Eigen_FOUND ${EIGEN_FOUND}) 80 | set(Eigen_VERSION ${EIGEN_VERSION}) 81 | set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) 82 | -------------------------------------------------------------------------------- /build.sh: -------------------------------------------------------------------------------- 1 | echo "Configuring and building thirdparty/fast ..." 2 | 3 | cd thirdparty/fast 4 | mkdir build 5 | cd build 6 | cmake .. -DCMAKE_BUILD_TYPE=Release 7 | make -j 8 | 9 | cd ../../Sophus 10 | 11 | echo "Configuring and building thirdparty/Sophus ..." 12 | 13 | mkdir build 14 | cd build 15 | cmake .. -DCMAKE_BUILD_TYPE=Release 16 | make -j 17 | 18 | cd ../../g2o 19 | 20 | echo "Configuring and building thirdparty/g2o ..." 21 | 22 | mkdir build 23 | cd build 24 | cmake .. -DCMAKE_BUILD_TYPE=Release 25 | make -j 26 | 27 | cd ../../../ 28 | 29 | echo "Configuring and building HSO ..." 30 | 31 | mkdir build 32 | cd build 33 | cmake .. -DCMAKE_BUILD_TYPE=Release 34 | make -j 35 | -------------------------------------------------------------------------------- /include/hso/ImageReader.h: -------------------------------------------------------------------------------- 1 | // 2 | // ImageReader.h 3 | // OnlinePhotometricCalibration 4 | // 5 | // Created by Paul on 16.11.17. 6 | // Copyright (c) 2017-2018 Paul Bergmann and co-authors. All rights reserved. 7 | // 8 | // See LICENSE.txt 9 | // 10 | 11 | #pragma once 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace hso { 19 | 20 | /** 21 | * Read input images from image files 22 | * Resizes the images if requested 23 | */ 24 | 25 | class ImageReader 26 | { 27 | public: 28 | 29 | /** 30 | * Initialize the image reader 31 | * @param image_folder Image folder 32 | * @param new_size Resize input images to new_size 33 | */ 34 | ImageReader(std::string image_folder, cv::Size new_size, std::string time_folder="None"); 35 | 36 | /** 37 | * Read a new input image from the hard drive and return it 38 | * 39 | * @param Input image index to read 40 | * @return Read input image 41 | */ 42 | cv::Mat readImage(int image_index); 43 | std::string readStamp(int image_index); 44 | 45 | int getNumImages() { return (int)m_files.size(); } 46 | 47 | int getDir(std::string dir, std::vector &files); 48 | 49 | inline bool stampValid() { return m_stamp_valid; } 50 | 51 | private: 52 | 53 | /** 54 | * Resize images to this size 55 | */ 56 | cv::Size m_img_new_size; 57 | 58 | bool m_stamp_valid; 59 | 60 | std::vector m_files; 61 | std::vector m_times; 62 | }; 63 | } 64 | -------------------------------------------------------------------------------- /include/hso/MatrixAccumulator.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | 26 | #include "hso/global.h" 27 | #include 28 | 29 | class Accumulator7 30 | { 31 | public: 32 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 33 | 34 | Eigen::Matrix H; 35 | Eigen::Matrix b; 36 | size_t num; 37 | 38 | inline void initialize() 39 | { 40 | H.setZero(); 41 | b.setZero(); 42 | std::memset(SSEData, 0, sizeof(float)*4*28); 43 | std::memset(SSEData1k, 0, sizeof(float)*4*28); 44 | std::memset(SSEData1m, 0, sizeof(float)*4*28); 45 | num = numIn1 = numIn1k = numIn1m = 0; 46 | } 47 | 48 | inline void finish() 49 | { 50 | shiftUp(true); 51 | assert(numIn1==0); 52 | assert(numIn1k==0); 53 | 54 | int idx=0; 55 | for(int r=0;r<7;r++) 56 | for(int c=r;c<7;c++) 57 | { 58 | float d = SSEData1m[idx+0] + SSEData1m[idx+1] + SSEData1m[idx+2] + SSEData1m[idx+3]; 59 | H(r,c) = H(c,r) = d; 60 | idx+=4; 61 | } 62 | assert(idx==4*28); 63 | } 64 | 65 | inline void updateSingleWeighted( 66 | float J0, float J1, 67 | float J2, float J3, 68 | float J4, float J5, 69 | float J6, 70 | float w, int off=0) 71 | { 72 | float* pt=SSEData+off; 73 | *pt += J0*J0*w; pt+=4; J0*=w; 74 | *pt += J1*J0; pt+=4; 75 | *pt += J2*J0; pt+=4; 76 | *pt += J3*J0; pt+=4; 77 | *pt += J4*J0; pt+=4; 78 | *pt += J5*J0; pt+=4; 79 | *pt += J6*J0; pt+=4; 80 | 81 | *pt += J1*J1*w; pt+=4; J1*=w; 82 | *pt += J2*J1; pt+=4; 83 | *pt += J3*J1; pt+=4; 84 | *pt += J4*J1; pt+=4; 85 | *pt += J5*J1; pt+=4; 86 | *pt += J6*J1; pt+=4; 87 | 88 | *pt += J2*J2*w; pt+=4; J2*=w; 89 | *pt += J3*J2; pt+=4; 90 | *pt += J4*J2; pt+=4; 91 | *pt += J5*J2; pt+=4; 92 | *pt += J6*J2; pt+=4; 93 | 94 | *pt += J3*J3*w; pt+=4; J3*=w; 95 | *pt += J4*J3; pt+=4; 96 | *pt += J5*J3; pt+=4; 97 | *pt += J6*J3; pt+=4; 98 | 99 | *pt += J4*J4*w; pt+=4; J4*=w; 100 | *pt += J5*J4; pt+=4; 101 | *pt += J6*J4; pt+=4; 102 | 103 | *pt += J5*J5*w; pt+=4; J5*=w; 104 | *pt += J6*J5; pt+=4; 105 | 106 | *pt += J6*J6*w; pt+=4; 107 | 108 | num++; 109 | numIn1++; 110 | shiftUp(false); 111 | } 112 | 113 | private: 114 | EIGEN_ALIGN16 float SSEData[4*28]; 115 | EIGEN_ALIGN16 float SSEData1k[4*28]; 116 | EIGEN_ALIGN16 float SSEData1m[4*28]; 117 | float numIn1, numIn1k, numIn1m; 118 | 119 | void shiftUp(bool force) 120 | { 121 | if(numIn1 > 1000 || force) 122 | { 123 | for(int i=0;i<28;i++) 124 | _mm_store_ps(SSEData1k+4*i, _mm_add_ps(_mm_load_ps(SSEData+4*i),_mm_load_ps(SSEData1k+4*i))); 125 | 126 | numIn1k+=numIn1; 127 | numIn1=0; 128 | std::memset(SSEData,0, sizeof(float)*4*28); 129 | } 130 | 131 | if(numIn1k > 1000 || force) 132 | { 133 | for(int i=0;i<28;i++) 134 | _mm_store_ps(SSEData1m+4*i, _mm_add_ps(_mm_load_ps(SSEData1k+4*i),_mm_load_ps(SSEData1m+4*i))); 135 | 136 | numIn1m+=numIn1k; 137 | numIn1k=0; 138 | std::memset(SSEData1k,0, sizeof(float)*4*28); 139 | } 140 | } 141 | }; 142 | 143 | -------------------------------------------------------------------------------- /include/hso/feature_alignment.h: -------------------------------------------------------------------------------- 1 | // This file is part of HSO: Hybrid Sparse Monocular Visual Odometry 2 | // With Online Photometric Calibration 3 | // 4 | // Copyright(c) 2021, Dongting Luo, Dalian University of Technology, Dalian 5 | // Copyright(c) 2021, Robotics Group, Dalian University of Technology 6 | // 7 | // This program is highly based on the previous implementation 8 | // of SVO: https://github.com/uzh-rpg/rpg_svo 9 | // and PL-SVO: https://github.com/rubengooj/pl-svo 10 | // 11 | // This program is free software: you can redistribute it and/or modify 12 | // it under the terms of the GNU General Public License as published by 13 | // the Free Software Foundation, either version 3 of the License, or 14 | // (at your option) any later version. 15 | // 16 | // This program is distributed in the hope that it will be useful, 17 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | // GNU General Public License for more details. 20 | // 21 | // You should have received a copy of the GNU General Public License 22 | // along with this program. If not, see . 23 | 24 | #ifndef HSO_FEATURE_ALIGNMENT_H_ 25 | #define HSO_FEATURE_ALIGNMENT_H_ 26 | 27 | #include 28 | 29 | namespace hso { 30 | 31 | class Point; 32 | struct Feature; 33 | /// Subpixel refinement of a reference feature patch with the current image. 34 | /// Implements the inverse-compositional approach (see "Lucas-Kanade 20 Years on" 35 | /// paper by Baker. 36 | namespace feature_alignment { 37 | 38 | bool align1D( 39 | const cv::Mat& cur_img, 40 | const Vector2f& dir, // direction in which the patch is allowed to move 41 | uint8_t* ref_patch_with_border, 42 | uint8_t* ref_patch, 43 | const int n_iter, 44 | Vector2d& cur_px_estimate, 45 | double& h_inv); 46 | 47 | bool align1D( 48 | const cv::Mat& cur_img, 49 | const Vector2f& dir, // direction in which the patch is allowed to move 50 | float* ref_patch_with_border, 51 | float* ref_patch, 52 | const int n_iter, 53 | Vector2d& cur_px_estimate, 54 | double& h_inv, 55 | float* cur_patch = NULL); 56 | 57 | bool align2D( 58 | const cv::Mat& cur_img, 59 | uint8_t* ref_patch_with_border, 60 | uint8_t* ref_patch, 61 | const int n_iter, 62 | Vector2d& cur_px_estimate, 63 | bool no_simd = true, 64 | float* cur_patch = NULL); 65 | 66 | bool align2D( 67 | const cv::Mat& cur_img, 68 | float* ref_patch_with_border, 69 | float* ref_patch, 70 | const int n_iter, 71 | Vector2d& cur_px_estimate, 72 | bool no_simd = true, 73 | float* cur_patch = NULL); 74 | 75 | bool align2D_SSE2( 76 | const cv::Mat& cur_img, 77 | uint8_t* ref_patch_with_border, 78 | uint8_t* ref_patch, 79 | const int n_iter, 80 | Vector2d& cur_px_estimate); 81 | 82 | bool align2D_NEON( 83 | const cv::Mat& cur_img, 84 | uint8_t* ref_patch_with_border, 85 | uint8_t* ref_patch, 86 | const int n_iter, 87 | Vector2d& cur_px_estimate); 88 | 89 | } // namespace feature_alignment 90 | } // namespace hso 91 | 92 | #endif // HSO_FEATURE_ALIGNMENT_H_ 93 | -------------------------------------------------------------------------------- /include/hso/pose_optimizer.h: -------------------------------------------------------------------------------- 1 | // This file is part of HSO: Hybrid Sparse Monocular Visual Odometry 2 | // With Online Photometric Calibration 3 | // 4 | // Copyright(c) 2021, Dongting Luo, Dalian University of Technology, Dalian 5 | // Copyright(c) 2021, Robotics Group, Dalian University of Technology 6 | // 7 | // This program is highly based on the previous implementation 8 | // of SVO: https://github.com/uzh-rpg/rpg_svo 9 | // and PL-SVO: https://github.com/rubengooj/pl-svo 10 | // 11 | // This program is free software: you can redistribute it and/or modify 12 | // it under the terms of the GNU General Public License as published by 13 | // the Free Software Foundation, either version 3 of the License, or 14 | // (at your option) any later version. 15 | // 16 | // This program is distributed in the hope that it will be useful, 17 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | // GNU General Public License for more details. 20 | // 21 | // You should have received a copy of the GNU General Public License 22 | // along with this program. If not, see . 23 | 24 | #ifndef HSO_POSE_OPTIMIZER_H_ 25 | #define HSO_POSE_OPTIMIZER_H_ 26 | 27 | #include 28 | #include 29 | 30 | 31 | namespace hso { 32 | 33 | using namespace Eigen; 34 | using namespace Sophus; 35 | using namespace std; 36 | 37 | typedef Matrix Matrix6d; 38 | typedef Matrix Matrix26d; 39 | typedef Matrix Vector6d; 40 | 41 | 42 | /// Motion-only bundle adjustment. Minimize the reprojection error of a single frame. 43 | namespace pose_optimizer { 44 | 45 | void optimizeGaussNewton( 46 | const double reproj_thresh, 47 | const size_t n_iter, 48 | const bool verbose, 49 | FramePtr& frame, 50 | double& estimated_scale, 51 | double& error_init, 52 | double& error_final, 53 | size_t& num_obs); 54 | 55 | 56 | void optimizeLevenbergMarquardt2nd( 57 | const double reproj_thresh, const size_t n_iter, const bool verbose, 58 | FramePtr& frame, double& estimated_scale, double& error_init, double& error_final, 59 | size_t& num_obs); 60 | 61 | void optimizeLevenbergMarquardt3rd( 62 | const double reproj_thresh, const size_t n_iter, const bool verbose, 63 | FramePtr& frame, double& estimated_scale, double& error_init, double& error_final, 64 | size_t& num_obs); 65 | 66 | void optimizeLevenbergMarquardtMagnitude( 67 | const double reproj_thresh, const size_t n_iter, const bool verbose, 68 | FramePtr& frame, double& estimated_scale, double& error_init, double& error_final, 69 | size_t& num_obs); 70 | 71 | 72 | // distribution 73 | static int residual_buffer[10000]={0}; 74 | 75 | } // namespace pose_optimizer 76 | } // namespace hso 77 | 78 | #endif // HSO_POSE_OPTIMIZER_H_ 79 | -------------------------------------------------------------------------------- /include/hso/viewer.h: -------------------------------------------------------------------------------- 1 | // This file is part of HSO: Hybrid Sparse Monocular Visual Odometry 2 | // With Online Photometric Calibration 3 | // 4 | // Copyright(c) 2021, Dongting Luo, Dalian University of Technology, Dalian 5 | // Copyright(c) 2021, Robotics Group, Dalian University of Technology 6 | // 7 | // This program is highly based on the previous implementation 8 | // of SVO: https://github.com/uzh-rpg/rpg_svo 9 | // and PL-SVO: https://github.com/rubengooj/pl-svo 10 | // 11 | // This program is free software: you can redistribute it and/or modify 12 | // it under the terms of the GNU General Public License as published by 13 | // the Free Software Foundation, either version 3 of the License, or 14 | // (at your option) any later version. 15 | // 16 | // This program is distributed in the hope that it will be useful, 17 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | // GNU General Public License for more details. 20 | // 21 | // You should have received a copy of the GNU General Public License 22 | // along with this program. If not, see . 23 | 24 | /** 25 | * modified file in ORB-SLAM (https://github.com/raulmur/ORB_SLAM) 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | namespace hso { 36 | class Frame; 37 | class Map; 38 | class FrameHandlerMono; } 39 | 40 | 41 | namespace hso { 42 | 43 | class Viewer 44 | { 45 | public: 46 | Viewer(hso::FrameHandlerMono* vo); 47 | void run(); 48 | bool CheckFinish(); 49 | void DrawKeyFrames(const bool bDrawKF); 50 | void DrawMapRegionPoints(); 51 | void DrawMapSeeds(); 52 | void DrawConstraints(); 53 | 54 | void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M); 55 | void DrawCurrentCamera(pangolin::OpenGlMatrix &Twc); 56 | 57 | private: 58 | hso::FrameHandlerMono* _vo; 59 | 60 | std::mutex mMutexCurrentPose; 61 | std::vector< Sophus::SE3 > _pos; 62 | Sophus::SE3 _CurrentPoseTwc ; 63 | int _drawedframeID=0; 64 | 65 | void SetFinish(); 66 | bool mbFinished; 67 | std::mutex mMutexFinish; 68 | 69 | float mKeyFrameSize; 70 | float mKeyFrameLineWidth; 71 | float mGraphLineWidth; 72 | float mPointSize; 73 | float mCameraSize; 74 | float mCameraLineWidth; 75 | 76 | float mViewpointX, mViewpointY, mViewpointZ, mViewpointF; 77 | }; 78 | } -------------------------------------------------------------------------------- /include/hso/vikit/aligned_mem.h: -------------------------------------------------------------------------------- 1 | /* 2 | * aligned_mem.h 3 | * 4 | * Created on: May 14, 2013 5 | * Author: cforster 6 | * 7 | * Code from libcvd: cvd/internal/aligned_mem.h 8 | * https://github.com/edrosten/libcvd 9 | * Licence: LGPL 2.1 10 | */ 11 | 12 | #pragma once 13 | 14 | #include // memset 15 | #include // memset 16 | #include 17 | #include 18 | 19 | namespace hso { 20 | namespace aligned_mem { 21 | 22 | /// Check if the pointer is aligned to the specified byte granularity 23 | inline bool 24 | is_aligned8(const void* ptr) 25 | { 26 | return ((reinterpret_cast(ptr)) & 0x7) == 0; 27 | } 28 | 29 | inline bool 30 | is_aligned16(const void* ptr) 31 | { 32 | return ((reinterpret_cast(ptr)) & 0xF) == 0; 33 | } 34 | 35 | template struct placement_delete 36 | { 37 | enum { Size = (1<= Size) { 51 | placement_delete::free(buf+Size,M-Size); 52 | placement_delete::destruct(buf); 53 | } else { 54 | placement_delete::free(buf, M); 55 | } 56 | } 57 | }; 58 | 59 | template struct placement_delete 60 | { 61 | static inline void free(T*, size_t ) {} 62 | }; 63 | 64 | inline void * aligned_alloc(size_t count, size_t alignment){ 65 | void * mem = NULL; 66 | assert(posix_memalign(&mem, alignment, count) == 0); 67 | return mem; 68 | } 69 | 70 | inline void aligned_free(void * memory) { 71 | free(memory); 72 | } 73 | 74 | template 75 | inline T * aligned_alloc(size_t count, size_t alignment){ 76 | void * data = aligned_alloc(sizeof(T)* count, alignment); 77 | return new (data) T[count]; 78 | } 79 | 80 | template 81 | inline void aligned_free(T * memory, size_t count){ 82 | placement_delete::free(memory, count); 83 | aligned_free(memory); 84 | } 85 | 86 | template inline void memfill(T* data, int n, const T val) 87 | { 88 | T* de = data + n; 89 | for(;data < de; data++) 90 | *data=val; 91 | } 92 | 93 | template<> inline void memfill(unsigned char* data, int n, const unsigned char val) 94 | { 95 | memset(data, val, n); 96 | } 97 | 98 | template<> inline void memfill(signed char* data, int n, const signed char val) 99 | { 100 | memset(data, val, n); 101 | } 102 | 103 | template<> inline void memfill(char* data, int n, const char val) 104 | { 105 | memset(data, val, n); 106 | } 107 | 108 | template 109 | struct AlignedMem { 110 | T* mem; 111 | size_t count; 112 | AlignedMem(size_t c) : count(c) { 113 | mem = aligned_alloc(count, N); 114 | } 115 | ~AlignedMem() { 116 | aligned_free(mem, count); 117 | } 118 | T* data() { return mem; } 119 | const T* data() const { return mem; } 120 | }; 121 | 122 | } // namespace aligned_mem 123 | } // namespace vikit 124 | -------------------------------------------------------------------------------- /include/hso/vikit/homography.h: -------------------------------------------------------------------------------- 1 | /* 2 | * homography.cpp 3 | * Adaptation of PTAM-GPL HomographyInit class. 4 | * https://github.com/Oxford-PTAM/PTAM-GPL 5 | * Licence: GPLv3 6 | * Copyright 2008 Isis Innovation Limited 7 | * 8 | * Created on: Sep 2, 2012 9 | * by: cforster 10 | * 11 | * This class implements the homography decomposition of Faugeras and Lustman's 12 | * 1988 tech report. Code converted to Eigen from PTAM. 13 | * 14 | */ 15 | 16 | #pragma once 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "hso/vikit/math_utils.h" 23 | 24 | namespace hso { 25 | 26 | using namespace Eigen; 27 | using namespace std; 28 | 29 | struct HomographyDecomposition 30 | { 31 | Vector3d t; 32 | Matrix3d R; 33 | double d; 34 | Vector3d n; 35 | 36 | // Resolved Composition 37 | Sophus::SE3 T; //!< second from first 38 | int score; 39 | }; 40 | 41 | class Homography 42 | { 43 | public: 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 45 | 46 | Homography (const vector >& _fts1, 47 | const vector >& _fts2, 48 | double _error_multiplier2, 49 | double _thresh_in_px); 50 | 51 | void 52 | calcFromPlaneParams (const Vector3d & normal, 53 | const Vector3d & point_on_plane); 54 | 55 | void 56 | calcFromMatches (); 57 | 58 | size_t 59 | computeMatchesInliers (); 60 | 61 | bool 62 | computeSE3fromMatches (); 63 | 64 | bool 65 | decompose (); 66 | 67 | void 68 | findBestDecomposition (); 69 | 70 | double thresh; 71 | double error_multiplier2; 72 | const vector >& fts_c1; //!< Features on first image on unit plane 73 | const vector >& fts_c2; //!< Features on second image on unit plane 74 | vector inliers; 75 | SE3 T_c2_from_c1; //!< Relative translation and rotation of two images 76 | Matrix3d H_c2_from_c1; //!< Homography 77 | vector decompositions; 78 | }; 79 | 80 | 81 | } /* end namespace vk */ 82 | 83 | -------------------------------------------------------------------------------- /include/hso/vikit/performance_monitor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * performance_monitor.h 3 | * 4 | * Created on: Aug 26, 2011 5 | * Author: Christian Forster 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "hso/vikit/timer.h" 16 | 17 | namespace hso 18 | { 19 | 20 | struct LogItem 21 | { 22 | double data; 23 | bool set; 24 | }; 25 | 26 | class PerformanceMonitor 27 | { 28 | public: 29 | PerformanceMonitor(); 30 | ~PerformanceMonitor(); 31 | void init(const std::string& trace_name, const std::string& trace_dir); 32 | void addTimer(const std::string& name); 33 | void addLog(const std::string& name); 34 | void writeToFile(); 35 | void startTimer(const std::string& name); 36 | void stopTimer(const std::string& name); 37 | double getTime(const std::string& name) const; 38 | void log(const std::string& name, double data); 39 | 40 | private: 41 | std::map timers_; 42 | std::map logs_; 43 | std::string trace_name_; // 26 | #include 27 | #include 28 | 29 | namespace hso 30 | { 31 | 32 | template 33 | class RingBuffer 34 | { 35 | public: 36 | RingBuffer (int size); 37 | 38 | void 39 | push_back (const T & elem); 40 | 41 | bool 42 | empty () const; 43 | 44 | T 45 | get (int i); 46 | 47 | T 48 | getSum () const; 49 | 50 | T 51 | getMean () const; 52 | 53 | int size() 54 | { 55 | return num_elem_; 56 | } 57 | 58 | private: 59 | std::vector arr_; 60 | int begin_; 61 | int end_; 62 | int num_elem_; 63 | int arr_size_; 64 | }; 65 | 66 | template 67 | RingBuffer 68 | ::RingBuffer(int size) : 69 | arr_(size), 70 | begin_(0), 71 | end_(-1), 72 | num_elem_(0), 73 | arr_size_(size) 74 | {} 75 | 76 | template 77 | bool RingBuffer 78 | ::empty() const 79 | { 80 | return arr_.empty(); 81 | } 82 | 83 | template 84 | void RingBuffer 85 | ::push_back(const T & elem) 86 | { 87 | if (num_elem_ 101 | T RingBuffer 102 | ::get(int i) 103 | { 104 | assert(i 109 | T RingBuffer 110 | ::getSum() const 111 | { 112 | T sum=0; 113 | for(int i=0; i 119 | T RingBuffer 120 | ::getMean() const 121 | { 122 | if(num_elem_ == 0) 123 | return 0; 124 | return getSum()/num_elem_; 125 | } 126 | 127 | } 128 | 129 | 130 | -------------------------------------------------------------------------------- /include/hso/vikit/timer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace hso 8 | { 9 | 10 | class Timer 11 | { 12 | private: 13 | timeval start_time_; 14 | double time_; 15 | double accumulated_; 16 | public: 17 | 18 | /// The constructor directly starts the timer. 19 | Timer() : time_(0), accumulated_(0) 20 | { 21 | start(); 22 | } 23 | 24 | ~Timer() 25 | {} 26 | 27 | inline void start() 28 | { 29 | accumulated_ = 0.0; 30 | gettimeofday(&start_time_, NULL); 31 | } 32 | 33 | inline void resume() 34 | { 35 | gettimeofday(&start_time_, NULL); 36 | } 37 | 38 | inline double stop() 39 | { 40 | timeval end_time; 41 | gettimeofday(&end_time, NULL); 42 | long seconds = end_time.tv_sec - start_time_.tv_sec; 43 | long useconds = end_time.tv_usec - start_time_.tv_usec; 44 | time_ = ((seconds) + useconds*0.000001) + accumulated_; 45 | accumulated_ = time_; 46 | return time_; 47 | } 48 | 49 | inline double getTime() const 50 | { 51 | return time_; 52 | } 53 | 54 | inline void reset() 55 | { 56 | time_ = 0; 57 | accumulated_ = 0; 58 | } 59 | 60 | static double getCurrentTime() 61 | { 62 | timeval time_now; 63 | gettimeofday(&time_now, NULL); 64 | return time_now.tv_sec + time_now.tv_usec*0.000001; 65 | } 66 | 67 | static double getCurrentSecond() 68 | { 69 | timeval time_now; 70 | gettimeofday(&time_now, NULL); 71 | return time_now.tv_sec; 72 | } 73 | 74 | }; 75 | 76 | } // end namespace vk 77 | 78 | -------------------------------------------------------------------------------- /include/hso/vikit/vision.h: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #pragma once 18 | 19 | #include "hso/vikit/aligned_mem.h" 20 | 21 | #include 22 | #include 23 | 24 | namespace hso { 25 | 26 | //! Return value between 0 and 1 27 | //! WARNING This function does not check whether the x/y is within the border 28 | inline float interpolateMat_32f(const cv::Mat& mat, float u, float v) 29 | { 30 | assert(mat.type()==CV_32F); 31 | float x = floor(u); 32 | float y = floor(v); 33 | float subpix_x = u-x; 34 | float subpix_y = v-y; 35 | float wx0 = 1.0-subpix_x; 36 | float wx1 = subpix_x; 37 | float wy0 = 1.0-subpix_y; 38 | float wy1 = subpix_y; 39 | 40 | float val00 = mat.at(y,x); 41 | float val10 = mat.at(y,x+1); 42 | float val01 = mat.at(y+1,x); 43 | float val11 = mat.at(y+1,x+1); 44 | return (wx0*wy0)*val00 + (wx1*wy0)*val10 + (wx0*wy1)*val01 + (wx1*wy1)*val11; 45 | } 46 | 47 | //! Return value between 0 and 255 48 | //! WARNING This function does not check whether the x/y is within the border 49 | inline float interpolateMat_8u(const cv::Mat& mat, float u, float v) 50 | { 51 | assert(mat.type()==CV_8U); 52 | int x = floor(u); 53 | int y = floor(v); 54 | float subpix_x = u-x; 55 | float subpix_y = v-y; 56 | 57 | float w00 = (1.0f-subpix_x)*(1.0f-subpix_y); 58 | float w01 = (1.0f-subpix_x)*subpix_y; 59 | float w10 = subpix_x*(1.0f-subpix_y); 60 | float w11 = 1.0f - w00 - w01 - w10; 61 | 62 | const int stride = mat.step.p[0]; 63 | unsigned char* ptr = mat.data + y*stride + x; 64 | return w00*ptr[0] + w01*ptr[stride] + w10*ptr[1] + w11*ptr[stride+1]; 65 | } 66 | 67 | void halfSample(const cv::Mat& in, cv::Mat& out); 68 | 69 | float shiTomasiScore(const cv::Mat& img, int u, int v); 70 | 71 | void calcSharrDeriv(const cv::Mat& src, cv::Mat& dst); 72 | 73 | 74 | #ifdef __SSE2__ 75 | 76 | /// Used to convert a Kinect depthmap 77 | /// Code by Christian Kerl DVO, GPL Licence 78 | void convertRawDepthImageSse_16u_to_32f(cv::Mat& depth_16u, cv::Mat& depth_32f, float scale); 79 | 80 | #endif 81 | 82 | 83 | } -------------------------------------------------------------------------------- /src/ImageReader.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // ImageReader.cpp 3 | // OnlinePhotometricCalibration 4 | // 5 | // Created by Paul on 16.11.17. 6 | // Copyright (c) 2017-2018 Paul Bergmann and co-authors. All rights reserved. 7 | // 8 | // See LICENSE.txt 9 | // 10 | 11 | #include "hso/ImageReader.h" 12 | #include 13 | 14 | namespace hso { 15 | 16 | ImageReader::ImageReader(std::string image_folder, cv::Size new_size, std::string time_folder) 17 | { 18 | getDir(image_folder, m_files); 19 | printf("ImageReader: got %d files in %s!\n", (int)m_files.size(), image_folder.c_str()); 20 | 21 | m_img_new_size = new_size; 22 | 23 | m_stamp_valid = false; 24 | if(time_folder != "None") 25 | { 26 | std::ifstream tr; 27 | tr.open(time_folder.c_str()); 28 | while(!tr.eof() && tr.good()) 29 | { 30 | char buf[1000]; 31 | tr.getline(buf, 1000); 32 | 33 | int id; 34 | char stamp[100]; 35 | float x,y,z,a,b,c,d; 36 | float exposure = 0; 37 | 38 | 39 | if(8 == sscanf(buf, "%s %f %f %f %f %f %f %f", stamp, &x, &y, &z, &a, &b, &c, &d)) 40 | { 41 | std::string time_stamp = stamp; 42 | m_times.push_back(time_stamp); 43 | } 44 | else if(3 == sscanf(buf, "%d %s %f", &id, stamp, &exposure)) 45 | { 46 | std::string time_stamp = stamp; 47 | m_times.push_back(time_stamp); 48 | } 49 | else if(2 == sscanf(buf, "%d %s", &id, stamp)) 50 | { 51 | std::string time_stamp = stamp; 52 | m_times.push_back(time_stamp); 53 | } 54 | else if(1 == sscanf(buf, "%s", stamp)) 55 | { 56 | std::string time_stamp = stamp; 57 | m_times.push_back(time_stamp); 58 | } 59 | 60 | } 61 | tr.close(); 62 | 63 | assert(m_times.size() == m_files.size()); 64 | m_stamp_valid = true; 65 | } 66 | } 67 | 68 | cv::Mat ImageReader::readImage(int image_index) 69 | { 70 | // Read image from disk 71 | cv::Mat image = cv::imread(m_files.at(image_index), cv::IMREAD_GRAYSCALE); 72 | 73 | if(!image.data) 74 | { 75 | std::cout << "ERROR READING IMAGE " << m_files.at(image_index) << std::endl; 76 | return cv::Mat(); 77 | } 78 | 79 | // Resize input image 80 | cv::resize(image, image, m_img_new_size); 81 | 82 | return image; 83 | } 84 | 85 | std::string ImageReader::readStamp(int image_index) 86 | { 87 | return m_times[image_index]; 88 | } 89 | 90 | int ImageReader::getDir(std::string dir, std::vector &files) 91 | { 92 | DIR *dp; 93 | struct dirent *dirp; 94 | if((dp = opendir(dir.c_str())) == NULL) 95 | { 96 | return -1; 97 | } 98 | 99 | while ((dirp = readdir(dp)) != NULL) 100 | { 101 | std::string name = std::string(dirp->d_name); 102 | 103 | if(name != "." && name != "..") 104 | { 105 | if(strstr(name.c_str(),".png")!=NULL || strstr(name.c_str(),".jpg")!=NULL) 106 | files.push_back(name); 107 | } 108 | } 109 | 110 | closedir(dp); 111 | std::sort(files.begin(), files.end()); 112 | 113 | 114 | if(dir.at(dir.length() - 1) != '/') 115 | dir = dir+"/"; 116 | 117 | for(unsigned int i = 0; i < files.size(); i++) 118 | { 119 | if(files[i].at(0) != '/') 120 | files[i] = dir + files[i]; 121 | } 122 | 123 | 124 | return (int)files.size(); 125 | } 126 | 127 | } 128 | -------------------------------------------------------------------------------- /src/config.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of HSO: Hybrid Sparse Monocular Visual Odometry 2 | // With Online Photometric Calibration 3 | // 4 | // Copyright(c) 2020, Dongting Luo, Dalian University of Technology, Dalian 5 | // Copyright(c) 2020, Robotics Group, Dalian University of Technology 6 | // 7 | // This program is highly based on the previous 8 | // implementation of SVO: https://github.com/uzh-rpg/rpg_svo 9 | // and PL-SVO: https://github.com/rubengooj/pl-svo 10 | // 11 | // This program is free software: you can redistribute it and/or modify 12 | // it under the terms of the GNU General Public License as published by 13 | // the Free Software Foundation, either version 3 of the License, or 14 | // (at your option) any later version. 15 | // 16 | // This program is distributed in the hope that it will be useful, 17 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | // GNU General Public License for more details. 20 | // 21 | // You should have received a copy of the GNU General Public License 22 | // along with this program. If not, see . 23 | 24 | #include 25 | 26 | namespace hso { 27 | 28 | Config::Config() : 29 | 30 | trace_name("hso"), 31 | trace_dir("/tmp"), 32 | n_pyr_levels(3), 33 | use_imu(false), 34 | core_n_kfs(7), 35 | map_scale(1.0), 36 | grid_size(36), 37 | init_min_disparity(40.0), 38 | init_min_tracked(50), 39 | init_min_inliers(40), 40 | klt_max_level(4), 41 | klt_min_level(0), 42 | reproj_thresh(2.0), 43 | poseoptim_thresh(2.0), 44 | poseoptim_num_iter(10), 45 | structureoptim_max_pts(30), 46 | structureoptim_num_iter(5), 47 | loba_thresh(2.0), 48 | loba_robust_huber_width(1.0), 49 | loba_num_iter(10), 50 | kfselect_mindist(0.12), 51 | triang_min_corner_score(20.0), 52 | triang_half_patch_size(4), 53 | subpix_n_iter(10), 54 | max_n_kfs(2000), 55 | img_imu_delay(0.0), 56 | max_fts(200), 57 | quality_min_fts(5), 58 | quality_max_drop_fts(40), 59 | 60 | edgelet_angle(0.86), 61 | 62 | n_max_drop_keyframe(13) 63 | // #endif 64 | {} 65 | 66 | Config& Config::getInstance() 67 | { 68 | static Config instance; // Instantiated on first use and guaranteed to be destroyed 69 | return instance; 70 | } 71 | 72 | } // namespace hso 73 | 74 | -------------------------------------------------------------------------------- /test/cameras/euroc.txt: -------------------------------------------------------------------------------- 1 | Pinhole 458.654 457.296 367.215 248.375 -0.28340811 0.07395907 0.00019359 1.76187114e-05 2 | 752 480 3 | false 4 | -------------------------------------------------------------------------------- /test/cameras/icl-nuim.txt: -------------------------------------------------------------------------------- 1 | Pinhole 481.2 480.0 319.5 239.5 0 0 0 0 2 | 640 480 3 | false 4 | -------------------------------------------------------------------------------- /test/cameras/tum_mono_vo_narrow.txt: -------------------------------------------------------------------------------- 1 | FOV 0.535719308086809 0.669566858850269 0.493248545285398 0.500408664348414 0.897966326944875 2 | 1280 1024 3 | false 4 | 5 | -------------------------------------------------------------------------------- /test/cameras/tum_mono_vo_wide.txt: -------------------------------------------------------------------------------- 1 | FOV 0.349153000000000 0.436593000000000 0.493140000000000 0.499021000000000 0.933271000000000 2 | 1280 1024 3 | true 4 | 5 | -------------------------------------------------------------------------------- /test/single_sequence.sh: -------------------------------------------------------------------------------- 1 | # run single 2 | #------------------------- 3 | 4 | #change it to your images folder 5 | pathImageFolder='/media/datasets/euroc/Vicon_Room1_01/cam0/data' 6 | 7 | 8 | #change it to the corresponding camera file. 9 | cameraCalibFile='./cameras/euroc.txt' 10 | 11 | 12 | ./../bin/test_dataset image="$pathImageFolder" calib="$cameraCalibFile" #start=xxx end=xxx times=xxx name=xxx 13 | 14 | -------------------------------------------------------------------------------- /thirdparty/Pangolin-master.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luodongting/HSO/12d78fb91a33195bc7e9363f6910edb2d9629469/thirdparty/Pangolin-master.zip -------------------------------------------------------------------------------- /thirdparty/Sophus/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET(PROJECT_NAME Sophus) 2 | 3 | PROJECT(${PROJECT_NAME}) 4 | CMAKE_MINIMUM_REQUIRED(VERSION 2.6) 5 | 6 | SET (CMAKE_VERBOSE_MAKEFILE ON) 7 | 8 | IF( NOT CMAKE_BUILD_TYPE ) 9 | SET( CMAKE_BUILD_TYPE Release ) 10 | ENDIF() 11 | 12 | IF (CMAKE_COMPILER_IS_GNUCXX ) 13 | SET(CMAKE_CXX_FLAGS_DEBUG "-O0 -g") 14 | SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG ") 15 | 16 | ADD_DEFINITIONS("-Wall -Werror -Wno-unused-variable 17 | -Wno-unused-but-set-variable -Wno-unknown-pragmas ") 18 | ENDIF() 19 | 20 | ################################################################################ 21 | # Add local path for finding packages, set the local version first 22 | set( CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake_modules" ) 23 | list( APPEND CMAKE_MODULE_PATH "${CMAKE_ROOT}/Modules" ) 24 | 25 | ################################################################################ 26 | # Create variables used for exporting in SophusConfig.cmake 27 | set( Sophus_LIBRARIES "" ) 28 | set( Sophus_INCLUDE_DIR ${PROJECT_SOURCE_DIR} ) 29 | 30 | ################################################################################ 31 | 32 | #SET (INCLUDE_DIRS "../eigen3.1/") 33 | find_package( Eigen3 REQUIRED ) 34 | INCLUDE_DIRECTORIES( ${EIGEN3_INCLUDE_DIR} ) 35 | SET( Sophus_INCLUDE_DIR ${Sophus_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} ) 36 | 37 | SET (SOURCE_DIR "sophus") 38 | SET (CLASSES so2 39 | se2 40 | se3 41 | so3 42 | scso3 43 | sim3 44 | ) 45 | 46 | SET (SOURCES) 47 | 48 | FOREACH(class ${CLASSES}) 49 | LIST(APPEND SOURCES ${SOURCE_DIR}/${class}.cpp ${SOURCE_DIR}/${class}.h) 50 | ENDFOREACH(class) 51 | 52 | LINK_LIBRARIES (${PROJECT_NAME} ${LIBS}) 53 | 54 | set( Sophus_LIBRARIES ${Sophus_LIBRARIES} ${LIBS} ) 55 | 56 | INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) 57 | ADD_LIBRARY(${PROJECT_NAME} SHARED ${SOURCES}) 58 | 59 | #ADD_EXECUTABLE(test_so2 sophus/test_so2.cpp) 60 | #ADD_EXECUTABLE(test_se2 sophus/test_se2.cpp) 61 | #ADD_EXECUTABLE(test_so3 sophus/test_so3.cpp) 62 | #ADD_EXECUTABLE(test_scso3 sophus/test_scso3.cpp) 63 | #ADD_EXECUTABLE(test_se3 sophus/test_se3.cpp) 64 | #ADD_EXECUTABLE(test_sim3 sophus/test_sim3.cpp) 65 | #ENABLE_TESTING() 66 | 67 | #ADD_TEST(test_so2 test_so2) 68 | #ADD_TEST(test_se2 test_se2) 69 | #ADD_TEST(test_so3 test_so3) 70 | #ADD_TEST(test_scso3 test_scso3) 71 | #ADD_TEST(test_se3 test_se3) 72 | #ADD_TEST(test_sim3 test_sim3) 73 | 74 | ############################################################################## 75 | # Get full library name 76 | GET_TARGET_PROPERTY( FULL_LIBRARY_NAME ${PROJECT_NAME} LOCATION ) 77 | set( Sophus_LIBRARIES ${Sophus_LIBRARIES} ${FULL_LIBRARY_NAME} ) 78 | set( Sophus_LIBRARY_DIR ${PROJECT_BINARY_DIR} ) 79 | 80 | ################################################################################ 81 | # Create the SophusConfig.cmake file for other cmake projects. 82 | #CONFIGURE_FILE( ${CMAKE_CURRENT_SOURCE_DIR}/SophusConfig.cmake.in 83 | # ${CMAKE_CURRENT_BINARY_DIR}/SophusConfig.cmake @ONLY IMMEDIATE ) 84 | #export( PACKAGE Sophus ) 85 | 86 | #INSTALL(DIRECTORY sophus DESTINATION ${CMAKE_INSTALL_PREFIX}/include FILES_MATCHING PATTERN "*.h" ) 87 | #INSTALL(TARGETS ${PROJECT_NAME} DESTINATION ${CMAKE_INSTALL_PREFIX}/lib ) 88 | -------------------------------------------------------------------------------- /thirdparty/Sophus/README: -------------------------------------------------------------------------------- 1 | C++ implementation of Lie Groups using Eigen. 2 | 3 | For a installation guideline, please refer to 4 | 5 | https://github.com/strasdat/ScaViSLAM 6 | 7 | for now. 8 | -------------------------------------------------------------------------------- /thirdparty/Sophus/SophusConfig.cmake.in: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Sophus source dir 3 | set( Sophus_SOURCE_DIR "@CMAKE_CURRENT_SOURCE_DIR@") 4 | 5 | ################################################################################ 6 | # Sophus build dir 7 | set( Sophus_DIR "@CMAKE_CURRENT_BINARY_DIR@") 8 | 9 | ################################################################################ 10 | set( Sophus_INCLUDE_DIR "@Sophus_INCLUDE_DIR@" ) 11 | set( Sophus_INCLUDE_DIRS "@Sophus_INCLUDE_DIR@" ) 12 | 13 | set( Sophus_LIBRARIES "@Sophus_LIBRARIES@" ) 14 | set( Sophus_LIBRARY "@Sophus_LIBRARIES@" ) 15 | 16 | set( Sophus_LIBRARY_DIR "@Sophus_LIBRARY_DIR@" ) 17 | set( Sophus_LIBRARY_DIRS "@Sophus_LIBRARY_DIR@" ) 18 | -------------------------------------------------------------------------------- /thirdparty/Sophus/cmake_modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # Once done this will define 3 | # 4 | # EIGEN3_FOUND - system has eigen lib 5 | # EIGEN3_INCLUDE_DIR - the eigen include directory 6 | 7 | # Copyright (c) 2006, 2007 Montel Laurent, 8 | # Redistribution and use is allowed according to the terms of the BSD license. 9 | # For details see the accompanying COPYING-CMAKE-SCRIPTS file. 10 | 11 | if( EIGEN3_INCLUDE_DIR ) 12 | # in cache already 13 | set( EIGEN3_FOUND TRUE ) 14 | else (EIGEN3_INCLUDE_DIR) 15 | find_path( EIGEN3_INCLUDE_DIR NAMES Eigen/Core 16 | PATH_SUFFIXES eigen3/ 17 | HINTS 18 | ${INCLUDE_INSTALL_DIR} 19 | /usr/local/include 20 | ${KDE4_INCLUDE_DIR} 21 | ) 22 | include( FindPackageHandleStandardArgs ) 23 | find_package_handle_standard_args( Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR ) 24 | mark_as_advanced( EIGEN3_INCLUDE_DIR ) 25 | endif(EIGEN3_INCLUDE_DIR) 26 | 27 | -------------------------------------------------------------------------------- /thirdparty/Sophus/sophus/se2.h: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2011 Hauke Strasdat (Imperial College London) 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #ifndef SOPHUS_SE2_H 24 | #define SOPHUS_SE2_H 25 | 26 | #include "so2.h" 27 | 28 | namespace Sophus 29 | { 30 | using namespace Eigen; 31 | using namespace std; 32 | 33 | class SE2 34 | { 35 | public: 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | 38 | SE2 (); 39 | 40 | SE2 (const SO2 & so2, 41 | const Vector2d & translation); 42 | 43 | SE2 (const Matrix2d & rotation_matrix, 44 | const Vector2d & translation); 45 | 46 | SE2 (double theta, 47 | const Vector2d & translation_); 48 | 49 | SE2 (const SE2 & other); 50 | 51 | SE2 & 52 | operator= (const SE2 & other); 53 | 54 | SE2 55 | operator* (const SE2& other) const; 56 | 57 | SE2& 58 | operator*= (const SE2& other); 59 | 60 | SE2 61 | inverse () const; 62 | 63 | Vector3d 64 | log () const; 65 | 66 | Vector2d 67 | operator* (const Vector2d & xy) const; 68 | 69 | static SE2 70 | exp (const Vector3d & update); 71 | 72 | static Vector3d 73 | log (const SE2 & SE2); 74 | 75 | Matrix 76 | matrix () const; 77 | 78 | Matrix 79 | Adj () const; 80 | 81 | static Matrix3d 82 | hat (const Vector3d & omega); 83 | 84 | static Vector3d 85 | vee (const Matrix3d & Omega); 86 | 87 | static Vector3d 88 | lieBracket (const Vector3d & v1, 89 | const Vector3d & v2); 90 | 91 | static Matrix3d 92 | d_lieBracketab_by_d_a (const Vector3d & b); 93 | 94 | const Vector2d & translation() const 95 | { 96 | return translation_; 97 | } 98 | 99 | Vector2d& translation() 100 | { 101 | return translation_; 102 | } 103 | 104 | Matrix2d rotation_matrix() const 105 | { 106 | return so2_.matrix(); 107 | } 108 | 109 | void setRotationMatrix(const Matrix2d & rotation_matrix) 110 | { 111 | so2_ = SO2(rotation_matrix); 112 | } 113 | 114 | const SO2& so2() const 115 | { 116 | return so2_; 117 | } 118 | 119 | SO2& so2() 120 | { 121 | return so2_; 122 | } 123 | 124 | static const int DoF = 3; 125 | 126 | private: 127 | SO2 so2_; 128 | Vector2d translation_; 129 | 130 | }; 131 | 132 | inline std::ostream& operator <<(std::ostream & out_str, 133 | const SE2 & SE2) 134 | { 135 | out_str << SE2.so2() << SE2.translation() << std::endl; 136 | return out_str; 137 | } 138 | 139 | } // end namespace 140 | 141 | 142 | #endif 143 | -------------------------------------------------------------------------------- /thirdparty/Sophus/sophus/so2.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2012 Hauke Strasdat (Imperial College London) 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #include 24 | #include "so2.h" 25 | #include "so3.h" 26 | 27 | namespace Sophus 28 | { 29 | 30 | SO2::SO2() 31 | { 32 | unit_complex_.real(1.); 33 | unit_complex_.imag(0.); 34 | } 35 | 36 | SO2 37 | ::SO2(const SO2 & other) : unit_complex_(other.unit_complex_) {} 38 | 39 | SO2 40 | ::SO2(const Matrix2d & R) : unit_complex_(0.5*(R(0,0)+R(1,1)), 41 | 0.5*(R(1,0)-R(0,1))) {} 42 | 43 | SO2 44 | ::SO2(const Complexd & complex) : unit_complex_(complex) 45 | { 46 | assert(abs(unit_complex_)!=0); 47 | normalize(); 48 | } 49 | 50 | SO2 51 | ::SO2(double theta) 52 | { 53 | unit_complex_ = SO2::exp(theta).unit_complex_; 54 | } 55 | 56 | void SO2 57 | ::operator=(const SO2 & other) 58 | { 59 | this->unit_complex_ = other.unit_complex_; 60 | } 61 | 62 | SO2 SO2 63 | ::operator*(const SO2& other) const 64 | { 65 | SO2 result(*this); 66 | result.unit_complex_ *= other.unit_complex_; 67 | result.normalize(); 68 | return result; 69 | } 70 | 71 | void SO2 72 | ::operator*=(const SO2& other) 73 | { 74 | unit_complex_ *= other.unit_complex_; 75 | normalize(); 76 | } 77 | 78 | Vector2d SO2 79 | ::operator*(const Vector2d & xy) const 80 | { 81 | const double & real = unit_complex_.real(); 82 | const double & imag = unit_complex_.imag(); 83 | return Vector2d(real*xy[0] - imag*xy[1], imag*xy[0] + real*xy[1]); 84 | } 85 | 86 | SO2 SO2 87 | ::inverse() const 88 | { 89 | return SO2(conj(unit_complex_)); 90 | } 91 | 92 | Matrix2d SO2 93 | ::matrix() const 94 | { 95 | const double & real = unit_complex_.real(); 96 | const double & imag = unit_complex_.imag(); 97 | Matrix2d R; 98 | R(0,0) = real; R(0,1) = -imag; 99 | R(1,0) = imag; R(1,1) = real; 100 | return R; 101 | } 102 | 103 | double SO2 104 | ::Adj() const 105 | { 106 | return 1.; 107 | } 108 | 109 | Matrix2d SO2 110 | ::generator(int i) 111 | { 112 | assert(i==0); 113 | return hat(1.); 114 | } 115 | 116 | double SO2 117 | ::log() const 118 | { 119 | return SO2::log(*this); 120 | } 121 | 122 | double SO2 123 | ::log(const SO2 & other) 124 | { 125 | return atan2(other.unit_complex_.imag(), other.unit_complex().real()); 126 | } 127 | 128 | SO2 SO2 129 | ::exp(double theta) 130 | { 131 | return SO2(Complexd(cos(theta), sin(theta))); 132 | } 133 | 134 | Matrix2d SO2 135 | ::hat(double v) 136 | { 137 | Matrix2d Omega; 138 | Omega << 0, -v 139 | , v, 0; 140 | return Omega; 141 | } 142 | 143 | double SO2 144 | ::vee(const Matrix2d & Omega) 145 | { 146 | assert(fabs(Omega(1,0)+Omega(0,1)) 27 | 28 | #include 29 | 30 | 31 | namespace Sophus 32 | { 33 | using namespace Eigen; 34 | 35 | 36 | class SO2 37 | { 38 | public: 39 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 40 | 41 | typedef std::complex Complexd; 42 | 43 | SO2 (); 44 | 45 | SO2 (const SO2 & other); 46 | 47 | explicit 48 | SO2 (const Matrix2d & R); 49 | 50 | explicit 51 | SO2 (const Complexd & unit_complex); 52 | 53 | SO2 (double theta); 54 | void 55 | operator= (const SO2 & SO2); 56 | 57 | SO2 58 | operator* (const SO2 & SO2) const; 59 | 60 | void 61 | operator*= (const SO2 & SO2); 62 | 63 | Vector2d 64 | operator* (const Vector2d & xy) const; 65 | 66 | SO2 67 | inverse () const; 68 | 69 | Matrix2d 70 | matrix () const; 71 | 72 | double 73 | Adj () const; 74 | 75 | Matrix2d 76 | generator (int i); 77 | 78 | double 79 | log () const; 80 | 81 | static SO2 82 | exp (double theta); 83 | 84 | static double 85 | log (const SO2 & SO2); 86 | 87 | static Matrix2d 88 | hat (double omega); 89 | 90 | static double 91 | vee (const Matrix2d & Omega); 92 | 93 | static double 94 | lieBracket (double omega1, 95 | double omega2); 96 | void 97 | setComplex (const Complexd& z); 98 | 99 | const Complexd & unit_complex() const 100 | { 101 | return unit_complex_; 102 | } 103 | 104 | static const int DoF = 2; 105 | 106 | protected: 107 | void 108 | normalize (); 109 | 110 | Complexd unit_complex_; 111 | }; 112 | 113 | inline std::ostream& operator <<(std::ostream & out_str, 114 | const SO2 & SO2) 115 | { 116 | 117 | out_str << SO2.log() << std::endl; 118 | return out_str; 119 | } 120 | 121 | } // end namespace 122 | 123 | 124 | #endif 125 | -------------------------------------------------------------------------------- /thirdparty/Sophus/sophus/so3.h: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2011 Hauke Strasdat (Imperial College London) 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #ifndef SOPHUS_SO3_H 24 | #define SOPHUS_SO3_H 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | 31 | namespace Sophus 32 | { 33 | using namespace Eigen; 34 | 35 | const double SMALL_EPS = 1e-10; 36 | 37 | class SO3 38 | { 39 | public: 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 41 | 42 | SO3 (); 43 | 44 | SO3 (const SO3 & other); 45 | 46 | explicit 47 | SO3 (const Matrix3d & _R); 48 | 49 | explicit 50 | SO3 (const Quaterniond & unit_quaternion); 51 | 52 | SO3 (double rot_x, 53 | double rot_y, 54 | double rot_z); 55 | void 56 | operator= (const SO3 & so3); 57 | 58 | SO3 59 | operator* (const SO3 & so3) const; 60 | 61 | void 62 | operator*= (const SO3 & so3); 63 | 64 | Vector3d 65 | operator* (const Vector3d & xyz) const; 66 | 67 | SO3 68 | inverse () const; 69 | 70 | Matrix3d 71 | matrix () const; 72 | 73 | Matrix3d 74 | Adj () const; 75 | 76 | Matrix3d 77 | generator (int i); 78 | 79 | Vector3d 80 | log () const; 81 | 82 | static SO3 83 | exp (const Vector3d & omega); 84 | 85 | static SO3 86 | expAndTheta (const Vector3d & omega, 87 | double * theta); 88 | static Vector3d 89 | log (const SO3 & so3); 90 | 91 | static Vector3d 92 | logAndTheta (const SO3 & so3, 93 | double * theta); 94 | 95 | static Matrix3d 96 | hat (const Vector3d & omega); 97 | 98 | static Vector3d 99 | vee (const Matrix3d & Omega); 100 | 101 | static Vector3d 102 | lieBracket (const Vector3d & omega1, 103 | const Vector3d & omega2); 104 | 105 | static Matrix3d 106 | d_lieBracketab_by_d_a (const Vector3d & b); 107 | 108 | void 109 | setQuaternion (const Quaterniond& quaternion); 110 | 111 | const Quaterniond & unit_quaternion() const 112 | { 113 | return unit_quaternion_; 114 | } 115 | 116 | static const int DoF = 3; 117 | 118 | protected: 119 | Quaterniond unit_quaternion_; 120 | }; 121 | 122 | inline std::ostream& operator <<(std::ostream & out_str, 123 | const SO3 & so3) 124 | { 125 | 126 | out_str << so3.log().transpose() << std::endl; 127 | return out_str; 128 | } 129 | 130 | } // end namespace 131 | 132 | 133 | #endif 134 | -------------------------------------------------------------------------------- /thirdparty/Sophus/sophus/test_so2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | #include "so2.h" 7 | #include "so3.h" 8 | 9 | using namespace Sophus; 10 | using namespace std; 11 | 12 | bool so2explog_tests() 13 | { 14 | double pi = 3.14159265; 15 | vector so2; 16 | so2.push_back(SO2::exp(0.0)); 17 | so2.push_back(SO2::exp(0.2)); 18 | so2.push_back(SO2::exp(10.)); 19 | so2.push_back(SO2::exp(0.00001)); 20 | so2.push_back(SO2::exp(pi)); 21 | so2.push_back(SO2::exp(0.2) 22 | *SO2::exp(pi) 23 | *SO2::exp(-0.2)); 24 | so2.push_back(SO2::exp(-0.3) 25 | *SO2::exp(pi) 26 | *SO2::exp(0.3)); 27 | 28 | 29 | bool failed = false; 30 | 31 | for (size_t i=0; iSMALL_EPS) 40 | { 41 | cerr << "SO3 - exp(log(SO3))" << endl; 42 | cerr << "Test case: " << i << endl; 43 | cerr << DiffR <SMALL_EPS) 59 | { 60 | cerr << "Transform vector" << endl; 61 | cerr << "Test case: " << i << endl; 62 | cerr << (res1-res2) <SMALL_EPS) 79 | { 80 | cerr << "Inverse" << endl; 81 | cerr << "Test case: " << i << endl; 82 | cerr << (res-I) <SMALL_EPS) 97 | { 98 | cerr << "expmap(hat(x)) - exp(x)" << endl; 99 | cerr << "Test case: " << i << endl; 100 | // cerr << exp_x < 5 | 6 | namespace fast 7 | { 8 | 9 | using ::std::vector; 10 | 11 | struct fast_xy 12 | { 13 | short x, y; 14 | fast_xy(short x_, short y_) : x(x_), y(y_) {} 15 | }; 16 | 17 | typedef unsigned char fast_byte; 18 | 19 | /// SSE2 optimized version of the corner 10 20 | void fast_corner_detect_10_sse2(const fast_byte* img, int imgWidth, int imgHeight, int widthStep, short barrier, vector& corners); 21 | void fast_corner_detect_9_sse2(const fast_byte* img, int imgWidth, int imgHeight, int widthStep, short barrier, vector& corners); 22 | 23 | /// plain C++ version of the corner 10 24 | void fast_corner_detect_10(const fast_byte* img, int imgWidth, int imgHeight, int widthStep, short barrier, vector& corners); 25 | 26 | void fast_corner_detect_plain_9(const fast_byte* img, int imgWidth, int imgHeight, int widthStep, short barrier, vector& corners); 27 | 28 | void fast_corner_detect_plain_8(const fast_byte* img, int imgWidth, int imgHeight, int widthStep, short barrier, vector& corners); 29 | 30 | /// plain C++ version of the corner 7 31 | void fast_corner_detect_plain_7(const fast_byte* img, int imgWidth, int imgHeight, int widthStep, short barrier, vector& corners); 32 | 33 | /// plain C++ version of the corner 12 34 | void fast_corner_detect_plain_12(const fast_byte* img, int imgWidth, int imgHeight, int widthStep, short barrier, vector& corners); 35 | 36 | /// corner score 10 37 | void fast_corner_score_10(const fast_byte* img, const int img_stride, const vector& corners, const int threshold, vector& scores); 38 | 39 | void fast_corner_score_9(const fast_byte* img, const int img_stride, const vector& corners, const int threshold, vector& scores); 40 | 41 | void fast_corner_score_8(const fast_byte* img, const int img_stride, const vector& corners, const int threshold, vector& scores); 42 | 43 | /// corner score 7 44 | void fast_corner_score_7(const fast_byte* img, const int img_stride, const vector& corners, const int threshold, vector& scores); 45 | 46 | /// corner score 12 47 | void fast_corner_score_12(const fast_byte* img, const int img_stride, const vector& corners, const int threshold, vector& scores); 48 | 49 | /// Nonmax Suppression on a 3x3 Window 50 | void fast_nonmax_3x3(const vector& corners, const vector& scores, vector& nonmax_corners); 51 | 52 | /// NEON optimized version of the corner 9 53 | void fast_corner_detect_9_neon(const fast_byte* img, int imgWidth, int imgHeight, int widthStep, short barrier, vector& corners); 54 | 55 | } // namespace fast 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /thirdparty/fast/include/fast/faster_corner_utilities.h: -------------------------------------------------------------------------------- 1 | #ifndef FAST_CORNER_UTILITIES_H 2 | #define FAST_CORNER_UTILITIES_H 3 | 4 | #if __ARM_NEON__ 5 | #include 6 | #elif __SSE2__ 7 | #include 8 | #endif 9 | 10 | namespace fast 11 | { 12 | 13 | /// Check if the pointer is aligned to the specified byte granularity 14 | template bool is_aligned(const void* ptr); 15 | template<> inline bool is_aligned<8>(const void* ptr) { return ((reinterpret_cast(ptr)) & 0x7) == 0; } 16 | template<> inline bool is_aligned<16>(const void* ptr) { return ((reinterpret_cast(ptr)) & 0xF) == 0; } 17 | 18 | 19 | struct Less 20 | { 21 | template static bool eval(const T1 a, const T2 b) 22 | { 23 | return a < b; 24 | } 25 | static short prep_t(short pixel_val, short barrier) 26 | { 27 | return pixel_val - barrier; 28 | } 29 | }; 30 | 31 | struct Greater 32 | { 33 | template static bool eval(const T1 a, const T2 b) 34 | { 35 | return a > b; 36 | } 37 | static short prep_t(short pixel_val, short barrier) 38 | { 39 | return pixel_val + barrier; 40 | } 41 | }; 42 | 43 | #if __SSE2__ 44 | 45 | #define CHECK_BARRIER(lo, hi, other, flags) \ 46 | { \ 47 | __m128i diff = _mm_subs_epu8(lo, other); \ 48 | __m128i diff2 = _mm_subs_epu8(other, hi); \ 49 | __m128i z = _mm_setzero_si128(); \ 50 | diff = _mm_cmpeq_epi8(diff, z); \ 51 | diff2 = _mm_cmpeq_epi8(diff2, z); \ 52 | flags = ~(_mm_movemask_epi8(diff) | (_mm_movemask_epi8(diff2) << 16)); \ 53 | } 54 | 55 | template inline __m128i load_si128(const void* addr) { return _mm_loadu_si128((const __m128i*)addr); } 56 | template <> inline __m128i load_si128(const void* addr) { return _mm_load_si128((const __m128i*)addr); } 57 | 58 | #endif 59 | 60 | } // namespace fast 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /thirdparty/fast/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fast 4 | 1.0.0 5 | 6 | A ROS wrapper for the FAST corner detector by Edward Rosten. 7 | The sourcefiles are from the CVD library: http://www.edwardrosten.com/cvd/ 8 | 9 | Christian Forster 10 | LGPL 11 | http://www.edwardrosten.com/work/fast.html 12 | catkin 13 | -------------------------------------------------------------------------------- /thirdparty/g2o/README.txt: -------------------------------------------------------------------------------- 1 | You should have received this g2o version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2). 2 | See the original g2o library at: https://github.com/RainerKuemmerle/g2o 3 | All files included in this g2o version are BSD, see license-bsd.txt 4 | -------------------------------------------------------------------------------- /thirdparty/g2o/cmake_modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | # specific additional paths for some OS 65 | if (WIN32) 66 | set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include") 67 | endif(WIN32) 68 | 69 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 70 | PATHS 71 | ${CMAKE_INSTALL_PREFIX}/include 72 | ${EIGEN_ADDITIONAL_SEARCH_PATHS} 73 | ${KDE4_INCLUDE_DIR} 74 | PATH_SUFFIXES eigen3 eigen 75 | ) 76 | 77 | if(EIGEN3_INCLUDE_DIR) 78 | _eigen3_check_version() 79 | endif(EIGEN3_INCLUDE_DIR) 80 | 81 | include(FindPackageHandleStandardArgs) 82 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 83 | 84 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 85 | 86 | endif(EIGEN3_INCLUDE_DIR) 87 | 88 | -------------------------------------------------------------------------------- /thirdparty/g2o/config.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_CONFIG_H 2 | #define G2O_CONFIG_H 3 | 4 | /* #undef G2O_OPENMP */ 5 | /* #undef G2O_SHARED_LIBS */ 6 | 7 | // available sparse matrix libraries 8 | /* #undef G2O_HAVE_CHOLMOD */ 9 | 10 | // give a warning if Eigen defaults to row-major matrices. 11 | // We internally assume column-major matrices throughout the code. 12 | #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR 13 | # error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)" 14 | #endif 15 | 16 | #endif 17 | -------------------------------------------------------------------------------- /thirdparty/g2o/config.h.in: -------------------------------------------------------------------------------- 1 | #ifndef G2O_CONFIG_H 2 | #define G2O_CONFIG_H 3 | 4 | #cmakedefine G2O_OPENMP 1 5 | #cmakedefine G2O_SHARED_LIBS 1 6 | 7 | // available sparse matrix libraries 8 | #cmakedefine G2O_HAVE_CHOLMOD 1 9 | 10 | // give a warning if Eigen defaults to row-major matrices. 11 | // We internally assume column-major matrices throughout the code. 12 | #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR 13 | # error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)" 14 | #endif 15 | 16 | #endif 17 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/base_edge.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_BASE_EDGE_H 28 | #define G2O_BASE_EDGE_H 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | #include "optimizable_graph.h" 36 | 37 | namespace g2o { 38 | 39 | using namespace Eigen; 40 | 41 | template 42 | class BaseEdge : public OptimizableGraph::Edge 43 | { 44 | public: 45 | 46 | static const int Dimension = D; 47 | typedef E Measurement; 48 | typedef Matrix ErrorVector; 49 | typedef Matrix InformationType; 50 | 51 | BaseEdge() : OptimizableGraph::Edge() 52 | { 53 | _dimension = D; 54 | } 55 | 56 | virtual ~BaseEdge() {} 57 | 58 | virtual double chi2() const 59 | { 60 | return _error.dot(information()*_error); 61 | } 62 | 63 | virtual const double* errorData() const { return _error.data();} 64 | virtual double* errorData() { return _error.data();} 65 | const ErrorVector& error() const { return _error;} 66 | ErrorVector& error() { return _error;} 67 | 68 | //! information matrix of the constraint 69 | const InformationType& information() const { return _information;} 70 | InformationType& information() { return _information;} 71 | void setInformation(const InformationType& information) { _information = information;} 72 | 73 | virtual const double* informationData() const { return _information.data();} 74 | virtual double* informationData() { return _information.data();} 75 | 76 | //! accessor functions for the measurement represented by the edge 77 | const Measurement& measurement() const { return _measurement;} 78 | virtual void setMeasurement(const Measurement& m) { _measurement = m;} 79 | 80 | virtual int rank() const {return _dimension;} 81 | 82 | virtual void initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*) 83 | { 84 | std::cerr << "inititialEstimate() is not implemented, please give implementation in your derived class" << std::endl; 85 | } 86 | 87 | protected: 88 | 89 | Measurement _measurement; 90 | InformationType _information; 91 | ErrorVector _error; 92 | 93 | /** 94 | * calculate the robust information matrix by updating the information matrix of the error 95 | */ 96 | InformationType robustInformation(const Eigen::Vector3d& rho) 97 | { 98 | InformationType result = rho[1] * _information; 99 | //ErrorVector weightedErrror = _information * _error; 100 | //result.noalias() += 2 * rho[2] * (weightedErrror * weightedErrror.transpose()); 101 | return result; 102 | } 103 | 104 | public: 105 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 106 | }; 107 | 108 | } // end namespace g2o 109 | 110 | #endif 111 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/base_multi_edge.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_BASE_MULTI_EDGE_H 28 | #define G2O_BASE_MULTI_EDGE_H 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | #include "base_edge.h" 37 | #include "robust_kernel.h" 38 | #include "../../config.h" 39 | 40 | namespace g2o { 41 | 42 | using namespace Eigen; 43 | 44 | /** 45 | * \brief base class to represent an edge connecting an arbitrary number of nodes 46 | * 47 | * D - Dimension of the measurement 48 | * E - type to represent the measurement 49 | */ 50 | template 51 | class BaseMultiEdge : public BaseEdge 52 | { 53 | public: 54 | /** 55 | * \brief helper for mapping the Hessian memory of the upper triangular block 56 | */ 57 | struct HessianHelper { 58 | Eigen::Map matrix; ///< the mapped memory 59 | bool transposed; ///< the block has to be transposed 60 | HessianHelper() : matrix(0, 0, 0), transposed(false) {} 61 | }; 62 | 63 | public: 64 | static const int Dimension = BaseEdge::Dimension; 65 | typedef typename BaseEdge::Measurement Measurement; 66 | typedef MatrixXd::MapType JacobianType; 67 | typedef typename BaseEdge::ErrorVector ErrorVector; 68 | typedef typename BaseEdge::InformationType InformationType; 69 | typedef Eigen::Map HessianBlockType; 70 | 71 | BaseMultiEdge() : BaseEdge() 72 | { 73 | } 74 | 75 | virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace); 76 | 77 | /** 78 | * Linearizes the oplus operator in the vertex, and stores 79 | * the result in temporary variable vector _jacobianOplus 80 | */ 81 | virtual void linearizeOplus(); 82 | 83 | virtual void resize(size_t size); 84 | 85 | virtual bool allVerticesFixed() const; 86 | 87 | virtual void constructQuadraticForm() ; 88 | 89 | virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor); 90 | 91 | using BaseEdge::computeError; 92 | 93 | protected: 94 | using BaseEdge::_measurement; 95 | using BaseEdge::_information; 96 | using BaseEdge::_error; 97 | using BaseEdge::_vertices; 98 | using BaseEdge::_dimension; 99 | 100 | std::vector _hessian; 101 | std::vector > _jacobianOplus; ///< jacobians of the edge (w.r.t. oplus) 102 | 103 | void computeQuadraticForm(const InformationType& omega, const ErrorVector& weightedError); 104 | 105 | public: 106 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 107 | }; 108 | 109 | #include "base_multi_edge.hpp" 110 | 111 | } // end namespace g2o 112 | 113 | #endif 114 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/base_unary_edge.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_BASE_UNARY_EDGE_H 28 | #define G2O_BASE_UNARY_EDGE_H 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "base_edge.h" 35 | #include "robust_kernel.h" 36 | #include "../../config.h" 37 | 38 | namespace g2o { 39 | 40 | using namespace Eigen; 41 | 42 | template 43 | class BaseUnaryEdge : public BaseEdge 44 | { 45 | public: 46 | static const int Dimension = BaseEdge::Dimension; 47 | typedef typename BaseEdge::Measurement Measurement; 48 | typedef VertexXi VertexXiType; 49 | typedef typename Matrix::AlignedMapType JacobianXiOplusType; 50 | typedef typename BaseEdge::ErrorVector ErrorVector; 51 | typedef typename BaseEdge::InformationType InformationType; 52 | 53 | BaseUnaryEdge() : BaseEdge(), 54 | _jacobianOplusXi(0, D, VertexXiType::Dimension) 55 | { 56 | _vertices.resize(1); 57 | } 58 | 59 | virtual void resize(size_t size); 60 | 61 | virtual bool allVerticesFixed() const; 62 | 63 | virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace); 64 | 65 | /** 66 | * Linearizes the oplus operator in the vertex, and stores 67 | * the result in temporary variables _jacobianOplusXi and _jacobianOplusXj 68 | */ 69 | virtual void linearizeOplus(); 70 | 71 | //! returns the result of the linearization in the manifold space for the node xi 72 | const JacobianXiOplusType& jacobianOplusXi() const { return _jacobianOplusXi;} 73 | 74 | virtual void constructQuadraticForm(); 75 | 76 | virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to); 77 | 78 | virtual void mapHessianMemory(double*, int, int, bool) {assert(0 && "BaseUnaryEdge does not map memory of the Hessian");} 79 | 80 | using BaseEdge::resize; 81 | using BaseEdge::computeError; 82 | 83 | protected: 84 | using BaseEdge::_measurement; 85 | using BaseEdge::_information; 86 | using BaseEdge::_error; 87 | using BaseEdge::_vertices; 88 | using BaseEdge::_dimension; 89 | 90 | JacobianXiOplusType _jacobianOplusXi; 91 | 92 | public: 93 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 94 | }; 95 | 96 | #include "base_unary_edge.hpp" 97 | 98 | } // end namespace g2o 99 | 100 | #endif 101 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/base_vertex.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | template 28 | BaseVertex::BaseVertex() : 29 | OptimizableGraph::Vertex(), 30 | _hessian(0, D, D) 31 | { 32 | _dimension = D; 33 | } 34 | 35 | template 36 | double BaseVertex::solveDirect(double lambda) { 37 | Matrix tempA=_hessian + Matrix ::Identity()*lambda; 38 | double det=tempA.determinant(); 39 | if (g2o_isnan(det) || det < std::numeric_limits::epsilon()) 40 | return det; 41 | Matrix dx=tempA.llt().solve(_b); 42 | oplus(&dx[0]); 43 | return det; 44 | } 45 | 46 | template 47 | void BaseVertex::clearQuadraticForm() { 48 | _b.setZero(); 49 | } 50 | 51 | template 52 | void BaseVertex::mapHessianMemory(double* d) 53 | { 54 | new (&_hessian) HessianBlockType(d, D, D); 55 | } 56 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/batch_stats.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "batch_stats.h" 28 | #include 29 | 30 | namespace g2o { 31 | using namespace std; 32 | 33 | G2OBatchStatistics* G2OBatchStatistics::_globalStats=0; 34 | 35 | #ifndef PTHING 36 | #define PTHING(s) \ 37 | #s << "= " << (st.s) << "\t " 38 | #endif 39 | 40 | G2OBatchStatistics::G2OBatchStatistics(){ 41 | // zero all. 42 | memset (this, 0, sizeof(G2OBatchStatistics)); 43 | 44 | // set the iteration to -1 to show that it isn't valid 45 | iteration = -1; 46 | } 47 | 48 | std::ostream& operator << (std::ostream& os , const G2OBatchStatistics& st) 49 | { 50 | os << PTHING(iteration); 51 | 52 | os << PTHING( numVertices ); // how many vertices are involved 53 | os << PTHING( numEdges ); // hoe many edges 54 | os << PTHING( chi2 ); // total chi2 55 | 56 | /** timings **/ 57 | // nonlinear part 58 | os << PTHING( timeResiduals ); 59 | os << PTHING( timeLinearize ); // jacobians 60 | os << PTHING( timeQuadraticForm ); // construct the quadratic form in the graph 61 | 62 | // block_solver (constructs Ax=b, plus maybe schur); 63 | os << PTHING( timeSchurComplement ); // compute schur complement (0 if not done); 64 | 65 | // linear solver (computes Ax=b); ); 66 | os << PTHING( timeSymbolicDecomposition ); // symbolic decomposition (0 if not done); 67 | os << PTHING( timeNumericDecomposition ); // numeric decomposition (0 if not done); 68 | os << PTHING( timeLinearSolution ); // total time for solving Ax=b 69 | os << PTHING( iterationsLinearSolver ); // iterations of PCG 70 | os << PTHING( timeUpdate ); // oplus 71 | os << PTHING( timeIteration ); // total time ); 72 | 73 | os << PTHING( levenbergIterations ); 74 | os << PTHING( timeLinearSolver); 75 | 76 | os << PTHING(hessianDimension); 77 | os << PTHING(hessianPoseDimension); 78 | os << PTHING(hessianLandmarkDimension); 79 | os << PTHING(choleskyNNZ); 80 | os << PTHING(timeMarginals); 81 | 82 | return os; 83 | }; 84 | 85 | void G2OBatchStatistics::setGlobalStats(G2OBatchStatistics* b) 86 | { 87 | _globalStats = b; 88 | } 89 | 90 | } // end namespace 91 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/batch_stats.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_BATCH_STATS_H_ 28 | #define G2O_BATCH_STATS_H_ 29 | 30 | #include 31 | #include 32 | 33 | 34 | namespace g2o { 35 | 36 | /** 37 | * \brief statistics about the optimization 38 | */ 39 | struct G2OBatchStatistics { 40 | G2OBatchStatistics(); 41 | int iteration; ///< which iteration 42 | int numVertices; ///< how many vertices are involved 43 | int numEdges; ///< how many edges 44 | double chi2; ///< total chi2 45 | 46 | /** timings **/ 47 | // nonlinear part 48 | double timeResiduals; ///< residuals 49 | double timeLinearize; ///< jacobians 50 | double timeQuadraticForm; ///< construct the quadratic form in the graph 51 | int levenbergIterations; ///< number of iterations performed by LM 52 | // block_solver (constructs Ax=b, plus maybe schur) 53 | double timeSchurComplement; ///< compute schur complement (0 if not done) 54 | 55 | // linear solver (computes Ax=b); 56 | double timeSymbolicDecomposition; ///< symbolic decomposition (0 if not done) 57 | double timeNumericDecomposition; ///< numeric decomposition (0 if not done) 58 | double timeLinearSolution; ///< total time for solving Ax=b (including detup for schur) 59 | double timeLinearSolver; ///< time for solving, excluding Schur setup 60 | int iterationsLinearSolver; ///< iterations of PCG, (0 if not used, i.e., Cholesky) 61 | double timeUpdate; ///< time to apply the update 62 | double timeIteration; ///< total time; 63 | 64 | double timeMarginals; ///< computing the inverse elements (solve blocks) and thus the marginal covariances 65 | 66 | // information about the Hessian matrix 67 | size_t hessianDimension; ///< rows / cols of the Hessian 68 | size_t hessianPoseDimension; ///< dimension of the pose matrix in Schur 69 | size_t hessianLandmarkDimension; ///< dimension of the landmark matrix in Schur 70 | size_t choleskyNNZ; ///< number of non-zeros in the cholesky factor 71 | 72 | static G2OBatchStatistics* globalStats() {return _globalStats;} 73 | static void setGlobalStats(G2OBatchStatistics* b); 74 | protected: 75 | static G2OBatchStatistics* _globalStats; 76 | }; 77 | 78 | std::ostream& operator<<(std::ostream&, const G2OBatchStatistics&); 79 | 80 | typedef std::vector BatchStatisticsContainer; 81 | } 82 | 83 | #endif 84 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/creators.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_CREATORS_H 28 | #define G2O_CREATORS_H 29 | 30 | #include "hyper_graph.h" 31 | 32 | #include 33 | #include 34 | 35 | namespace g2o 36 | { 37 | 38 | /** 39 | * \brief Abstract interface for allocating HyperGraphElement 40 | */ 41 | class AbstractHyperGraphElementCreator 42 | { 43 | public: 44 | /** 45 | * create a hyper graph element. Has to implemented in derived class. 46 | */ 47 | virtual HyperGraph::HyperGraphElement* construct() = 0; 48 | /** 49 | * name of the class to be created. Has to implemented in derived class. 50 | */ 51 | virtual const std::string& name() const = 0; 52 | 53 | virtual ~AbstractHyperGraphElementCreator() { } 54 | }; 55 | 56 | /** 57 | * \brief templatized creator class which creates graph elements 58 | */ 59 | template 60 | class HyperGraphElementCreator : public AbstractHyperGraphElementCreator 61 | { 62 | public: 63 | HyperGraphElementCreator() : _name(typeid(T).name()) {} 64 | #if defined (WINDOWS) && defined(__GNUC__) // force stack alignment on Windows with GCC 65 | __attribute__((force_align_arg_pointer)) 66 | #endif 67 | HyperGraph::HyperGraphElement* construct() { return new T;} 68 | virtual const std::string& name() const { return _name;} 69 | protected: 70 | std::string _name; 71 | }; 72 | 73 | } // end namespace 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/eigen_types.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_EIGEN_TYPES_H 28 | #define G2O_EIGEN_TYPES_H 29 | 30 | #include 31 | #include 32 | 33 | namespace g2o { 34 | 35 | typedef Eigen::Matrix Vector2I; 36 | typedef Eigen::Matrix Vector3I; 37 | typedef Eigen::Matrix Vector4I; 38 | typedef Eigen::Matrix VectorXI; 39 | 40 | typedef Eigen::Matrix Vector2F; 41 | typedef Eigen::Matrix Vector3F; 42 | typedef Eigen::Matrix Vector4F; 43 | typedef Eigen::Matrix VectorXF; 44 | 45 | typedef Eigen::Matrix Vector2D; 46 | typedef Eigen::Matrix Vector3D; 47 | typedef Eigen::Matrix Vector4D; 48 | typedef Eigen::Matrix VectorXD; 49 | 50 | typedef Eigen::Matrix Matrix2I; 51 | typedef Eigen::Matrix Matrix3I; 52 | typedef Eigen::Matrix Matrix4I; 53 | typedef Eigen::Matrix MatrixXI; 54 | 55 | typedef Eigen::Matrix Matrix2F; 56 | typedef Eigen::Matrix Matrix3F; 57 | typedef Eigen::Matrix Matrix4F; 58 | typedef Eigen::Matrix MatrixXF; 59 | 60 | typedef Eigen::Matrix Matrix2D; 61 | typedef Eigen::Matrix Matrix3D; 62 | typedef Eigen::Matrix Matrix4D; 63 | typedef Eigen::Matrix MatrixXD; 64 | 65 | typedef Eigen::Transform Isometry2D; 66 | typedef Eigen::Transform Isometry3D; 67 | 68 | typedef Eigen::Transform Affine2D; 69 | typedef Eigen::Transform Affine3D; 70 | 71 | } // end namespace g2o 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/jacobian_workspace.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "jacobian_workspace.h" 28 | 29 | #include 30 | 31 | #include "optimizable_graph.h" 32 | 33 | using namespace std; 34 | 35 | namespace g2o { 36 | 37 | JacobianWorkspace::JacobianWorkspace() : 38 | _maxNumVertices(-1), _maxDimension(-1) 39 | { 40 | } 41 | 42 | JacobianWorkspace::~JacobianWorkspace() 43 | { 44 | } 45 | 46 | bool JacobianWorkspace::allocate() 47 | { 48 | //cerr << __PRETTY_FUNCTION__ << " " << PVAR(this) << " " << PVAR(_maxNumVertices) << " " << PVAR(_maxDimension) << endl; 49 | if (_maxNumVertices <=0 || _maxDimension <= 0) 50 | return false; 51 | _workspace.resize(_maxNumVertices); 52 | for (WorkspaceVector::iterator it = _workspace.begin(); it != _workspace.end(); ++it) { 53 | it->resize(_maxDimension); 54 | it->setZero(); 55 | } 56 | return true; 57 | } 58 | 59 | void JacobianWorkspace::updateSize(const HyperGraph::Edge* e_) 60 | { 61 | const OptimizableGraph::Edge* e = static_cast(e_); 62 | int errorDimension = e->dimension(); 63 | int numVertices = e->vertices().size(); 64 | int maxDimensionForEdge = -1; 65 | for (int i = 0; i < numVertices; ++i) { 66 | const OptimizableGraph::Vertex* v = static_cast(e->vertex(i)); 67 | assert(v && "Edge has no vertex assigned"); 68 | maxDimensionForEdge = max(v->dimension() * errorDimension, maxDimensionForEdge); 69 | } 70 | _maxNumVertices = max(numVertices, _maxNumVertices); 71 | _maxDimension = max(maxDimensionForEdge, _maxDimension); 72 | //cerr << __PRETTY_FUNCTION__ << " " << PVAR(this) << " " << PVAR(_maxNumVertices) << " " << PVAR(_maxDimension) << endl; 73 | } 74 | 75 | void JacobianWorkspace::updateSize(const OptimizableGraph& graph) 76 | { 77 | for (OptimizableGraph::EdgeSet::const_iterator it = graph.edges().begin(); it != graph.edges().end(); ++it) { 78 | const OptimizableGraph::Edge* e = static_cast(*it); 79 | updateSize(e); 80 | } 81 | } 82 | 83 | void JacobianWorkspace::updateSize(int numVertices, int dimension) 84 | { 85 | _maxNumVertices = max(numVertices, _maxNumVertices); 86 | _maxDimension = max(dimension, _maxDimension); 87 | } 88 | 89 | } // end namespace 90 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/jacobian_workspace.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef JACOBIAN_WORKSPACE_H 28 | #define JACOBIAN_WORKSPACE_H 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | #include "hyper_graph.h" 37 | 38 | namespace g2o { 39 | 40 | struct OptimizableGraph; 41 | 42 | /** 43 | * \brief provide memory workspace for computing the Jacobians 44 | * 45 | * The workspace is used by an OptimizableGraph to provide temporary memory 46 | * for computing the Jacobian of the error functions. 47 | * Before calling linearizeOplus on an edge, the workspace needs to be allocated 48 | * by calling allocate(). 49 | */ 50 | class JacobianWorkspace 51 | { 52 | public: 53 | typedef std::vector > WorkspaceVector; 54 | 55 | public: 56 | JacobianWorkspace(); 57 | ~JacobianWorkspace(); 58 | 59 | /** 60 | * allocate the workspace 61 | */ 62 | bool allocate(); 63 | 64 | /** 65 | * update the maximum required workspace needed by taking into account this edge 66 | */ 67 | void updateSize(const HyperGraph::Edge* e); 68 | 69 | /** 70 | * update the required workspace by looking at a full graph 71 | */ 72 | void updateSize(const OptimizableGraph& graph); 73 | 74 | /** 75 | * manually update with the given parameters 76 | */ 77 | void updateSize(int numVertices, int dimension); 78 | 79 | /** 80 | * return the workspace for a vertex in an edge 81 | */ 82 | double* workspaceForVertex(int vertexIndex) 83 | { 84 | assert(vertexIndex >= 0 && (size_t)vertexIndex < _workspace.size() && "Index out of bounds"); 85 | return _workspace[vertexIndex].data(); 86 | } 87 | 88 | protected: 89 | WorkspaceVector _workspace; ///< the memory pre-allocated for computing the Jacobians 90 | int _maxNumVertices; ///< the maximum number of vertices connected by a hyper-edge 91 | int _maxDimension; ///< the maximum dimension (number of elements) for a Jacobian 92 | }; 93 | 94 | } // end namespace 95 | 96 | #endif 97 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/linear_solver.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_LINEAR_SOLVER_H 28 | #define G2O_LINEAR_SOLVER_H 29 | #include "sparse_block_matrix.h" 30 | #include "sparse_block_matrix_ccs.h" 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief basic solver for Ax = b 36 | * 37 | * basic solver for Ax = b which has to reimplemented for different linear algebra libraries. 38 | * A is assumed to be symmetric (only upper triangular block is stored) and positive-semi-definit. 39 | */ 40 | template 41 | class LinearSolver 42 | { 43 | public: 44 | LinearSolver() {}; 45 | virtual ~LinearSolver() {} 46 | 47 | /** 48 | * init for operating on matrices with a different non-zero pattern like before 49 | */ 50 | virtual bool init() = 0; 51 | 52 | /** 53 | * Assumes that A is the same matrix for several calls. 54 | * Among other assumptions, the non-zero pattern does not change! 55 | * If the matrix changes call init() before. 56 | * solve system Ax = b, x and b have to allocated beforehand!! 57 | */ 58 | virtual bool solve(const SparseBlockMatrix& A, double* x, double* b) = 0; 59 | 60 | /** 61 | * Inverts the diagonal blocks of A 62 | * @returns false if not defined. 63 | */ 64 | virtual bool solveBlocks(double**&blocks, const SparseBlockMatrix& A) { (void)blocks; (void) A; return false; } 65 | 66 | 67 | /** 68 | * Inverts the a block pattern of A in spinv 69 | * @returns false if not defined. 70 | */ 71 | virtual bool solvePattern(SparseBlockMatrix& spinv, const std::vector >& blockIndices, const SparseBlockMatrix& A){ 72 | (void) spinv; 73 | (void) blockIndices; 74 | (void) A; 75 | return false; 76 | } 77 | 78 | //! write a debug dump of the system matrix if it is not PSD in solve 79 | virtual bool writeDebug() const { return false;} 80 | virtual void setWriteDebug(bool) {} 81 | }; 82 | 83 | /** 84 | * \brief Solver with faster iterating structure for the linear matrix 85 | */ 86 | template 87 | class LinearSolverCCS : public LinearSolver 88 | { 89 | public: 90 | LinearSolverCCS() : LinearSolver(), _ccsMatrix(0) {} 91 | ~LinearSolverCCS() 92 | { 93 | delete _ccsMatrix; 94 | } 95 | 96 | protected: 97 | SparseBlockMatrixCCS* _ccsMatrix; 98 | 99 | void initMatrixStructure(const SparseBlockMatrix& A) 100 | { 101 | delete _ccsMatrix; 102 | _ccsMatrix = new SparseBlockMatrixCCS(A.rowBlockIndices(), A.colBlockIndices()); 103 | A.fillSparseBlockMatrixCCS(*_ccsMatrix); 104 | } 105 | }; 106 | 107 | } // end namespace 108 | 109 | #endif 110 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/matrix_operations.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_CORE_MATRIX_OPERATIONS_H 28 | #define G2O_CORE_MATRIX_OPERATIONS_H 29 | 30 | #include 31 | 32 | namespace g2o { 33 | namespace internal { 34 | 35 | template 36 | inline void axpy(const MatrixType& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 37 | { 38 | y.segment(yoff) += A * x.segment(xoff); 39 | } 40 | 41 | template 42 | inline void axpy(const Eigen::Matrix& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 43 | { 44 | y.segment(yoff, A.rows()) += A * x.segment::ColsAtCompileTime>(xoff); 45 | } 46 | 47 | template<> 48 | inline void axpy(const Eigen::MatrixXd& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 49 | { 50 | y.segment(yoff, A.rows()) += A * x.segment(xoff, A.cols()); 51 | } 52 | 53 | template 54 | inline void atxpy(const MatrixType& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 55 | { 56 | y.segment(yoff) += A.transpose() * x.segment(xoff); 57 | } 58 | 59 | template 60 | inline void atxpy(const Eigen::Matrix& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 61 | { 62 | y.segment::ColsAtCompileTime>(yoff) += A.transpose() * x.segment(xoff, A.rows()); 63 | } 64 | 65 | template<> 66 | inline void atxpy(const Eigen::MatrixXd& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 67 | { 68 | y.segment(yoff, A.cols()) += A.transpose() * x.segment(xoff, A.rows()); 69 | } 70 | 71 | } // end namespace internal 72 | } // end namespace g2o 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/matrix_structure.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "matrix_structure.h" 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | using namespace std; 34 | 35 | namespace g2o { 36 | 37 | struct ColSort 38 | { 39 | bool operator()(const pair& e1, const pair& e2) const 40 | { 41 | return e1.second < e2.second || (e1.second == e2.second && e1.first < e2.first); 42 | } 43 | }; 44 | 45 | MatrixStructure::MatrixStructure() : 46 | n(0), m(0), Ap(0), Aii(0), maxN(0), maxNz(0) 47 | { 48 | } 49 | 50 | MatrixStructure::~MatrixStructure() 51 | { 52 | free(); 53 | } 54 | 55 | void MatrixStructure::alloc(int n_, int nz) 56 | { 57 | if (n == 0) { 58 | maxN = n = n_; 59 | maxNz = nz; 60 | Ap = new int[maxN + 1]; 61 | Aii = new int[maxNz]; 62 | } 63 | else { 64 | n = n_; 65 | if (maxNz < nz) { 66 | maxNz = 2 * nz; 67 | delete[] Aii; 68 | Aii = new int[maxNz]; 69 | } 70 | if (maxN < n) { 71 | maxN = 2 * n; 72 | delete[] Ap; 73 | Ap = new int[maxN + 1]; 74 | } 75 | } 76 | } 77 | 78 | void MatrixStructure::free() 79 | { 80 | n = 0; 81 | m = 0; 82 | maxN = 0; 83 | maxNz = 0; 84 | delete[] Aii; Aii = 0; 85 | delete[] Ap; Ap = 0; 86 | } 87 | 88 | bool MatrixStructure::write(const char* filename) const 89 | { 90 | const int& cols = n; 91 | const int& rows = m; 92 | 93 | string name = filename; 94 | std::string::size_type lastDot = name.find_last_of('.'); 95 | if (lastDot != std::string::npos) 96 | name = name.substr(0, lastDot); 97 | 98 | vector > entries; 99 | for (int i=0; i < cols; ++i) { 100 | const int& rbeg = Ap[i]; 101 | const int& rend = Ap[i+1]; 102 | for (int j = rbeg; j < rend; ++j) { 103 | entries.push_back(make_pair(Aii[j], i)); 104 | if (Aii[j] != i) 105 | entries.push_back(make_pair(i, Aii[j])); 106 | } 107 | } 108 | 109 | sort(entries.begin(), entries.end(), ColSort()); 110 | 111 | std::ofstream fout(filename); 112 | fout << "# name: " << name << std::endl; 113 | fout << "# type: sparse matrix" << std::endl; 114 | fout << "# nnz: " << entries.size() << std::endl; 115 | fout << "# rows: " << rows << std::endl; 116 | fout << "# columns: " << cols << std::endl; 117 | for (vector >::const_iterator it = entries.begin(); it != entries.end(); ++it) { 118 | const pair& entry = *it; 119 | fout << entry.first << " " << entry.second << " 0" << std::endl; // write a constant value of 0 120 | } 121 | 122 | return fout.good(); 123 | } 124 | 125 | } // end namespace 126 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/matrix_structure.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_MATRIX_STRUCTURE_H 28 | #define G2O_MATRIX_STRUCTURE_H 29 | 30 | 31 | namespace g2o { 32 | 33 | /** 34 | * \brief representing the structure of a matrix in column compressed structure (only the upper triangular part of the matrix) 35 | */ 36 | class MatrixStructure 37 | { 38 | public: 39 | MatrixStructure(); 40 | ~MatrixStructure(); 41 | /** 42 | * allocate space for the Matrix Structure. You may call this on an already allocated struct, it will 43 | * then reallocate the memory + additional space (double the required space). 44 | */ 45 | void alloc(int n_, int nz); 46 | 47 | void free(); 48 | 49 | /** 50 | * Write the matrix pattern to a file. File is also loadable by octave, e.g., then use spy(matrix) 51 | */ 52 | bool write(const char* filename) const; 53 | 54 | int n; ///< A is m-by-n. n must be >= 0. 55 | int m; ///< A is m-by-n. m must be >= 0. 56 | int* Ap; ///< column pointers for A, of size n+1 57 | int* Aii; ///< row indices of A, of size nz = Ap [n] 58 | 59 | //! max number of non-zeros blocks 60 | int nzMax() const { return maxNz;} 61 | 62 | protected: 63 | int maxN; ///< size of the allocated memory 64 | int maxNz; ///< size of the allocated memory 65 | }; 66 | 67 | } // end namespace 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/openmp_mutex.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPENMP_MUTEX 28 | #define G2O_OPENMP_MUTEX 29 | 30 | #include "../../config.h" 31 | 32 | #ifdef G2O_OPENMP 33 | #include 34 | #else 35 | #include 36 | #endif 37 | 38 | namespace g2o { 39 | 40 | #ifdef G2O_OPENMP 41 | 42 | /** 43 | * \brief Mutex realized via OpenMP 44 | */ 45 | class OpenMPMutex 46 | { 47 | public: 48 | OpenMPMutex() { omp_init_lock(&_lock); } 49 | ~OpenMPMutex() { omp_destroy_lock(&_lock); } 50 | void lock() { omp_set_lock(&_lock); } 51 | void unlock() { omp_unset_lock(&_lock); } 52 | protected: 53 | omp_lock_t _lock; 54 | }; 55 | 56 | #else 57 | 58 | /* 59 | * dummy which does nothing in case we don't have OpenMP support. 60 | * In debug mode, the mutex allows to verify the correct lock and unlock behavior 61 | */ 62 | class OpenMPMutex 63 | { 64 | public: 65 | #ifdef NDEBUG 66 | OpenMPMutex() {} 67 | #else 68 | OpenMPMutex() : _cnt(0) {} 69 | #endif 70 | ~OpenMPMutex() { assert(_cnt == 0 && "Freeing locked mutex");} 71 | void lock() { assert(++_cnt == 1 && "Locking already locked mutex");} 72 | void unlock() { assert(--_cnt == 0 && "Trying to unlock a mutex which is not locked");} 73 | protected: 74 | #ifndef NDEBUG 75 | char _cnt; 76 | #endif 77 | }; 78 | 79 | #endif 80 | 81 | /** 82 | * \brief lock a mutex within a scope 83 | */ 84 | class ScopedOpenMPMutex 85 | { 86 | public: 87 | explicit ScopedOpenMPMutex(OpenMPMutex* mutex) : _mutex(mutex) { _mutex->lock(); } 88 | ~ScopedOpenMPMutex() { _mutex->unlock(); } 89 | private: 90 | OpenMPMutex* const _mutex; 91 | ScopedOpenMPMutex(const ScopedOpenMPMutex&); 92 | void operator=(const ScopedOpenMPMutex&); 93 | }; 94 | 95 | } 96 | 97 | #endif 98 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/optimization_algorithm.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "optimization_algorithm.h" 28 | 29 | using namespace std; 30 | 31 | namespace g2o { 32 | 33 | OptimizationAlgorithm::OptimizationAlgorithm() : 34 | _optimizer(0) 35 | { 36 | } 37 | 38 | OptimizationAlgorithm::~OptimizationAlgorithm() 39 | { 40 | } 41 | 42 | void OptimizationAlgorithm::printProperties(std::ostream& os) const 43 | { 44 | os << "------------- Algorithm Properties -------------" << endl; 45 | for (PropertyMap::const_iterator it = _properties.begin(); it != _properties.end(); ++it) { 46 | BaseProperty* p = it->second; 47 | os << it->first << "\t" << p->toString() << endl; 48 | } 49 | os << "------------------------------------------------" << endl; 50 | } 51 | 52 | bool OptimizationAlgorithm::updatePropertiesFromString(const std::string& propString) 53 | { 54 | return _properties.updateMapFromString(propString); 55 | } 56 | 57 | void OptimizationAlgorithm::setOptimizer(SparseOptimizer* optimizer) 58 | { 59 | _optimizer = optimizer; 60 | } 61 | 62 | } // end namespace 63 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H 29 | 30 | #include "optimization_algorithm_with_hessian.h" 31 | 32 | namespace g2o { 33 | 34 | class BlockSolverBase; 35 | 36 | /** 37 | * \brief Implementation of Powell's Dogleg Algorithm 38 | */ 39 | class OptimizationAlgorithmDogleg : public OptimizationAlgorithmWithHessian 40 | { 41 | public: 42 | /** \brief type of the step to take */ 43 | enum { 44 | STEP_UNDEFINED, 45 | STEP_SD, STEP_GN, STEP_DL 46 | }; 47 | 48 | public: 49 | /** 50 | * construct the Dogleg algorithm, which will use the given Solver for solving the 51 | * linearized system. 52 | */ 53 | explicit OptimizationAlgorithmDogleg(BlockSolverBase* solver); 54 | virtual ~OptimizationAlgorithmDogleg(); 55 | 56 | virtual SolverResult solve(int iteration, bool online = false); 57 | 58 | virtual void printVerbose(std::ostream& os) const; 59 | 60 | //! return the type of the last step taken by the algorithm 61 | int lastStep() const { return _lastStep;} 62 | //! return the diameter of the trust region 63 | double trustRegion() const { return _delta;} 64 | 65 | //! convert the type into an integer 66 | static const char* stepType2Str(int stepType); 67 | 68 | protected: 69 | // parameters 70 | Property* _maxTrialsAfterFailure; 71 | Property* _userDeltaInit; 72 | // damping to enforce positive definite matrix 73 | Property* _initialLambda; 74 | Property* _lamdbaFactor; 75 | 76 | Eigen::VectorXd _hsd; ///< steepest decent step 77 | Eigen::VectorXd _hdl; ///< final dogleg step 78 | Eigen::VectorXd _auxVector; ///< auxilary vector used to perform multiplications or other stuff 79 | 80 | double _currentLambda; ///< the damping factor to force positive definite matrix 81 | double _delta; ///< trust region 82 | int _lastStep; ///< type of the step taken by the algorithm 83 | bool _wasPDInAllIterations; ///< the matrix we solve was positive definite in all iterations -> if not apply damping 84 | int _lastNumTries; 85 | }; 86 | 87 | } // end namespace 88 | 89 | #endif 90 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "optimization_algorithm_gauss_newton.h" 28 | 29 | #include 30 | 31 | #include "../stuff/timeutil.h" 32 | #include "../stuff/macros.h" 33 | 34 | #include "solver.h" 35 | #include "batch_stats.h" 36 | #include "sparse_optimizer.h" 37 | using namespace std; 38 | 39 | namespace g2o { 40 | 41 | OptimizationAlgorithmGaussNewton::OptimizationAlgorithmGaussNewton(Solver* solver) : 42 | OptimizationAlgorithmWithHessian(solver) 43 | { 44 | } 45 | 46 | OptimizationAlgorithmGaussNewton::~OptimizationAlgorithmGaussNewton() 47 | { 48 | } 49 | 50 | OptimizationAlgorithm::SolverResult OptimizationAlgorithmGaussNewton::solve(int iteration, bool online) 51 | { 52 | assert(_optimizer && "_optimizer not set"); 53 | assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph"); 54 | bool ok = true; 55 | 56 | //here so that correct component for max-mixtures can be computed before the build structure 57 | double t=get_monotonic_time(); 58 | _optimizer->computeActiveErrors(); 59 | G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats(); 60 | if (globalStats) { 61 | globalStats->timeResiduals = get_monotonic_time()-t; 62 | } 63 | 64 | if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure 65 | ok = _solver->buildStructure(); 66 | if (! ok) { 67 | cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl; 68 | return OptimizationAlgorithm::Fail; 69 | } 70 | } 71 | 72 | t=get_monotonic_time(); 73 | _solver->buildSystem(); 74 | if (globalStats) { 75 | globalStats->timeQuadraticForm = get_monotonic_time()-t; 76 | t=get_monotonic_time(); 77 | } 78 | 79 | ok = _solver->solve(); 80 | if (globalStats) { 81 | globalStats->timeLinearSolution = get_monotonic_time()-t; 82 | t=get_monotonic_time(); 83 | } 84 | 85 | _optimizer->update(_solver->x()); 86 | if (globalStats) { 87 | globalStats->timeUpdate = get_monotonic_time()-t; 88 | } 89 | if (ok) 90 | return OK; 91 | else 92 | return Fail; 93 | } 94 | 95 | void OptimizationAlgorithmGaussNewton::printVerbose(std::ostream& os) const 96 | { 97 | os 98 | << "\t schur= " << _solver->schur(); 99 | } 100 | 101 | } // end namespace 102 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H 29 | 30 | #include "optimization_algorithm_with_hessian.h" 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief Implementation of the Gauss Newton Algorithm 36 | */ 37 | class OptimizationAlgorithmGaussNewton : public OptimizationAlgorithmWithHessian 38 | { 39 | public: 40 | /** 41 | * construct the Gauss Newton algorithm, which use the given Solver for solving the 42 | * linearized system. 43 | */ 44 | explicit OptimizationAlgorithmGaussNewton(Solver* solver); 45 | virtual ~OptimizationAlgorithmGaussNewton(); 46 | 47 | virtual SolverResult solve(int iteration, bool online = false); 48 | 49 | virtual void printVerbose(std::ostream& os) const; 50 | }; 51 | 52 | } // end namespace 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_SOLVER_LEVENBERG_H 28 | #define G2O_SOLVER_LEVENBERG_H 29 | 30 | #include "optimization_algorithm_with_hessian.h" 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief Implementation of the Levenberg Algorithm 36 | */ 37 | class OptimizationAlgorithmLevenberg : public OptimizationAlgorithmWithHessian 38 | { 39 | public: 40 | /** 41 | * construct the Levenberg algorithm, which will use the given Solver for solving the 42 | * linearized system. 43 | */ 44 | explicit OptimizationAlgorithmLevenberg(Solver* solver); 45 | virtual ~OptimizationAlgorithmLevenberg(); 46 | 47 | virtual SolverResult solve(int iteration, bool online = false); 48 | 49 | virtual void printVerbose(std::ostream& os) const; 50 | 51 | //! return the currently used damping factor 52 | double currentLambda() const { return _currentLambda;} 53 | 54 | //! the number of internal iteration if an update step increases chi^2 within Levenberg-Marquardt 55 | void setMaxTrialsAfterFailure(int max_trials); 56 | 57 | //! get the number of inner iterations for Levenberg-Marquardt 58 | int maxTrialsAfterFailure() const { return _maxTrialsAfterFailure->value();} 59 | 60 | //! return the lambda set by the user, if < 0 the SparseOptimizer will compute the initial lambda 61 | double userLambdaInit() {return _userLambdaInit->value();} 62 | //! specify the initial lambda used for the first iteraion, if not given the SparseOptimizer tries to compute a suitable value 63 | void setUserLambdaInit(double lambda); 64 | 65 | //! return the number of levenberg iterations performed in the last round 66 | int levenbergIteration() { return _levenbergIterations;} 67 | 68 | protected: 69 | // Levenberg parameters 70 | Property* _maxTrialsAfterFailure; 71 | Property* _userLambdaInit; 72 | double _currentLambda; 73 | double _tau; 74 | double _goodStepLowerScale; ///< lower bound for lambda decrease if a good LM step 75 | double _goodStepUpperScale; ///< upper bound for lambda decrease if a good LM step 76 | double _ni; 77 | int _levenbergIterations; ///< the numer of levenberg iterations performed to accept the last step 78 | //RAUL 79 | int _nBad; 80 | 81 | /** 82 | * helper for Levenberg, this function computes the initial damping factor, if the user did not 83 | * specify an own value, see setUserLambdaInit() 84 | */ 85 | double computeLambdaInit() const; 86 | double computeScale() const; 87 | 88 | }; 89 | 90 | } // end namespace 91 | 92 | #endif 93 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/optimization_algorithm_property.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H 29 | 30 | #include 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief describe the properties of a solver 36 | */ 37 | struct OptimizationAlgorithmProperty 38 | { 39 | std::string name; ///< name of the solver, e.g., var 40 | std::string desc; ///< short description of the solver 41 | std::string type; ///< type of solver, e.g., "CSparse Cholesky", "PCG" 42 | bool requiresMarginalize; ///< whether the solver requires marginalization of landmarks 43 | int poseDim; ///< dimension of the pose vertices (-1 if variable) 44 | int landmarkDim; ///< dimension of the landmar vertices (-1 if variable) 45 | OptimizationAlgorithmProperty() : 46 | name(), desc(), type(), requiresMarginalize(false), poseDim(-1), landmarkDim(-1) 47 | { 48 | } 49 | OptimizationAlgorithmProperty(const std::string& name_, const std::string& desc_, const std::string& type_, bool requiresMarginalize_, int poseDim_, int landmarkDim_) : 50 | name(name_), desc(desc_), type(type_), requiresMarginalize(requiresMarginalize_), poseDim(poseDim_), landmarkDim(landmarkDim_) 51 | { 52 | } 53 | }; 54 | 55 | } // end namespace 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "optimization_algorithm_with_hessian.h" 28 | 29 | #include "solver.h" 30 | #include "optimizable_graph.h" 31 | #include "sparse_optimizer.h" 32 | 33 | #include 34 | using namespace std; 35 | 36 | namespace g2o { 37 | 38 | OptimizationAlgorithmWithHessian::OptimizationAlgorithmWithHessian(Solver* solver) : 39 | OptimizationAlgorithm(), 40 | _solver(solver) 41 | { 42 | _writeDebug = _properties.makeProperty >("writeDebug", true); 43 | } 44 | 45 | OptimizationAlgorithmWithHessian::~OptimizationAlgorithmWithHessian() 46 | { 47 | delete _solver; 48 | } 49 | 50 | bool OptimizationAlgorithmWithHessian::init(bool online) 51 | { 52 | assert(_optimizer && "_optimizer not set"); 53 | assert(_solver && "Solver not set"); 54 | _solver->setWriteDebug(_writeDebug->value()); 55 | bool useSchur=false; 56 | for (OptimizableGraph::VertexContainer::const_iterator it=_optimizer->activeVertices().begin(); it!=_optimizer->activeVertices().end(); ++it) { 57 | OptimizableGraph::Vertex* v= *it; 58 | if (v->marginalized()){ 59 | useSchur=true; 60 | break; 61 | } 62 | } 63 | if (useSchur){ 64 | if (_solver->supportsSchur()) 65 | _solver->setSchur(true); 66 | } else { 67 | if (_solver->supportsSchur()) 68 | _solver->setSchur(false); 69 | } 70 | 71 | bool initState = _solver->init(_optimizer, online); 72 | return initState; 73 | } 74 | 75 | bool OptimizationAlgorithmWithHessian::computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices) 76 | { 77 | return _solver ? _solver->computeMarginals(spinv, blockIndices) : false; 78 | } 79 | 80 | bool OptimizationAlgorithmWithHessian::buildLinearStructure() 81 | { 82 | return _solver ? _solver->buildStructure() : false; 83 | } 84 | 85 | void OptimizationAlgorithmWithHessian::updateLinearSystem() 86 | { 87 | if (_solver) 88 | _solver->buildSystem(); 89 | } 90 | 91 | bool OptimizationAlgorithmWithHessian::updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges) 92 | { 93 | return _solver ? _solver->updateStructure(vset, edges) : false; 94 | } 95 | 96 | void OptimizationAlgorithmWithHessian::setWriteDebug(bool writeDebug) 97 | { 98 | _writeDebug->setValue(writeDebug); 99 | } 100 | 101 | } // end namespace 102 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H 29 | 30 | #include "optimization_algorithm.h" 31 | 32 | namespace g2o { 33 | 34 | class Solver; 35 | 36 | /** 37 | * \brief Base for solvers operating on the approximated Hessian, e.g., Gauss-Newton, Levenberg 38 | */ 39 | class OptimizationAlgorithmWithHessian : public OptimizationAlgorithm 40 | { 41 | public: 42 | explicit OptimizationAlgorithmWithHessian(Solver* solver); 43 | virtual ~OptimizationAlgorithmWithHessian(); 44 | 45 | virtual bool init(bool online = false); 46 | 47 | virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices); 48 | 49 | virtual bool buildLinearStructure(); 50 | 51 | virtual void updateLinearSystem(); 52 | 53 | virtual bool updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges); 54 | 55 | //! return the underlying solver used to solve the linear system 56 | Solver* solver() { return _solver;} 57 | 58 | /** 59 | * write debug output of the Hessian if system is not positive definite 60 | */ 61 | virtual void setWriteDebug(bool writeDebug); 62 | virtual bool writeDebug() const { return _writeDebug->value();} 63 | 64 | protected: 65 | Solver* _solver; 66 | Property* _writeDebug; 67 | 68 | }; 69 | 70 | }// end namespace 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/parameter.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "parameter.h" 28 | 29 | namespace g2o { 30 | 31 | Parameter::Parameter() : _id(-1) 32 | { 33 | } 34 | 35 | void Parameter::setId(int id_) 36 | { 37 | _id = id_; 38 | } 39 | 40 | } // end namespace 41 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/parameter.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_GRAPH_PARAMETER_HH_ 28 | #define G2O_GRAPH_PARAMETER_HH_ 29 | 30 | #include 31 | 32 | #include "hyper_graph.h" 33 | 34 | namespace g2o { 35 | 36 | class Parameter : public HyperGraph::HyperGraphElement 37 | { 38 | public: 39 | Parameter(); 40 | virtual ~Parameter() {}; 41 | //! read the data from a stream 42 | virtual bool read(std::istream& is) = 0; 43 | //! write the data to a stream 44 | virtual bool write(std::ostream& os) const = 0; 45 | int id() const {return _id;} 46 | void setId(int id_); 47 | virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_PARAMETER;} 48 | protected: 49 | int _id; 50 | }; 51 | 52 | typedef std::vector ParameterVector; 53 | 54 | } // end namespace 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/parameter_container.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_GRAPH_PARAMETER_CONTAINER_HH_ 28 | #define G2O_GRAPH_PARAMETER_CONTAINER_HH_ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace g2o { 35 | 36 | class Parameter; 37 | 38 | /** 39 | * \brief map id to parameters 40 | */ 41 | class ParameterContainer : protected std::map 42 | { 43 | public: 44 | typedef std::map BaseClass; 45 | 46 | /** 47 | * create a container for the parameters. 48 | * @param isMainStorage_ pointers to the parameters are owned by this container, i.e., freed in its constructor 49 | */ 50 | ParameterContainer(bool isMainStorage_=true); 51 | virtual ~ParameterContainer(); 52 | //! add parameter to the container 53 | bool addParameter(Parameter* p); 54 | //! return a parameter based on its ID 55 | Parameter* getParameter(int id); 56 | //! remove a parameter from the container, i.e., the user now owns the pointer 57 | Parameter* detachParameter(int id); 58 | //! read parameters from a stream 59 | virtual bool read(std::istream& is, const std::map* renamedMap =0); 60 | //! write the data to a stream 61 | virtual bool write(std::ostream& os) const; 62 | bool isMainStorage() const {return _isMainStorage;} 63 | void clear(); 64 | 65 | // stuff of the base class that should re-appear 66 | using BaseClass::size; 67 | 68 | protected: 69 | bool _isMainStorage; 70 | }; 71 | 72 | } // end namespace 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/robust_kernel.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "robust_kernel.h" 28 | 29 | namespace g2o { 30 | 31 | RobustKernel::RobustKernel() : 32 | _delta(1.) 33 | { 34 | } 35 | 36 | RobustKernel::RobustKernel(double delta) : 37 | _delta(delta) 38 | { 39 | } 40 | 41 | void RobustKernel::setDelta(double delta) 42 | { 43 | _delta = delta; 44 | } 45 | 46 | } // end namespace g2o 47 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/robust_kernel.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_ROBUST_KERNEL_H 28 | #define G2O_ROBUST_KERNEL_H 29 | 30 | #ifdef _MSC_VER 31 | #include 32 | #else 33 | #include 34 | #endif 35 | #include 36 | 37 | 38 | namespace g2o { 39 | 40 | /** 41 | * \brief base for all robust cost functions 42 | * 43 | * Note in all the implementations for the other cost functions the e in the 44 | * funtions corresponds to the sqaured errors, i.e., the robustification 45 | * functions gets passed the squared error. 46 | * 47 | * e.g. the robustified least squares function is 48 | * 49 | * chi^2 = sum_{e} rho( e^T Omega e ) 50 | */ 51 | class RobustKernel 52 | { 53 | public: 54 | RobustKernel(); 55 | explicit RobustKernel(double delta); 56 | virtual ~RobustKernel() {} 57 | /** 58 | * compute the scaling factor for a error: 59 | * The error is e^T Omega e 60 | * The output rho is 61 | * rho[0]: The actual scaled error value 62 | * rho[1]: First derivative of the scaling function 63 | * rho[2]: Second derivative of the scaling function 64 | */ 65 | virtual void robustify(double squaredError, Eigen::Vector3d& rho) const = 0; 66 | 67 | /** 68 | * set the window size of the error. A squared error above delta^2 is considered 69 | * as outlier in the data. 70 | */ 71 | virtual void setDelta(double delta); 72 | double delta() const { return _delta;} 73 | 74 | protected: 75 | double _delta; 76 | }; 77 | typedef std::tr1::shared_ptr RobustKernelPtr; 78 | 79 | } // end namespace g2o 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/robust_kernel_factory.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "robust_kernel_factory.h" 28 | #include "robust_kernel.h" 29 | 30 | #include 31 | 32 | using namespace std; 33 | 34 | namespace g2o { 35 | 36 | RobustKernelFactory* RobustKernelFactory::factoryInstance = 0; 37 | 38 | RobustKernelFactory::RobustKernelFactory() 39 | { 40 | } 41 | 42 | RobustKernelFactory::~RobustKernelFactory() 43 | { 44 | for (CreatorMap::iterator it = _creator.begin(); it != _creator.end(); ++it) { 45 | delete it->second; 46 | } 47 | _creator.clear(); 48 | } 49 | 50 | RobustKernelFactory* RobustKernelFactory::instance() 51 | { 52 | if (factoryInstance == 0) { 53 | factoryInstance = new RobustKernelFactory; 54 | } 55 | 56 | return factoryInstance; 57 | } 58 | 59 | void RobustKernelFactory::registerRobustKernel(const std::string& tag, AbstractRobustKernelCreator* c) 60 | { 61 | CreatorMap::const_iterator foundIt = _creator.find(tag); 62 | if (foundIt != _creator.end()) { 63 | cerr << "RobustKernelFactory WARNING: Overwriting robust kernel tag " << tag << endl; 64 | assert(0); 65 | } 66 | 67 | _creator[tag] = c; 68 | } 69 | 70 | void RobustKernelFactory::unregisterType(const std::string& tag) 71 | { 72 | CreatorMap::iterator tagPosition = _creator.find(tag); 73 | if (tagPosition != _creator.end()) { 74 | AbstractRobustKernelCreator* c = tagPosition->second; 75 | delete c; 76 | _creator.erase(tagPosition); 77 | } 78 | } 79 | 80 | RobustKernel* RobustKernelFactory::construct(const std::string& tag) const 81 | { 82 | CreatorMap::const_iterator foundIt = _creator.find(tag); 83 | if (foundIt != _creator.end()) { 84 | return foundIt->second->construct(); 85 | } 86 | return 0; 87 | } 88 | 89 | AbstractRobustKernelCreator* RobustKernelFactory::creator(const std::string& tag) const 90 | { 91 | CreatorMap::const_iterator foundIt = _creator.find(tag); 92 | if (foundIt != _creator.end()) { 93 | return foundIt->second; 94 | } 95 | return 0; 96 | } 97 | 98 | void RobustKernelFactory::fillKnownKernels(std::vector& types) const 99 | { 100 | types.clear(); 101 | for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it) 102 | types.push_back(it->first); 103 | } 104 | 105 | void RobustKernelFactory::destroy() 106 | { 107 | delete factoryInstance; 108 | factoryInstance = 0; 109 | } 110 | 111 | } // end namespace 112 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/solver.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "solver.h" 28 | 29 | #include 30 | #include 31 | 32 | namespace g2o { 33 | 34 | Solver::Solver() : 35 | _optimizer(0), _x(0), _b(0), _xSize(0), _maxXSize(0), 36 | _isLevenberg(false), _additionalVectorSpace(0) 37 | { 38 | } 39 | 40 | Solver::~Solver() 41 | { 42 | delete[] _x; 43 | delete[] _b; 44 | } 45 | 46 | void Solver::resizeVector(size_t sx) 47 | { 48 | size_t oldSize = _xSize; 49 | _xSize = sx; 50 | sx += _additionalVectorSpace; // allocate some additional space if requested 51 | if (_maxXSize < sx) { 52 | _maxXSize = 2*sx; 53 | delete[] _x; 54 | _x = new double[_maxXSize]; 55 | #ifndef NDEBUG 56 | memset(_x, 0, _maxXSize * sizeof(double)); 57 | #endif 58 | if (_b) { // backup the former b, might still be needed for online processing 59 | memcpy(_x, _b, oldSize * sizeof(double)); 60 | delete[] _b; 61 | _b = new double[_maxXSize]; 62 | std::swap(_b, _x); 63 | } else { 64 | _b = new double[_maxXSize]; 65 | #ifndef NDEBUG 66 | memset(_b, 0, _maxXSize * sizeof(double)); 67 | #endif 68 | } 69 | } 70 | } 71 | 72 | void Solver::setOptimizer(SparseOptimizer* optimizer) 73 | { 74 | _optimizer = optimizer; 75 | } 76 | 77 | void Solver::setLevenberg(bool levenberg) 78 | { 79 | _isLevenberg = levenberg; 80 | } 81 | 82 | void Solver::setAdditionalVectorSpace(size_t s) 83 | { 84 | _additionalVectorSpace = s; 85 | } 86 | 87 | } // end namespace 88 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/sparse_block_matrix_test.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "sparse_block_matrix.h" 28 | #include 29 | 30 | using namespace std; 31 | using namespace g2o; 32 | using namespace Eigen; 33 | 34 | typedef SparseBlockMatrix< MatrixXd > 35 | SparseBlockMatrixX; 36 | 37 | std::ostream& operator << (std::ostream& os, const SparseBlockMatrixX::SparseMatrixBlock& m) { 38 | for (int i=0; iblock(0,0, true); 55 | cerr << b->rows() << " " << b->cols() << endl; 56 | for (int i=0; irows(); ++i) 57 | for (int j=0; jcols(); ++j){ 58 | (*b)(i,j)=i*b->cols()+j; 59 | } 60 | 61 | 62 | cerr << "block access 2" << endl; 63 | b=M->block(0,2, true); 64 | cerr << b->rows() << " " << b->cols() << endl; 65 | for (int i=0; irows(); ++i) 66 | for (int j=0; jcols(); ++j){ 67 | (*b)(i,j)=i*b->cols()+j; 68 | } 69 | 70 | b=M->block(3,2, true); 71 | cerr << b->rows() << " " << b->cols() << endl; 72 | for (int i=0; irows(); ++i) 73 | for (int j=0; jcols(); ++j){ 74 | (*b)(i,j)=i*b->cols()+j; 75 | } 76 | 77 | cerr << *M << endl; 78 | 79 | cerr << "SUM" << endl; 80 | 81 | SparseBlockMatrixX* Ms=0; 82 | M->add(Ms); 83 | M->add(Ms); 84 | cerr << *Ms; 85 | 86 | SparseBlockMatrixX* Mt=0; 87 | M->transpose(Mt); 88 | cerr << *Mt << endl; 89 | 90 | SparseBlockMatrixX* Mp=0; 91 | M->multiply(Mp, Mt); 92 | cerr << *Mp << endl; 93 | 94 | int iperm[]={3,2,1,0}; 95 | SparseBlockMatrixX* PMp=0; 96 | 97 | Mp->symmPermutation(PMp,iperm, false); 98 | cerr << *PMp << endl; 99 | 100 | PMp->clear(true); 101 | Mp->block(3,0)->fill(0.); 102 | Mp->symmPermutation(PMp,iperm, true); 103 | cerr << *PMp << endl; 104 | 105 | 106 | 107 | } 108 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/stuff/color_macros.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_COLOR_MACROS_H 28 | #define G2O_COLOR_MACROS_H 29 | 30 | // font attributes 31 | #define FT_BOLD "\033[1m" 32 | #define FT_UNDERLINE "\033[4m" 33 | 34 | //background color 35 | #define BG_BLACK "\033[40m" 36 | #define BG_RED "\033[41m" 37 | #define BG_GREEN "\033[42m" 38 | #define BG_YELLOW "\033[43m" 39 | #define BG_LIGHTBLUE "\033[44m" 40 | #define BG_MAGENTA "\033[45m" 41 | #define BG_BLUE "\033[46m" 42 | #define BG_WHITE "\033[47m" 43 | 44 | // font color 45 | #define CL_BLACK(s) "\033[30m" << s << "\033[0m" 46 | #define CL_RED(s) "\033[31m" << s << "\033[0m" 47 | #define CL_GREEN(s) "\033[32m" << s << "\033[0m" 48 | #define CL_YELLOW(s) "\033[33m" << s << "\033[0m" 49 | #define CL_LIGHTBLUE(s) "\033[34m" << s << "\033[0m" 50 | #define CL_MAGENTA(s) "\033[35m" << s << "\033[0m" 51 | #define CL_BLUE(s) "\033[36m" << s << "\033[0m" 52 | #define CL_WHITE(s) "\033[37m" << s << "\033[0m" 53 | 54 | #define FG_BLACK "\033[30m" 55 | #define FG_RED "\033[31m" 56 | #define FG_GREEN "\033[32m" 57 | #define FG_YELLOW "\033[33m" 58 | #define FG_LIGHTBLUE "\033[34m" 59 | #define FG_MAGENTA "\033[35m" 60 | #define FG_BLUE "\033[36m" 61 | #define FG_WHITE "\033[37m" 62 | 63 | #define FG_NORM "\033[0m" 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/stuff/g2o_stuff_api.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Description: import/export macros for creating DLLS with Microsoft 3 | * compiler. Any exported function needs to be declared with the 4 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 5 | * for each DLL (arrrrrgh!!!) 6 | * 7 | * 17 Jan 2012 8 | * Email: pupilli@cs.bris.ac.uk 9 | ****************************************************************************/ 10 | #ifndef G2O_STUFF_API_H 11 | #define G2O_STUFF_API_H 12 | 13 | #include "config.h" 14 | 15 | #ifdef _MSC_VER 16 | // We are using a Microsoft compiler: 17 | 18 | #ifdef G2O_SHARED_LIBS 19 | #ifdef stuff_EXPORTS 20 | #define G2O_STUFF_API __declspec(dllexport) 21 | #else 22 | #define G2O_STUFF_API __declspec(dllimport) 23 | #endif 24 | #else 25 | #define G2O_STUFF_API 26 | #endif 27 | 28 | #else 29 | // Not Microsoft compiler so set empty definition: 30 | #define G2O_STUFF_API 31 | #endif 32 | 33 | #endif // G2O_STUFF_API_H 34 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/stuff/os_specific.c: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "os_specific.h" 28 | 29 | #ifdef WINDOWS 30 | 31 | int vasprintf(char** strp, const char* fmt, va_list ap) 32 | { 33 | int n; 34 | int size = 100; 35 | char* p; 36 | char* np; 37 | 38 | if ((p = (char*)malloc(size * sizeof(char))) == NULL) 39 | return -1; 40 | 41 | while (1) { 42 | #ifdef _MSC_VER 43 | n = vsnprintf_s(p, size, size - 1, fmt, ap); 44 | #else 45 | n = vsnprintf(p, size, fmt, ap); 46 | #endif 47 | if (n > -1 && n < size) { 48 | *strp = p; 49 | return n; 50 | } 51 | if (n > -1) 52 | size = n+1; 53 | else 54 | size *= 2; 55 | if ((np = (char*)realloc (p, size * sizeof(char))) == NULL) { 56 | free(p); 57 | return -1; 58 | } else 59 | p = np; 60 | } 61 | } 62 | 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/stuff/os_specific.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OS_SPECIFIC_HH_ 28 | #define G2O_OS_SPECIFIC_HH_ 29 | 30 | #ifdef WINDOWS 31 | #include 32 | #include 33 | #include 34 | #ifndef _WINDOWS 35 | #include 36 | #endif 37 | #define drand48() ((double) rand()/(double)RAND_MAX) 38 | 39 | #ifdef __cplusplus 40 | extern "C" { 41 | #endif 42 | 43 | int vasprintf(char** strp, const char* fmt, va_list ap); 44 | 45 | #ifdef __cplusplus 46 | } 47 | #endif 48 | 49 | #endif 50 | 51 | #ifdef UNIX 52 | #include 53 | // nothing to do on real operating systems 54 | #endif 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/stuff/property.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "property.h" 28 | 29 | #include 30 | #include 31 | 32 | #include "macros.h" 33 | 34 | #include "string_tools.h" 35 | using namespace std; 36 | 37 | namespace g2o { 38 | 39 | BaseProperty::BaseProperty(const std::string name_) :_name(name_){ 40 | } 41 | 42 | BaseProperty::~BaseProperty(){} 43 | 44 | bool PropertyMap::addProperty(BaseProperty* p) { 45 | std::pair result = insert(make_pair(p->name(), p)); 46 | return result.second; 47 | } 48 | 49 | bool PropertyMap::eraseProperty(const std::string& name) { 50 | PropertyMapIterator it=find(name); 51 | if (it==end()) 52 | return false; 53 | delete it->second; 54 | erase(it); 55 | return true; 56 | } 57 | 58 | PropertyMap::~PropertyMap() { 59 | for (PropertyMapIterator it=begin(); it!=end(); it++){ 60 | if (it->second) 61 | delete it->second; 62 | } 63 | } 64 | 65 | bool PropertyMap::updatePropertyFromString(const std::string& name, const std::string& value) 66 | { 67 | PropertyMapIterator it = find(name); 68 | if (it == end()) 69 | return false; 70 | it->second->fromString(value); 71 | return true; 72 | } 73 | 74 | void PropertyMap::writeToCSV(std::ostream& os) const 75 | { 76 | for (PropertyMapConstIterator it=begin(); it!=end(); it++){ 77 | BaseProperty* p =it->second; 78 | os << p->name() << ", "; 79 | } 80 | os << std::endl; 81 | for (PropertyMapConstIterator it=begin(); it!=end(); it++){ 82 | BaseProperty* p =it->second; 83 | os << p->toString() << ", "; 84 | } 85 | os << std::endl; 86 | } 87 | 88 | bool PropertyMap::updateMapFromString(const std::string& values) 89 | { 90 | bool status = true; 91 | vector valuesMap = strSplit(values, ","); 92 | for (size_t i = 0; i < valuesMap.size(); ++i) { 93 | vector m = strSplit(valuesMap[i], "="); 94 | if (m.size() != 2) { 95 | cerr << __PRETTY_FUNCTION__ << ": unable to extract name=value pair from " << valuesMap[i] << endl; 96 | continue; 97 | } 98 | string name = trim(m[0]); 99 | string value = trim(m[1]); 100 | status = status && updatePropertyFromString(name, value); 101 | } 102 | return status; 103 | } 104 | 105 | } // end namespace 106 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/stuff/sparse_helper.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "sparse_helper.h" 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | using namespace std; 36 | 37 | namespace g2o { 38 | 39 | namespace { 40 | struct TripletEntry 41 | { 42 | int r, c; 43 | double x; 44 | TripletEntry(int r_, int c_, double x_) : r(r_), c(c_), x(x_) {} 45 | }; 46 | struct TripletColSort 47 | { 48 | bool operator()(const TripletEntry& e1, const TripletEntry& e2) const 49 | { 50 | return e1.c < e2.c || (e1.c == e2.c && e1.r < e2.r); 51 | } 52 | }; 53 | } 54 | 55 | bool writeVector(const string& filename, const double*v, int n) 56 | { 57 | ofstream os(filename.c_str()); 58 | os << fixed; 59 | for (int i=0; i entries; 67 | entries.reserve((size_t)Ap[cols]); 68 | for (int i=0; i < cols; i++) { 69 | const int& rbeg = Ap[i]; 70 | const int& rend = Ap[i+1]; 71 | for (int j = rbeg; j < rend; j++) { 72 | entries.push_back(TripletEntry(Ai[j], i, Ax[j])); 73 | if (upperTriangleSymmetric && Ai[j] != i) 74 | entries.push_back(TripletEntry(i, Ai[j], Ax[j])); 75 | } 76 | } 77 | sort(entries.begin(), entries.end(), TripletColSort()); 78 | 79 | string name = filename; 80 | std::string::size_type lastDot = name.find_last_of('.'); 81 | if (lastDot != std::string::npos) 82 | name = name.substr(0, lastDot); 83 | 84 | std::ofstream fout(filename.c_str()); 85 | fout << "# name: " << name << std::endl; 86 | fout << "# type: sparse matrix" << std::endl; 87 | fout << "# nnz: " << entries.size() << std::endl; 88 | fout << "# rows: " << rows << std::endl; 89 | fout << "# columns: " << cols << std::endl; 90 | //fout << fixed; 91 | fout << setprecision(9) << endl; 92 | for (vector::const_iterator it = entries.begin(); it != entries.end(); ++it) { 93 | const TripletEntry& entry = *it; 94 | fout << entry.r+1 << " " << entry.c+1 << " " << entry.x << std::endl; 95 | } 96 | return fout.good(); 97 | } 98 | 99 | } // end namespace 100 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/stuff/sparse_helper.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_SPARSE_HELPER_H 28 | #define G2O_SPARSE_HELPER_H 29 | 30 | #include "g2o_stuff_api.h" 31 | 32 | #include 33 | 34 | namespace g2o { 35 | 36 | /** 37 | * write an array to a file, debugging 38 | */ 39 | G2O_STUFF_API bool writeVector(const std::string& filename, const double*v, int n); 40 | 41 | /** 42 | * write a CCS matrix given by pointer to column, row, and values 43 | */ 44 | G2O_STUFF_API bool writeCCSMatrix(const std::string& filename, int rows, int cols, const int* p, const int* i, const double* v, bool upperTriangleSymmetric = true); 45 | 46 | } // end namespace 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/stuff/timeutil.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "timeutil.h" 28 | #include 29 | 30 | #ifdef _WINDOWS 31 | #include 32 | #include 33 | #endif 34 | 35 | #ifdef UNIX 36 | #include 37 | #endif 38 | 39 | namespace g2o { 40 | 41 | #ifdef _WINDOWS 42 | #if defined(_MSC_VER) || defined(_MSC_EXTENSIONS) 43 | #define DELTA_EPOCH_IN_MICROSECS 11644473600000000Ui64 44 | #else 45 | #define DELTA_EPOCH_IN_MICROSECS 11644473600000000ULL 46 | #endif 47 | 48 | struct timezone 49 | { 50 | int tz_minuteswest; /* minutes W of Greenwich */ 51 | int tz_dsttime; /* type of dst correction */ 52 | }; 53 | 54 | int gettimeofday(struct timeval *tv, struct timezone *tz) 55 | { 56 | // Define a structure to receive the current Windows filetime 57 | FILETIME ft; 58 | 59 | // Initialize the present time to 0 and the timezone to UTC 60 | unsigned __int64 tmpres = 0; 61 | static int tzflag = 0; 62 | 63 | if (NULL != tv) 64 | { 65 | GetSystemTimeAsFileTime(&ft); 66 | 67 | // The GetSystemTimeAsFileTime returns the number of 100 nanosecond 68 | // intervals since Jan 1, 1601 in a structure. Copy the high bits to 69 | // the 64 bit tmpres, shift it left by 32 then or in the low 32 bits. 70 | tmpres |= ft.dwHighDateTime; 71 | tmpres <<= 32; 72 | tmpres |= ft.dwLowDateTime; 73 | 74 | // Convert to microseconds by dividing by 10 75 | tmpres /= 10; 76 | 77 | // The Unix epoch starts on Jan 1 1970. Need to subtract the difference 78 | // in seconds from Jan 1 1601. 79 | tmpres -= DELTA_EPOCH_IN_MICROSECS; 80 | 81 | // Finally change microseconds to seconds and place in the seconds value. 82 | // The modulus picks up the microseconds. 83 | tv->tv_sec = (long)(tmpres / 1000000UL); 84 | tv->tv_usec = (long)(tmpres % 1000000UL); 85 | } 86 | 87 | if (NULL != tz) { 88 | if (!tzflag) { 89 | _tzset(); 90 | tzflag++; 91 | } 92 | 93 | long sec; 94 | int hours; 95 | _get_timezone(&sec); 96 | _get_daylight(&hours); 97 | 98 | // Adjust for the timezone west of Greenwich 99 | tz->tz_minuteswest = sec / 60; 100 | tz->tz_dsttime = hours; 101 | } 102 | 103 | return 0; 104 | } 105 | #endif 106 | 107 | ScopeTime::ScopeTime(const char* title) : _title(title), _startTime(get_monotonic_time()) {} 108 | 109 | ScopeTime::~ScopeTime() { 110 | std::cerr << _title<<" took "<<1000*(get_monotonic_time()-_startTime)<<"ms.\n"; 111 | } 112 | 113 | double get_monotonic_time() 114 | { 115 | #if (defined(_POSIX_TIMERS) && (_POSIX_TIMERS+0 >= 0) && defined(_POSIX_MONOTONIC_CLOCK)) 116 | struct timespec ts; 117 | clock_gettime(CLOCK_MONOTONIC, &ts); 118 | return ts.tv_sec + ts.tv_nsec*1e-9; 119 | #else 120 | return get_time(); 121 | #endif 122 | } 123 | 124 | } // end namespace 125 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/stuff/timeutil.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_TIMEUTIL_H 28 | #define G2O_TIMEUTIL_H 29 | 30 | #ifdef _WINDOWS 31 | #include 32 | #else 33 | #include 34 | #endif 35 | 36 | #include 37 | 38 | 39 | /** @addtogroup utils **/ 40 | // @{ 41 | 42 | /** \file timeutil.h 43 | * \brief utility functions for handling time related stuff 44 | */ 45 | 46 | /// Executes code, only if secs are gone since last exec. 47 | /// extended version, in which the current time is given, e.g., timestamp of IPC message 48 | #ifndef DO_EVERY_TS 49 | #define DO_EVERY_TS(secs, currentTime, code) \ 50 | if (1) {\ 51 | static double s_lastDone_ = (currentTime); \ 52 | double s_now_ = (currentTime); \ 53 | if (s_lastDone_ > s_now_) \ 54 | s_lastDone_ = s_now_; \ 55 | if (s_now_ - s_lastDone_ > (secs)) { \ 56 | code; \ 57 | s_lastDone_ = s_now_; \ 58 | }\ 59 | } else \ 60 | (void)0 61 | #endif 62 | 63 | /// Executes code, only if secs are gone since last exec. 64 | #ifndef DO_EVERY 65 | #define DO_EVERY(secs, code) DO_EVERY_TS(secs, g2o::get_time(), code) 66 | #endif 67 | 68 | #ifndef MEASURE_TIME 69 | #define MEASURE_TIME(text, code) \ 70 | if(1) { \ 71 | double _start_time_ = g2o::get_time(); \ 72 | code; \ 73 | fprintf(stderr, "%s took %f sec\n", text, g2o::get_time() - _start_time_); \ 74 | } else \ 75 | (void) 0 76 | #endif 77 | 78 | namespace g2o { 79 | 80 | #ifdef _WINDOWS 81 | typedef struct timeval { 82 | long tv_sec; 83 | long tv_usec; 84 | } timeval; 85 | int gettimeofday(struct timeval *tv, struct timezone *tz); 86 | #endif 87 | 88 | /** 89 | * return the current time in seconds since 1. Jan 1970 90 | */ 91 | inline double get_time() 92 | { 93 | struct timeval ts; 94 | gettimeofday(&ts,0); 95 | return ts.tv_sec + ts.tv_usec*1e-6; 96 | } 97 | 98 | /** 99 | * return a monotonic increasing time which basically does not need to 100 | * have a reference point. Consider this for measuring how long some 101 | * code fragments required to execute. 102 | * 103 | * On Linux we call clock_gettime() on other systems we currently 104 | * call get_time(). 105 | */ 106 | double get_monotonic_time(); 107 | 108 | /** 109 | * \brief Class to measure the time spent in a scope 110 | * 111 | * To use this class, e.g. to measure the time spent in a function, 112 | * just create and instance at the beginning of the function. 113 | */ 114 | class ScopeTime { 115 | public: 116 | ScopeTime(const char* title); 117 | ~ScopeTime(); 118 | private: 119 | std::string _title; 120 | double _startTime; 121 | }; 122 | 123 | } // end namespace 124 | 125 | #ifndef MEASURE_FUNCTION_TIME 126 | #define MEASURE_FUNCTION_TIME \ 127 | g2o::ScopeTime scopeTime(__PRETTY_FUNCTION__) 128 | #endif 129 | 130 | 131 | // @} 132 | #endif 133 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/types/se3_ops.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 H. Strasdat 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_MATH_STUFF 28 | #define G2O_MATH_STUFF 29 | 30 | #include 31 | #include 32 | 33 | namespace g2o { 34 | using namespace Eigen; 35 | 36 | inline Matrix3d skew(const Vector3d&v); 37 | inline Vector3d deltaR(const Matrix3d& R); 38 | inline Vector2d project(const Vector3d&); 39 | inline Vector3d project(const Vector4d&); 40 | inline Vector3d unproject(const Vector2d&); 41 | inline Vector4d unproject(const Vector3d&); 42 | 43 | #include "se3_ops.hpp" 44 | 45 | } 46 | 47 | #endif //MATH_STUFF 48 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/types/se3_ops.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 H. Strasdat 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | Matrix3d skew(const Vector3d&v) 28 | { 29 | Matrix3d m; 30 | m.fill(0.); 31 | m(0,1) = -v(2); 32 | m(0,2) = v(1); 33 | m(1,2) = -v(0); 34 | m(1,0) = v(2); 35 | m(2,0) = -v(1); 36 | m(2,1) = v(0); 37 | return m; 38 | } 39 | 40 | Vector3d deltaR(const Matrix3d& R) 41 | { 42 | Vector3d v; 43 | v(0)=R(2,1)-R(1,2); 44 | v(1)=R(0,2)-R(2,0); 45 | v(2)=R(1,0)-R(0,1); 46 | return v; 47 | } 48 | 49 | Vector2d project(const Vector3d& v) 50 | { 51 | Vector2d res; 52 | res(0) = v(0)/v(2); 53 | res(1) = v(1)/v(2); 54 | return res; 55 | } 56 | 57 | Vector3d project(const Vector4d& v) 58 | { 59 | Vector3d res; 60 | res(0) = v(0)/v(3); 61 | res(1) = v(1)/v(3); 62 | res(2) = v(2)/v(3); 63 | return res; 64 | } 65 | 66 | Vector3d unproject(const Vector2d& v) 67 | { 68 | Vector3d res; 69 | res(0) = v(0); 70 | res(1) = v(1); 71 | res(2) = 1; 72 | return res; 73 | } 74 | 75 | Vector4d unproject(const Vector3d& v) 76 | { 77 | Vector4d res; 78 | res(0) = v(0); 79 | res(1) = v(1); 80 | res(2) = v(2); 81 | res(3) = 1; 82 | return res; 83 | } 84 | 85 | 86 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/types/types_sba.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 Kurt Konolige 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "types_sba.h" 28 | #include 29 | 30 | namespace g2o { 31 | 32 | using namespace std; 33 | 34 | 35 | VertexSBAPointXYZ::VertexSBAPointXYZ() : BaseVertex<3, Vector3d>() 36 | { 37 | } 38 | 39 | bool VertexSBAPointXYZ::read(std::istream& is) 40 | { 41 | Vector3d lv; 42 | for (int i=0; i<3; i++) 43 | is >> _estimate[i]; 44 | return true; 45 | } 46 | 47 | bool VertexSBAPointXYZ::write(std::ostream& os) const 48 | { 49 | Vector3d lv=estimate(); 50 | for (int i=0; i<3; i++){ 51 | os << lv[i] << " "; 52 | } 53 | return os.good(); 54 | } 55 | 56 | } // end namespace 57 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/types/types_sba.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 Kurt Konolige 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_SBA_TYPES 28 | #define G2O_SBA_TYPES 29 | 30 | #include "../core/base_vertex.h" 31 | 32 | #include 33 | #include 34 | 35 | namespace g2o { 36 | 37 | /** 38 | * \brief Point vertex, XYZ 39 | */ 40 | class VertexSBAPointXYZ : public BaseVertex<3, Vector3d> 41 | { 42 | public: 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 44 | VertexSBAPointXYZ(); 45 | virtual bool read(std::istream& is); 46 | virtual bool write(std::ostream& os) const; 47 | 48 | virtual void setToOriginImpl() { 49 | _estimate.fill(0.); 50 | } 51 | 52 | virtual void oplusImpl(const double* update) 53 | { 54 | Eigen::Map v(update); 55 | _estimate += v; 56 | } 57 | }; 58 | 59 | } // end namespace 60 | 61 | #endif // SBA_TYPES 62 | -------------------------------------------------------------------------------- /thirdparty/g2o/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 | --------------------------------------------------------------------------------