├── CMakeLists.txt ├── CudaComputeHref.cu ├── CudaComputeHref.cuh ├── CudaPoints3d.cu ├── CudaPoints3d.cuh ├── License.txt ├── NID_pose_estimation.cpp ├── NID_standard_property.cpp ├── README.md ├── build.sh ├── cmake_modules └── FindEigen3.cmake ├── config_eth_cvg.yaml ├── 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 │ │ ├── computeH.cu │ │ ├── computeH.cuh │ │ ├── creators.h │ │ ├── eigen_types.h │ │ ├── estimate_propagator.cpp │ │ ├── estimate_propagator.h │ │ ├── factory.cpp │ │ ├── factory.h │ │ ├── hyper_dijkstra.cpp │ │ ├── hyper_dijkstra.h │ │ ├── hyper_graph.cpp │ │ ├── hyper_graph.h │ │ ├── hyper_graph_action.cpp │ │ ├── hyper_graph_action.h │ │ ├── jacobian_workspace.cpp │ │ ├── jacobian_workspace.h │ │ ├── linear_solver.h │ │ ├── marginal_covariance_cholesky.cpp │ │ ├── marginal_covariance_cholesky.h │ │ ├── matrix_operations.h │ │ ├── matrix_structure.cpp │ │ ├── matrix_structure.h │ │ ├── openmp_mutex.h │ │ ├── optimizable_graph.cpp │ │ ├── optimizable_graph.h │ │ ├── optimization_algorithm.cpp │ │ ├── optimization_algorithm.h │ │ ├── optimization_algorithm_dogleg.cpp │ │ ├── optimization_algorithm_dogleg.h │ │ ├── optimization_algorithm_factory.cpp │ │ ├── optimization_algorithm_factory.h │ │ ├── optimization_algorithm_gauss_newton.cpp │ │ ├── optimization_algorithm_gauss_newton.h │ │ ├── optimization_algorithm_levenberg.cpp │ │ ├── optimization_algorithm_levenberg.h │ │ ├── optimization_algorithm_property.h │ │ ├── optimization_algorithm_with_hessian.cpp │ │ ├── optimization_algorithm_with_hessian.h │ │ ├── parameter.cpp │ │ ├── parameter.h │ │ ├── parameter_container.cpp │ │ ├── parameter_container.h │ │ ├── robust_kernel.cpp │ │ ├── robust_kernel.h │ │ ├── robust_kernel_factory.cpp │ │ ├── robust_kernel_factory.h │ │ ├── robust_kernel_impl.cpp │ │ ├── robust_kernel_impl.h │ │ ├── solver.cpp │ │ ├── solver.h │ │ ├── sparse_block_matrix.h │ │ ├── sparse_block_matrix.hpp │ │ ├── sparse_block_matrix_ccs.h │ │ ├── sparse_block_matrix_diagonal.h │ │ ├── sparse_block_matrix_test.cpp │ │ ├── sparse_optimizer.cpp │ │ └── sparse_optimizer.h │ ├── solvers │ │ ├── linear_solver_dense.h │ │ └── linear_solver_eigen.h │ ├── stuff │ │ ├── color_macros.h │ │ ├── macros.h │ │ ├── misc.h │ │ ├── os_specific.c │ │ ├── os_specific.h │ │ ├── property.cpp │ │ ├── property.h │ │ ├── string_tools.cpp │ │ ├── string_tools.h │ │ ├── timeutil.cpp │ │ └── timeutil.h │ └── types │ │ ├── se3_ops.h │ │ ├── se3_ops.hpp │ │ ├── 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 └── run_test_eth_cvg.sh /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | #PROJECT(NID LANGUAGES CXX CUDA) 4 | 5 | set(CMAKE_BUILD_TYPE Release) 6 | 7 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -lstdc++fs") 8 | add_definitions(-DCOMPILEDWITHC14)#add_definitions(-DCOMPILEDWITHC11) 9 | message(STATUS "Using flag -std=c++14.") 10 | add_definitions(-w) #not show warning 11 | 12 | find_package(CUDA QUIET REQUIRED) 13 | include_directories(${CUDA_INCLUDE_DIRS}) 14 | 15 | set(CUDA_NVCC_FLAGS -arch=compute_60) 16 | 17 | LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) 18 | find_package(OpenCV QUIET) 19 | 20 | SET(EIGEN3_INCLUDE_DIR ${G2O_EIGEN3_INCLUDE}) 21 | FIND_PACKAGE(Eigen3 REQUIRED) 22 | IF(EIGEN3_FOUND) 23 | SET(G2O_EIGEN3_INCLUDE ${EIGEN3_INCLUDE_DIR} CACHE PATH "Directory of Eigen3") 24 | ELSE(EIGEN3_FOUND) 25 | SET(G2O_EIGEN3_INCLUDE "" CACHE PATH "Directory of Eigen3") 26 | ENDIF(EIGEN3_FOUND) 27 | 28 | include_directories( 29 | ${PROJECT_SOURCE_DIR} 30 | ${PROJECT_SOURCE_DIR}/include 31 | ${EIGEN3_INCLUDE_DIR} 32 | ${OpenCV_INCLUDE_DIRS} 33 | ) 34 | 35 | SET( EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 36 | 37 | add_executable(NID_standard_property NID_standard_property.cpp) 38 | target_link_libraries(NID_standard_property ${OpenCV_LIBS}) 39 | 40 | cuda_add_executable(NID_pose_estimation NID_pose_estimation.cpp CudaComputeHref.cu CudaPoints3d.cu) 41 | target_link_libraries(NID_pose_estimation ${OpenCV_LIBS} ${PROJECT_SOURCE_DIR}/g2o/lib/libg2o.so) 42 | 43 | -------------------------------------------------------------------------------- /CudaComputeHref.cuh: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "cuda_runtime.h" 3 | #include "device_launch_parameters.h" 4 | 5 | void CudaComputeHref(double* im0, double* points3d, double* pose, double* camera_intrincis, int bin_num, int bs_degree, int cell_num, int rows, int cols, double* bs_value, int* bs_index, int* bs_counter, double* Href); 6 | 7 | -------------------------------------------------------------------------------- /CudaPoints3d.cu: -------------------------------------------------------------------------------- 1 | #include "CudaPoints3d.cuh" 2 | #include 3 | #include 4 | 5 | __global__ void Calculate3DpointKernel(double* depth, double* pose, double* points_3d, double* d_in, int thread_work, int rows, int cols){ 6 | int idx = threadIdx.x + blockDim.x * blockIdx.x; 7 | int idy = threadIdx.y + blockDim.y * blockIdx.y; 8 | 9 | int id = thread_work * (idy * blockDim.x * gridDim.x + idx); 10 | for(int i = 0; i 100){ 13 | points_3d[3*id] = NAN; 14 | points_3d[3*id + 1] = NAN; 15 | points_3d[3*id + 2] = NAN; 16 | id++; 17 | continue; 18 | } 19 | 20 | int row = id / cols; 21 | int col = id % cols; 22 | double x0 = depth[id] * (col - d_in[2]) / d_in[0]; 23 | double y0 = depth[id] * (row - d_in[3]) / d_in[1]; 24 | double z0 = depth[id]; 25 | 26 | points_3d[3*id] = pose[0] * x0 + pose[4] * y0 + pose[8] * z0 + pose[12]; 27 | points_3d[3*id + 1] = pose[1] * x0 + pose[5] * y0 + pose[9] * z0 + pose[13]; 28 | points_3d[3*id + 2] = pose[2] * x0 + pose[6] * y0 + pose[10]* z0 + pose[14]; 29 | 30 | id++; 31 | } 32 | } 33 | 34 | //points_3d is from the unifide memory, no need to copy 35 | void Calculate3Dpoint(double* depth, double* pose_c2w, double* points_3d, double* camera_intrincis, int rows, int cols){ 36 | //check device property 37 | cudaDeviceProp devProp; 38 | cudaGetDeviceProperties(&devProp, 0); 39 | 40 | int block_dim, thread_work; 41 | block_dim = devProp.multiProcessorCount * devProp.maxThreadsPerMultiProcessor / 1024; 42 | //make sure we maximize the use of each thread and in all they can cover all points. 43 | thread_work = rows * cols / (block_dim * 1024); 44 | 45 | while(thread_work * block_dim *1024 < rows * cols) 46 | thread_work++; 47 | 48 | //device variable, allocation memory 49 | //note you cannot define pointer like thie int* a,b; 50 | double* d_depth; 51 | double* d_in; 52 | double* d_c2w; 53 | cudaMalloc((void**)&d_depth, rows*cols*sizeof(double)); 54 | cudaMalloc((void**)&d_in, 5*sizeof(double)); 55 | cudaMalloc((void**)&d_c2w, 16*sizeof(double)); 56 | 57 | //copy data from the host to the device 58 | cudaMemcpy(d_depth, depth, rows*cols*sizeof(double), cudaMemcpyHostToDevice); 59 | cudaMemcpy(d_in, camera_intrincis, 5*sizeof(double), cudaMemcpyHostToDevice); 60 | cudaMemcpy(d_c2w, pose_c2w, 16*sizeof(double), cudaMemcpyHostToDevice); 61 | 62 | //block dimension 63 | dim3 blockNumber(block_dim,1); 64 | dim3 threadNumber(32,32); 65 | 66 | Calculate3DpointKernel<<>>(d_depth, d_c2w, points_3d, d_in, thread_work, rows, cols); 67 | 68 | cudaDeviceSynchronize(); 69 | 70 | //clear 71 | cudaFree(d_depth); 72 | cudaFree(d_in); 73 | cudaFree(d_c2w); 74 | }; -------------------------------------------------------------------------------- /CudaPoints3d.cuh: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "cuda_runtime.h" 3 | #include "device_launch_parameters.h" 4 | 5 | 6 | void Calculate3Dpoint(double* depth, double* pose_c2w, double* points_3d, double* camera_intrincis, int rows, int cols); 7 | -------------------------------------------------------------------------------- /License.txt: -------------------------------------------------------------------------------- 1 | Copyright 2013 Autonomous Robotics & Perception Group 2 | 3 | Licensed under the Apache License, Version 2.0 (the "License"); 4 | you may not use this file except in compliance with the License. 5 | You may obtain a copy of the License at 6 | 7 | http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | Unless required by applicable law or agreed to in writing, software 10 | distributed under the License is distributed on an "AS IS" BASIS, 11 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | See the License for the specific language governing permissions and 13 | limitations under the License. 14 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | This repo is an implementation of NID based pose estimation. Implementtion detial can be seen in paper `Robust Dense Visual SLAM Using Cell Based Normalized Information Distance`. If you use this work, please kindly consider citing 2 | ``` 3 | @inproceedings{chen2021robust, 4 | title={Robust Pose Estimation Based on Normalized Information Distance}, 5 | author={Chen, Zhaozhong and Heckman, Christoffer}, 6 | booktitle={2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 7 | pages={2217--2223}, 8 | year={2021}, 9 | organization={IEEE} 10 | } 11 | 12 | ``` 13 | 14 | ### Requirement 15 | Opencv >= 3.0 16 | Eigen >= 3.1 17 | You have to use the G2O along with the code because NID jacobian estimation is written in the this G2O 18 | If you want to use GPU version, make sure you have CUDA > 8 19 | GPU architecture > `Pascal` 20 | 21 | Test on Ubuntu 18.04, Opecv 3 pass 22 | Test on Ubuntu 20.04 , Opencv 4 failed 23 | ### Install 24 | ``` 25 | git clone https://github.com/arpg/NID-Pose-Estimation.git 26 | cd NID-Pose-Estimation 27 | ./buid.sh 28 | ``` 29 | 30 | ### Use 31 | Downlaod the dataset here https://cvg.ethz.ch/research/illumination-change-robust-dslam/ 32 | In the `config_eth_cvg.yaml` you must specify where the dataset is first. Then can sepcify which two images you want to use. Finally you specify you want to run it on CPU or GPU. Then simply use the following to run the code. You need wait for a while to see the result if you run it on CPU. It should be around 100~300 times faster on different GPU. 33 | ``` 34 | cd bin 35 | ./NID_pose_estimation ../config_eth_cvg.yaml 36 | ``` 37 | 38 | In the code, a noise is added to the groundtruth, then you can see the original pose, pose with noise and pose estimated by NID. 39 | You may also be interested in seeing what's the property of NID estimation without B spline, simply run 40 | ``` 41 | ./NID_standard_property ../config_eth_cvg.yaml 42 | ``` 43 | -------------------------------------------------------------------------------- /build.sh: -------------------------------------------------------------------------------- 1 | echo "Configuring and building g2o ..." 2 | 3 | #Use Relase in g2o here will lead to a problem. 4 | #const VertexSE3Expmap* v1 = static_cast(_vertices[0]) 5 | #std::cout<<"to be estimated matrix \n"<estimate().to_homogeneous_matrix()<setEstimate(g2o::SE3Quat(r_cw1,t_cw1)); 8 | 9 | #Don't use Eigen3.1.0 10 | #Compile error 11 | #static assertion failed: YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR 12 | #will show up 13 | cd g2o 14 | mkdir build 15 | cd build 16 | cmake .. -DCMAKE_BUILD_TYPE=Relase 17 | make -j 18 | 19 | cd ../../ 20 | 21 | echo "Configuring and building main_program ..." 22 | 23 | mkdir build 24 | cd build 25 | cmake .. -DCMAKE_BUILD_TYPE=Relase 26 | make -j 27 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /config_eth_cvg.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | image0_id: '0030' 3 | image1_id: '0035' 4 | image0_type: rgb 5 | image1_type: rgb 6 | 7 | use_groundtruth: '1' 8 | 9 | dataset: eth_cvg 10 | 11 | im_address: /home/zhaozhong/dataset/ethz_cvg/ethl_syn1_global/ethl1_global/ 12 | 13 | depth_factor: 5000.0 14 | 15 | fx: 481.20 16 | fy: -480.0 17 | cx: 319.50 18 | cy: 239.50 19 | 20 | #make sure you have GPU and CUDA. 1: use GPU, 0: use CPU 21 | use_gpu: 1 22 | -------------------------------------------------------------------------------- /g2o/README.txt: -------------------------------------------------------------------------------- 1 | You should have received this g2o version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2). 2 | See the original g2o library at: https://github.com/RainerKuemmerle/g2o 3 | All files included in this g2o version are BSD, see license-bsd.txt 4 | -------------------------------------------------------------------------------- /g2o/cmake_modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | # specific additional paths for some OS 65 | if (WIN32) 66 | set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include") 67 | endif(WIN32) 68 | 69 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 70 | PATHS 71 | ${CMAKE_INSTALL_PREFIX}/include 72 | ${EIGEN_ADDITIONAL_SEARCH_PATHS} 73 | ${KDE4_INCLUDE_DIR} 74 | PATH_SUFFIXES eigen3 eigen 75 | ) 76 | 77 | if(EIGEN3_INCLUDE_DIR) 78 | _eigen3_check_version() 79 | endif(EIGEN3_INCLUDE_DIR) 80 | 81 | include(FindPackageHandleStandardArgs) 82 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 83 | 84 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 85 | 86 | endif(EIGEN3_INCLUDE_DIR) 87 | 88 | -------------------------------------------------------------------------------- /g2o/config.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_CONFIG_H 2 | #define G2O_CONFIG_H 3 | 4 | /* #undef G2O_OPENMP */ 5 | /* #undef G2O_SHARED_LIBS */ 6 | 7 | // give a warning if Eigen defaults to row-major matrices. 8 | // We internally assume column-major matrices throughout the code. 9 | #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR 10 | # error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)" 11 | #endif 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /g2o/config.h.in: -------------------------------------------------------------------------------- 1 | #ifndef G2O_CONFIG_H 2 | #define G2O_CONFIG_H 3 | 4 | #cmakedefine G2O_OPENMP 1 5 | #cmakedefine G2O_SHARED_LIBS 1 6 | 7 | // give a warning if Eigen defaults to row-major matrices. 8 | // We internally assume column-major matrices throughout the code. 9 | #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR 10 | # error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)" 11 | #endif 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /g2o/g2o/core/base_binary_edge.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_BASE_BINARY_EDGE_H 28 | #define G2O_BASE_BINARY_EDGE_H 29 | 30 | #include 31 | #include 32 | 33 | #include "base_edge.h" 34 | #include "robust_kernel.h" 35 | #include "../../config.h" 36 | 37 | namespace g2o { 38 | 39 | using namespace Eigen; 40 | 41 | template 42 | class BaseBinaryEdge : public BaseEdge 43 | { 44 | public: 45 | 46 | typedef VertexXi VertexXiType; 47 | typedef VertexXj VertexXjType; 48 | 49 | static const int Di = VertexXiType::Dimension; 50 | static const int Dj = VertexXjType::Dimension; 51 | 52 | static const int Dimension = BaseEdge::Dimension; 53 | typedef typename BaseEdge::Measurement Measurement; 54 | typedef typename Matrix::AlignedMapType JacobianXiOplusType; 55 | typedef typename Matrix::AlignedMapType JacobianXjOplusType; 56 | typedef typename BaseEdge::ErrorVector ErrorVector; 57 | typedef typename BaseEdge::InformationType InformationType; 58 | 59 | typedef Eigen::Map, Matrix::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType; 60 | typedef Eigen::Map, Matrix::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockTransposedType; 61 | 62 | BaseBinaryEdge() : BaseEdge(), 63 | _hessianRowMajor(false), 64 | _hessian(0, VertexXiType::Dimension, VertexXjType::Dimension), // HACK we map to the null pointer for initializing the Maps 65 | _hessianTransposed(0, VertexXjType::Dimension, VertexXiType::Dimension), 66 | _jacobianOplusXi(0, D, Di), _jacobianOplusXj(0, D, Dj) 67 | { 68 | _vertices.resize(2); 69 | } 70 | 71 | virtual OptimizableGraph::Vertex* createFrom(); 72 | virtual OptimizableGraph::Vertex* createTo(); 73 | 74 | virtual void resize(size_t size); 75 | 76 | virtual bool allVerticesFixed() const; 77 | 78 | virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace); 79 | 80 | /** 81 | * Linearizes the oplus operator in the vertex, and stores 82 | * the result in temporary variables _jacobianOplusXi and _jacobianOplusXj 83 | */ 84 | virtual void linearizeOplus(); 85 | 86 | //! returns the result of the linearization in the manifold space for the node xi 87 | const JacobianXiOplusType& jacobianOplusXi() const { return _jacobianOplusXi;} 88 | //! returns the result of the linearization in the manifold space for the node xj 89 | const JacobianXjOplusType& jacobianOplusXj() const { return _jacobianOplusXj;} 90 | 91 | virtual void constructQuadraticForm() ; 92 | 93 | virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor); 94 | 95 | using BaseEdge::resize; 96 | using BaseEdge::computeError; 97 | 98 | protected: 99 | using BaseEdge::_measurement; 100 | using BaseEdge::_information; 101 | using BaseEdge::_error; 102 | using BaseEdge::_vertices; 103 | using BaseEdge::_dimension; 104 | 105 | bool _hessianRowMajor; 106 | HessianBlockType _hessian; 107 | HessianBlockTransposedType _hessianTransposed; 108 | JacobianXiOplusType _jacobianOplusXi; 109 | JacobianXjOplusType _jacobianOplusXj; 110 | 111 | public: 112 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 113 | }; 114 | 115 | #include "base_binary_edge.hpp" 116 | 117 | } // end namespace g2o 118 | 119 | #endif 120 | -------------------------------------------------------------------------------- /g2o/g2o/core/base_edge.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_BASE_EDGE_H 28 | #define G2O_BASE_EDGE_H 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | #include "optimizable_graph.h" 36 | 37 | namespace g2o { 38 | 39 | using namespace Eigen; 40 | 41 | template 42 | class BaseEdge : public OptimizableGraph::Edge 43 | { 44 | public: 45 | 46 | static const int Dimension = D; 47 | typedef E Measurement; 48 | typedef Matrix ErrorVector; 49 | typedef Matrix InformationType; 50 | 51 | BaseEdge() : OptimizableGraph::Edge() 52 | { 53 | _dimension = D; 54 | } 55 | 56 | virtual ~BaseEdge() {} 57 | 58 | virtual double chi2() const 59 | { 60 | return _error.dot(information()*_error); 61 | } 62 | 63 | virtual const double* errorData() const { return _error.data();} 64 | virtual double* errorData() { return _error.data();} 65 | const ErrorVector& error() const { return _error;} 66 | ErrorVector& error() { return _error;} 67 | 68 | //! information matrix of the constraint 69 | const InformationType& information() const { return _information;} 70 | InformationType& information() { return _information;} 71 | void setInformation(const InformationType& information) { _information = information;} 72 | 73 | virtual const double* informationData() const { return _information.data();} 74 | virtual double* informationData() { return _information.data();} 75 | 76 | //! accessor functions for the measurement represented by the edge 77 | const Measurement& measurement() const { return _measurement;} 78 | virtual void setMeasurement(const Measurement& m) { _measurement = m;} 79 | 80 | virtual int rank() const {return _dimension;} 81 | 82 | virtual void initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*) 83 | { 84 | std::cerr << "inititialEstimate() is not implemented, please give implementation in your derived class" << std::endl; 85 | } 86 | 87 | protected: 88 | 89 | Measurement _measurement; 90 | InformationType _information; 91 | ErrorVector _error; 92 | 93 | /** 94 | * calculate the robust information matrix by updating the information matrix of the error 95 | */ 96 | InformationType robustInformation(const Eigen::Vector3d& rho) 97 | { 98 | InformationType result = rho[1] * _information; 99 | //ErrorVector weightedErrror = _information * _error; 100 | //result.noalias() += 2 * rho[2] * (weightedErrror * weightedErrror.transpose()); 101 | return result; 102 | } 103 | 104 | public: 105 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 106 | }; 107 | 108 | } // end namespace g2o 109 | 110 | #endif 111 | -------------------------------------------------------------------------------- /g2o/g2o/core/base_multi_edge.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_BASE_MULTI_EDGE_H 28 | #define G2O_BASE_MULTI_EDGE_H 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | #include "base_edge.h" 37 | #include "robust_kernel.h" 38 | #include "../../config.h" 39 | 40 | namespace g2o { 41 | 42 | using namespace Eigen; 43 | 44 | /** 45 | * \brief base class to represent an edge connecting an arbitrary number of nodes 46 | * 47 | * D - Dimension of the measurement 48 | * E - type to represent the measurement 49 | */ 50 | template 51 | class BaseMultiEdge : public BaseEdge 52 | { 53 | public: 54 | /** 55 | * \brief helper for mapping the Hessian memory of the upper triangular block 56 | */ 57 | struct HessianHelper { 58 | Eigen::Map matrix; ///< the mapped memory 59 | bool transposed; ///< the block has to be transposed 60 | HessianHelper() : matrix(0, 0, 0), transposed(false) {} 61 | }; 62 | 63 | public: 64 | static const int Dimension = BaseEdge::Dimension; 65 | typedef typename BaseEdge::Measurement Measurement; 66 | typedef MatrixXd::MapType JacobianType; 67 | typedef typename BaseEdge::ErrorVector ErrorVector; 68 | typedef typename BaseEdge::InformationType InformationType; 69 | typedef Eigen::Map HessianBlockType; 70 | 71 | BaseMultiEdge() : BaseEdge() 72 | { 73 | } 74 | 75 | virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace); 76 | 77 | /** 78 | * Linearizes the oplus operator in the vertex, and stores 79 | * the result in temporary variable vector _jacobianOplus 80 | */ 81 | virtual void linearizeOplus(); 82 | 83 | virtual void resize(size_t size); 84 | 85 | virtual bool allVerticesFixed() const; 86 | 87 | virtual void constructQuadraticForm() ; 88 | 89 | virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor); 90 | 91 | using BaseEdge::computeError; 92 | 93 | protected: 94 | using BaseEdge::_measurement; 95 | using BaseEdge::_information; 96 | using BaseEdge::_error; 97 | using BaseEdge::_vertices; 98 | using BaseEdge::_dimension; 99 | 100 | std::vector _hessian; 101 | std::vector > _jacobianOplus; ///< jacobians of the edge (w.r.t. oplus) 102 | 103 | void computeQuadraticForm(const InformationType& omega, const ErrorVector& weightedError); 104 | 105 | public: 106 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 107 | }; 108 | 109 | #include "base_multi_edge.hpp" 110 | 111 | } // end namespace g2o 112 | 113 | #endif 114 | -------------------------------------------------------------------------------- /g2o/g2o/core/base_unary_edge.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_BASE_UNARY_EDGE_H 28 | #define G2O_BASE_UNARY_EDGE_H 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "base_edge.h" 35 | #include "robust_kernel.h" 36 | #include "../../config.h" 37 | 38 | namespace g2o { 39 | 40 | using namespace Eigen; 41 | 42 | template 43 | class BaseUnaryEdge : public BaseEdge 44 | { 45 | public: 46 | static const int Dimension = BaseEdge::Dimension; 47 | typedef typename BaseEdge::Measurement Measurement; 48 | typedef VertexXi VertexXiType; 49 | typedef typename Matrix::AlignedMapType JacobianXiOplusType; 50 | typedef typename BaseEdge::ErrorVector ErrorVector; 51 | typedef typename BaseEdge::InformationType InformationType; 52 | 53 | BaseUnaryEdge() : BaseEdge(), 54 | _jacobianOplusXi(0, D, VertexXiType::Dimension) 55 | { 56 | _vertices.resize(1); 57 | } 58 | 59 | virtual void resize(size_t size); 60 | 61 | virtual bool allVerticesFixed() const; 62 | 63 | virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace); 64 | 65 | /** 66 | * Linearizes the oplus operator in the vertex, and stores 67 | * the result in temporary variables _jacobianOplusXi and _jacobianOplusXj 68 | */ 69 | virtual void linearizeOplus(); 70 | 71 | //! returns the result of the linearization in the manifold space for the node xi 72 | const JacobianXiOplusType& jacobianOplusXi() const { return _jacobianOplusXi;} 73 | 74 | virtual void constructQuadraticForm(); 75 | 76 | virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to); 77 | 78 | virtual void mapHessianMemory(double*, int, int, bool) {assert(0 && "BaseUnaryEdge does not map memory of the Hessian");} 79 | 80 | using BaseEdge::resize; 81 | using BaseEdge::computeError; 82 | 83 | protected: 84 | using BaseEdge::_measurement; 85 | using BaseEdge::_information; 86 | using BaseEdge::_error; 87 | using BaseEdge::_vertices; 88 | using BaseEdge::_dimension; 89 | 90 | JacobianXiOplusType _jacobianOplusXi; 91 | 92 | public: 93 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 94 | }; 95 | 96 | #include "base_unary_edge.hpp" 97 | 98 | } // end namespace g2o 99 | 100 | #endif 101 | -------------------------------------------------------------------------------- /g2o/g2o/core/base_unary_edge.hpp: -------------------------------------------------------------------------------- 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 | template 28 | void BaseUnaryEdge::resize(size_t size) 29 | { 30 | if (size != 1) { 31 | std::cerr << "WARNING, attempting to resize unary edge " << BaseEdge::id() << " to " << size << std::endl; 32 | } 33 | BaseEdge::resize(size); 34 | } 35 | 36 | template 37 | bool BaseUnaryEdge::allVerticesFixed() const 38 | { 39 | return static_cast (_vertices[0])->fixed(); 40 | } 41 | 42 | template 43 | void BaseUnaryEdge::constructQuadraticForm() 44 | { 45 | VertexXiType* from=static_cast(_vertices[0]); 46 | 47 | // chain rule to get the Jacobian of the nodes in the manifold domain 48 | const JacobianXiOplusType& A = jacobianOplusXi(); 49 | const InformationType& omega = _information; 50 | 51 | bool istatus = !from->fixed(); 52 | if (istatus) { 53 | #ifdef G2O_OPENMP 54 | from->lockQuadraticForm(); 55 | #endif 56 | if (this->robustKernel()) { 57 | double error = this->chi2(); 58 | Eigen::Vector3d rho; 59 | this->robustKernel()->robustify(error, rho); 60 | InformationType weightedOmega = this->robustInformation(rho); 61 | 62 | from->b().noalias() -= rho[1] * A.transpose() * omega * _error; 63 | from->A().noalias() += A.transpose() * weightedOmega * A; 64 | } else { 65 | from->b().noalias() -= A.transpose() * omega * _error; 66 | from->A().noalias() += A.transpose() * omega * A; 67 | } 68 | #ifdef G2O_OPENMP 69 | from->unlockQuadraticForm(); 70 | #endif 71 | } 72 | } 73 | 74 | template 75 | void BaseUnaryEdge::linearizeOplus(JacobianWorkspace& jacobianWorkspace) 76 | { 77 | new (&_jacobianOplusXi) JacobianXiOplusType(jacobianWorkspace.workspaceForVertex(0), D, VertexXiType::Dimension); 78 | linearizeOplus(); 79 | } 80 | 81 | template 82 | void BaseUnaryEdge::linearizeOplus() 83 | { 84 | //Xi - estimate the jacobian numerically 85 | VertexXiType* vi = static_cast(_vertices[0]); 86 | 87 | if (vi->fixed()) 88 | return; 89 | 90 | #ifdef G2O_OPENMP 91 | vi->lockQuadraticForm(); 92 | #endif 93 | 94 | const double delta = 1e-9; 95 | const double scalar = 1.0 / (2*delta); 96 | ErrorVector error1; 97 | ErrorVector errorBeforeNumeric = _error; 98 | 99 | double add_vi[VertexXiType::Dimension]; 100 | std::fill(add_vi, add_vi + VertexXiType::Dimension, 0.0); 101 | // add small step along the unit vector in each dimension 102 | for (int d = 0; d < VertexXiType::Dimension; ++d) { 103 | vi->push(); 104 | add_vi[d] = delta; 105 | vi->oplus(add_vi); 106 | computeError(); 107 | error1 = _error; 108 | vi->pop(); 109 | vi->push(); 110 | add_vi[d] = -delta; 111 | vi->oplus(add_vi); 112 | computeError(); 113 | vi->pop(); 114 | add_vi[d] = 0.0; 115 | 116 | _jacobianOplusXi.col(d) = scalar * (error1 - _error); 117 | } // end dimension 118 | 119 | _error = errorBeforeNumeric; 120 | #ifdef G2O_OPENMP 121 | vi->unlockQuadraticForm(); 122 | #endif 123 | } 124 | 125 | template 126 | void BaseUnaryEdge::initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*) 127 | { 128 | std::cerr << __PRETTY_FUNCTION__ << " is not implemented, please give implementation in your derived class" << std::endl; 129 | } 130 | -------------------------------------------------------------------------------- /g2o/g2o/core/base_vertex.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_BASE_VERTEX_H 28 | #define G2O_BASE_VERTEX_H 29 | 30 | #include "optimizable_graph.h" 31 | #include "creators.h" 32 | #include "../stuff/macros.h" 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | namespace g2o { 41 | 42 | using namespace Eigen; 43 | 44 | 45 | /** 46 | * \brief Templatized BaseVertex 47 | * 48 | * Templatized BaseVertex 49 | * D : minimal dimension of the vertex, e.g., 3 for rotation in 3D 50 | * T : internal type to represent the estimate, e.g., Quaternion for rotation in 3D 51 | */ 52 | template 53 | class BaseVertex : public OptimizableGraph::Vertex { 54 | public: 55 | typedef T EstimateType; 56 | typedef std::stack > > 58 | BackupStackType; 59 | 60 | static const int Dimension = D; ///< dimension of the estimate (minimal) in the manifold space 61 | 62 | typedef Eigen::Map, Matrix::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType; 63 | 64 | public: 65 | BaseVertex(); 66 | 67 | virtual const double& hessian(int i, int j) const { assert(i(_hessian.data());} 71 | 72 | virtual void mapHessianMemory(double* d); 73 | 74 | virtual int copyB(double* b_) const { 75 | memcpy(b_, _b.data(), Dimension * sizeof(double)); 76 | return Dimension; 77 | } 78 | 79 | virtual const double& b(int i) const { assert(i < D); return _b(i);} 80 | virtual double& b(int i) { assert(i < D); return _b(i);} 81 | virtual double* bData() { return _b.data();} 82 | 83 | virtual void clearQuadraticForm(); 84 | 85 | //! updates the current vertex with the direct solution x += H_ii\b_ii 86 | //! @returns the determinant of the inverted hessian 87 | virtual double solveDirect(double lambda=0); 88 | 89 | //! return right hand side b of the constructed linear system 90 | Matrix& b() { return _b;} 91 | const Matrix& b() const { return _b;} 92 | //! return the hessian block associated with the vertex 93 | HessianBlockType& A() { return _hessian;} 94 | const HessianBlockType& A() const { return _hessian;} 95 | 96 | virtual void push() { _backup.push(_estimate);} 97 | virtual void pop() { assert(!_backup.empty()); _estimate = _backup.top(); _backup.pop(); updateCache();} 98 | virtual void discardTop() { assert(!_backup.empty()); _backup.pop();} 99 | virtual int stackSize() const {return _backup.size();} 100 | 101 | //! return the current estimate of the vertex 102 | const EstimateType& estimate() const { return _estimate;} 103 | //! set the estimate for the vertex also calls updateCache() 104 | void setEstimate(const EstimateType& et) { _estimate = et; updateCache();} 105 | 106 | protected: 107 | HessianBlockType _hessian; 108 | Matrix _b; 109 | EstimateType _estimate; 110 | BackupStackType _backup; 111 | public: 112 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 113 | }; 114 | 115 | #include "base_vertex.hpp" 116 | 117 | } // end namespace g2o 118 | 119 | 120 | #endif 121 | -------------------------------------------------------------------------------- /g2o/g2o/core/base_vertex.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | template 28 | BaseVertex::BaseVertex() : 29 | OptimizableGraph::Vertex(), 30 | _hessian(0, D, D) 31 | { 32 | _dimension = D; 33 | } 34 | 35 | template 36 | double BaseVertex::solveDirect(double lambda) { 37 | Matrix tempA=_hessian + Matrix ::Identity()*lambda; 38 | double det=tempA.determinant(); 39 | if (g2o_isnan(det) || det < std::numeric_limits::epsilon()) 40 | return det; 41 | Matrix dx=tempA.llt().solve(_b); 42 | oplus(&dx[0]); 43 | return det; 44 | } 45 | 46 | template 47 | void BaseVertex::clearQuadraticForm() { 48 | _b.setZero(); 49 | } 50 | 51 | template 52 | void BaseVertex::mapHessianMemory(double* d) 53 | { 54 | new (&_hessian) HessianBlockType(d, D, D); 55 | } 56 | -------------------------------------------------------------------------------- /g2o/g2o/core/batch_stats.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "batch_stats.h" 28 | #include 29 | 30 | namespace g2o { 31 | using namespace std; 32 | 33 | G2OBatchStatistics* G2OBatchStatistics::_globalStats=0; 34 | 35 | #ifndef PTHING 36 | #define PTHING(s) \ 37 | #s << "= " << (st.s) << "\t " 38 | #endif 39 | 40 | G2OBatchStatistics::G2OBatchStatistics(){ 41 | // zero all. 42 | memset (this, 0, sizeof(G2OBatchStatistics)); 43 | 44 | // set the iteration to -1 to show that it isn't valid 45 | iteration = -1; 46 | } 47 | 48 | std::ostream& operator << (std::ostream& os , const G2OBatchStatistics& st) 49 | { 50 | os << PTHING(iteration); 51 | 52 | os << PTHING( numVertices ); // how many vertices are involved 53 | os << PTHING( numEdges ); // hoe many edges 54 | os << PTHING( chi2 ); // total chi2 55 | 56 | /** timings **/ 57 | // nonlinear part 58 | os << PTHING( timeResiduals ); 59 | os << PTHING( timeLinearize ); // jacobians 60 | os << PTHING( timeQuadraticForm ); // construct the quadratic form in the graph 61 | 62 | // block_solver (constructs Ax=b, plus maybe schur); 63 | os << PTHING( timeSchurComplement ); // compute schur complement (0 if not done); 64 | 65 | // linear solver (computes Ax=b); ); 66 | os << PTHING( timeSymbolicDecomposition ); // symbolic decomposition (0 if not done); 67 | os << PTHING( timeNumericDecomposition ); // numeric decomposition (0 if not done); 68 | os << PTHING( timeLinearSolution ); // total time for solving Ax=b 69 | os << PTHING( iterationsLinearSolver ); // iterations of PCG 70 | os << PTHING( timeUpdate ); // oplus 71 | os << PTHING( timeIteration ); // total time ); 72 | 73 | os << PTHING( levenbergIterations ); 74 | os << PTHING( timeLinearSolver); 75 | 76 | os << PTHING(hessianDimension); 77 | os << PTHING(hessianPoseDimension); 78 | os << PTHING(hessianLandmarkDimension); 79 | os << PTHING(choleskyNNZ); 80 | os << PTHING(timeMarginals); 81 | 82 | return os; 83 | }; 84 | 85 | void G2OBatchStatistics::setGlobalStats(G2OBatchStatistics* b) 86 | { 87 | _globalStats = b; 88 | } 89 | 90 | } // end namespace 91 | -------------------------------------------------------------------------------- /g2o/g2o/core/batch_stats.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_BATCH_STATS_H_ 28 | #define G2O_BATCH_STATS_H_ 29 | 30 | #include 31 | #include 32 | 33 | 34 | namespace g2o { 35 | 36 | /** 37 | * \brief statistics about the optimization 38 | */ 39 | struct G2OBatchStatistics { 40 | G2OBatchStatistics(); 41 | int iteration; ///< which iteration 42 | int numVertices; ///< how many vertices are involved 43 | int numEdges; ///< how many edges 44 | double chi2; ///< total chi2 45 | 46 | /** timings **/ 47 | // nonlinear part 48 | double timeResiduals; ///< residuals 49 | double timeLinearize; ///< jacobians 50 | double timeQuadraticForm; ///< construct the quadratic form in the graph 51 | int levenbergIterations; ///< number of iterations performed by LM 52 | // block_solver (constructs Ax=b, plus maybe schur) 53 | double timeSchurComplement; ///< compute schur complement (0 if not done) 54 | 55 | // linear solver (computes Ax=b); 56 | double timeSymbolicDecomposition; ///< symbolic decomposition (0 if not done) 57 | double timeNumericDecomposition; ///< numeric decomposition (0 if not done) 58 | double timeLinearSolution; ///< total time for solving Ax=b (including detup for schur) 59 | double timeLinearSolver; ///< time for solving, excluding Schur setup 60 | int iterationsLinearSolver; ///< iterations of PCG, (0 if not used, i.e., Cholesky) 61 | double timeUpdate; ///< time to apply the update 62 | double timeIteration; ///< total time; 63 | 64 | double timeMarginals; ///< computing the inverse elements (solve blocks) and thus the marginal covariances 65 | 66 | // information about the Hessian matrix 67 | size_t hessianDimension; ///< rows / cols of the Hessian 68 | size_t hessianPoseDimension; ///< dimension of the pose matrix in Schur 69 | size_t hessianLandmarkDimension; ///< dimension of the landmark matrix in Schur 70 | size_t choleskyNNZ; ///< number of non-zeros in the cholesky factor 71 | 72 | static G2OBatchStatistics* globalStats() {return _globalStats;} 73 | static void setGlobalStats(G2OBatchStatistics* b); 74 | protected: 75 | static G2OBatchStatistics* _globalStats; 76 | }; 77 | 78 | std::ostream& operator<<(std::ostream&, const G2OBatchStatistics&); 79 | 80 | typedef std::vector BatchStatisticsContainer; 81 | } 82 | 83 | #endif 84 | -------------------------------------------------------------------------------- /g2o/g2o/core/computeH.cuh: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "cuda_runtime.h" 3 | #include "device_launch_parameters.h" 4 | #include 5 | 6 | namespace g2o{ 7 | 8 | void CudaComputeH(bool calculate_der, double* im0, double*im1, double* points3d, int* bs_counter, double* bs_ref, int* bs_index_ref, double* pose, double* camera_intrincis, int bin_num, int bs_degree, int cell_num, int rows, int cols, double* Href, double* pro_target, double* pro_joint, double* Htarget, double* Hjoint, double* der); 9 | 10 | } -------------------------------------------------------------------------------- /g2o/g2o/core/creators.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_CREATORS_H 28 | #define G2O_CREATORS_H 29 | 30 | #include "hyper_graph.h" 31 | 32 | #include 33 | #include 34 | 35 | namespace g2o 36 | { 37 | 38 | /** 39 | * \brief Abstract interface for allocating HyperGraphElement 40 | */ 41 | class AbstractHyperGraphElementCreator 42 | { 43 | public: 44 | /** 45 | * create a hyper graph element. Has to implemented in derived class. 46 | */ 47 | virtual HyperGraph::HyperGraphElement* construct() = 0; 48 | /** 49 | * name of the class to be created. Has to implemented in derived class. 50 | */ 51 | virtual const std::string& name() const = 0; 52 | 53 | virtual ~AbstractHyperGraphElementCreator() { } 54 | }; 55 | 56 | /** 57 | * \brief templatized creator class which creates graph elements 58 | */ 59 | template 60 | class HyperGraphElementCreator : public AbstractHyperGraphElementCreator 61 | { 62 | public: 63 | HyperGraphElementCreator() : _name(typeid(T).name()) {} 64 | #if defined (WINDOWS) && defined(__GNUC__) // force stack alignment on Windows with GCC 65 | __attribute__((force_align_arg_pointer)) 66 | #endif 67 | HyperGraph::HyperGraphElement* construct() { return new T;} 68 | virtual const std::string& name() const { return _name;} 69 | protected: 70 | std::string _name; 71 | }; 72 | 73 | } // end namespace 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /g2o/g2o/core/eigen_types.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_EIGEN_TYPES_H 28 | #define G2O_EIGEN_TYPES_H 29 | 30 | #include 31 | #include 32 | 33 | namespace g2o { 34 | 35 | typedef Eigen::Matrix Vector2I; 36 | typedef Eigen::Matrix Vector3I; 37 | typedef Eigen::Matrix Vector4I; 38 | typedef Eigen::Matrix VectorXI; 39 | 40 | typedef Eigen::Matrix Vector2F; 41 | typedef Eigen::Matrix Vector3F; 42 | typedef Eigen::Matrix Vector4F; 43 | typedef Eigen::Matrix VectorXF; 44 | 45 | typedef Eigen::Matrix Vector2D; 46 | typedef Eigen::Matrix Vector3D; 47 | typedef Eigen::Matrix Vector4D; 48 | typedef Eigen::Matrix VectorXD; 49 | 50 | typedef Eigen::Matrix Matrix2I; 51 | typedef Eigen::Matrix Matrix3I; 52 | typedef Eigen::Matrix Matrix4I; 53 | typedef Eigen::Matrix MatrixXI; 54 | 55 | typedef Eigen::Matrix Matrix2F; 56 | typedef Eigen::Matrix Matrix3F; 57 | typedef Eigen::Matrix Matrix4F; 58 | typedef Eigen::Matrix MatrixXF; 59 | 60 | typedef Eigen::Matrix Matrix2D; 61 | typedef Eigen::Matrix Matrix3D; 62 | typedef Eigen::Matrix Matrix4D; 63 | typedef Eigen::Matrix MatrixXD; 64 | 65 | typedef Eigen::Transform Isometry2D; 66 | typedef Eigen::Transform Isometry3D; 67 | 68 | typedef Eigen::Transform Affine2D; 69 | typedef Eigen::Transform Affine3D; 70 | 71 | } // end namespace g2o 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /g2o/g2o/core/hyper_dijkstra.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_AIS_GENERAL_DIJKSTRA_HH 28 | #define G2O_AIS_GENERAL_DIJKSTRA_HH 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "hyper_graph.h" 35 | 36 | namespace g2o{ 37 | 38 | struct HyperDijkstra{ 39 | struct CostFunction { 40 | virtual double operator() (HyperGraph::Edge* e, HyperGraph::Vertex* from, HyperGraph::Vertex* to)=0; 41 | virtual ~CostFunction() { } 42 | }; 43 | 44 | struct TreeAction { 45 | virtual double perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e); 46 | virtual double perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e, double distance); 47 | }; 48 | 49 | 50 | struct AdjacencyMapEntry{ 51 | friend struct HyperDijkstra; 52 | AdjacencyMapEntry(HyperGraph::Vertex* _child=0, 53 | HyperGraph::Vertex* _parent=0, 54 | HyperGraph::Edge* _edge=0, 55 | double _distance=std::numeric_limits::max()); 56 | HyperGraph::Vertex* child() const {return _child;} 57 | HyperGraph::Vertex* parent() const {return _parent;} 58 | HyperGraph::Edge* edge() const {return _edge;} 59 | double distance() const {return _distance;} 60 | HyperGraph::VertexSet& children() {return _children;} 61 | const HyperGraph::VertexSet& children() const {return _children;} 62 | protected: 63 | HyperGraph::Vertex* _child; 64 | HyperGraph::Vertex* _parent; 65 | HyperGraph::Edge* _edge; 66 | double _distance; 67 | HyperGraph::VertexSet _children; 68 | }; 69 | 70 | typedef std::map AdjacencyMap; 71 | HyperDijkstra(HyperGraph* g); 72 | HyperGraph::VertexSet& visited() {return _visited; } 73 | AdjacencyMap& adjacencyMap() {return _adjacencyMap; } 74 | HyperGraph* graph() {return _graph;} 75 | 76 | void shortestPaths(HyperGraph::Vertex* v, 77 | HyperDijkstra::CostFunction* cost, 78 | double maxDistance=std::numeric_limits< double >::max(), 79 | double comparisonConditioner=1e-3, 80 | bool directed=false, 81 | double maxEdgeCost=std::numeric_limits< double >::max()); 82 | 83 | void shortestPaths(HyperGraph::VertexSet& vset, 84 | HyperDijkstra::CostFunction* cost, 85 | double maxDistance=std::numeric_limits< double >::max(), 86 | double comparisonConditioner=1e-3, 87 | bool directed=false, 88 | double maxEdgeCost=std::numeric_limits< double >::max()); 89 | 90 | 91 | static void computeTree(AdjacencyMap& amap); 92 | static void visitAdjacencyMap(AdjacencyMap& amap, TreeAction* action, bool useDistance=false); 93 | static void connectedSubset(HyperGraph::VertexSet& connected, HyperGraph::VertexSet& visited, 94 | HyperGraph::VertexSet& startingSet, 95 | HyperGraph* g, HyperGraph::Vertex* v, 96 | HyperDijkstra::CostFunction* cost, double distance, double comparisonConditioner, 97 | double maxEdgeCost=std::numeric_limits< double >::max() ); 98 | 99 | protected: 100 | void reset(); 101 | 102 | AdjacencyMap _adjacencyMap; 103 | HyperGraph::VertexSet _visited; 104 | HyperGraph* _graph; 105 | }; 106 | 107 | struct UniformCostFunction: public HyperDijkstra::CostFunction { 108 | virtual double operator ()(HyperGraph::Edge* edge, HyperGraph::Vertex* from, HyperGraph::Vertex* to); 109 | }; 110 | 111 | } 112 | #endif 113 | -------------------------------------------------------------------------------- /g2o/g2o/core/hyper_graph.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 "hyper_graph.h" 28 | 29 | #include 30 | #include 31 | 32 | namespace g2o { 33 | 34 | HyperGraph::Vertex::Vertex(int id) : _id(id) 35 | { 36 | } 37 | 38 | HyperGraph::Vertex::~Vertex() 39 | { 40 | } 41 | 42 | HyperGraph::Edge::Edge(int id) : _id(id) 43 | { 44 | } 45 | 46 | HyperGraph::Edge::~Edge() 47 | { 48 | } 49 | 50 | void HyperGraph::Edge::resize(size_t size) 51 | { 52 | _vertices.resize(size, 0); 53 | } 54 | 55 | void HyperGraph::Edge::setId(int id) 56 | { 57 | _id = id; 58 | } 59 | 60 | HyperGraph::Vertex* HyperGraph::vertex(int id) 61 | { 62 | VertexIDMap::iterator it=_vertices.find(id); 63 | if (it==_vertices.end()) 64 | return 0; 65 | return it->second; 66 | } 67 | 68 | const HyperGraph::Vertex* HyperGraph::vertex(int id) const 69 | { 70 | VertexIDMap::const_iterator it=_vertices.find(id); 71 | if (it==_vertices.end()) 72 | return 0; 73 | return it->second; 74 | } 75 | 76 | bool HyperGraph::addVertex(Vertex* v) 77 | { 78 | Vertex* vn=vertex(v->id()); 79 | if (vn) 80 | return false; 81 | _vertices.insert( std::make_pair(v->id(),v) ); 82 | return true; 83 | } 84 | 85 | /** 86 | * changes the id of a vertex already in the graph, and updates the bookkeeping 87 | @ returns false if the vertex is not in the graph; 88 | */ 89 | bool HyperGraph::changeId(Vertex* v, int newId){ 90 | Vertex* v2 = vertex(v->id()); 91 | if (v != v2) 92 | return false; 93 | _vertices.erase(v->id()); 94 | v->setId(newId); 95 | _vertices.insert(std::make_pair(v->id(), v)); 96 | return true; 97 | } 98 | 99 | bool HyperGraph::addEdge(Edge* e) 100 | { 101 | std::pair result = _edges.insert(e); 102 | if (! result.second) 103 | return false; 104 | for (std::vector::iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) { 105 | Vertex* v = *it; 106 | v->edges().insert(e); 107 | } 108 | return true; 109 | } 110 | 111 | bool HyperGraph::removeVertex(Vertex* v) 112 | { 113 | VertexIDMap::iterator it=_vertices.find(v->id()); 114 | if (it==_vertices.end()) 115 | return false; 116 | assert(it->second==v); 117 | //remove all edges which are entering or leaving v; 118 | EdgeSet tmp(v->edges()); 119 | for (EdgeSet::iterator it=tmp.begin(); it!=tmp.end(); ++it){ 120 | if (!removeEdge(*it)){ 121 | assert(0); 122 | } 123 | } 124 | _vertices.erase(it); 125 | delete v; 126 | return true; 127 | } 128 | 129 | bool HyperGraph::removeEdge(Edge* e) 130 | { 131 | EdgeSet::iterator it = _edges.find(e); 132 | if (it == _edges.end()) 133 | return false; 134 | _edges.erase(it); 135 | 136 | for (std::vector::iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) { 137 | Vertex* v = *vit; 138 | it = v->edges().find(e); 139 | assert(it!=v->edges().end()); 140 | v->edges().erase(it); 141 | } 142 | 143 | delete e; 144 | return true; 145 | } 146 | 147 | HyperGraph::HyperGraph() 148 | { 149 | } 150 | 151 | void HyperGraph::clear() 152 | { 153 | for (VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) 154 | delete (it->second); 155 | for (EdgeSet::iterator it=_edges.begin(); it!=_edges.end(); ++it) 156 | delete (*it); 157 | _vertices.clear(); 158 | _edges.clear(); 159 | } 160 | 161 | HyperGraph::~HyperGraph() 162 | { 163 | clear(); 164 | } 165 | 166 | } // end namespace 167 | -------------------------------------------------------------------------------- /g2o/g2o/core/jacobian_workspace.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "jacobian_workspace.h" 28 | 29 | #include 30 | 31 | #include "optimizable_graph.h" 32 | 33 | using namespace std; 34 | 35 | namespace g2o { 36 | 37 | JacobianWorkspace::JacobianWorkspace() : 38 | _maxNumVertices(-1), _maxDimension(-1) 39 | { 40 | } 41 | 42 | JacobianWorkspace::~JacobianWorkspace() 43 | { 44 | } 45 | 46 | bool JacobianWorkspace::allocate() 47 | { 48 | //cerr << __PRETTY_FUNCTION__ << " " << PVAR(this) << " " << PVAR(_maxNumVertices) << " " << PVAR(_maxDimension) << endl; 49 | if (_maxNumVertices <=0 || _maxDimension <= 0) 50 | return false; 51 | _workspace.resize(_maxNumVertices); 52 | for (WorkspaceVector::iterator it = _workspace.begin(); it != _workspace.end(); ++it) { 53 | it->resize(_maxDimension); 54 | it->setZero(); 55 | } 56 | return true; 57 | } 58 | 59 | void JacobianWorkspace::updateSize(const HyperGraph::Edge* e_) 60 | { 61 | const OptimizableGraph::Edge* e = static_cast(e_); 62 | int errorDimension = e->dimension(); 63 | int numVertices = e->vertices().size(); 64 | int maxDimensionForEdge = -1; 65 | for (int i = 0; i < numVertices; ++i) { 66 | const OptimizableGraph::Vertex* v = static_cast(e->vertex(i)); 67 | assert(v && "Edge has no vertex assigned"); 68 | maxDimensionForEdge = max(v->dimension() * errorDimension, maxDimensionForEdge); 69 | } 70 | _maxNumVertices = max(numVertices, _maxNumVertices); 71 | _maxDimension = max(maxDimensionForEdge, _maxDimension); 72 | //cerr << __PRETTY_FUNCTION__ << " " << PVAR(this) << " " << PVAR(_maxNumVertices) << " " << PVAR(_maxDimension) << endl; 73 | } 74 | 75 | void JacobianWorkspace::updateSize(const OptimizableGraph& graph) 76 | { 77 | for (OptimizableGraph::EdgeSet::const_iterator it = graph.edges().begin(); it != graph.edges().end(); ++it) { 78 | const OptimizableGraph::Edge* e = static_cast(*it); 79 | updateSize(e); 80 | } 81 | } 82 | 83 | void JacobianWorkspace::updateSize(int numVertices, int dimension) 84 | { 85 | _maxNumVertices = max(numVertices, _maxNumVertices); 86 | _maxDimension = max(dimension, _maxDimension); 87 | } 88 | 89 | } // end namespace 90 | -------------------------------------------------------------------------------- /g2o/g2o/core/jacobian_workspace.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef JACOBIAN_WORKSPACE_H 28 | #define JACOBIAN_WORKSPACE_H 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | #include "hyper_graph.h" 37 | 38 | namespace g2o { 39 | 40 | struct OptimizableGraph; 41 | 42 | /** 43 | * \brief provide memory workspace for computing the Jacobians 44 | * 45 | * The workspace is used by an OptimizableGraph to provide temporary memory 46 | * for computing the Jacobian of the error functions. 47 | * Before calling linearizeOplus on an edge, the workspace needs to be allocated 48 | * by calling allocate(). 49 | */ 50 | class JacobianWorkspace 51 | { 52 | public: 53 | typedef std::vector > WorkspaceVector; 54 | 55 | public: 56 | JacobianWorkspace(); 57 | ~JacobianWorkspace(); 58 | 59 | /** 60 | * allocate the workspace 61 | */ 62 | bool allocate(); 63 | 64 | /** 65 | * update the maximum required workspace needed by taking into account this edge 66 | */ 67 | void updateSize(const HyperGraph::Edge* e); 68 | 69 | /** 70 | * update the required workspace by looking at a full graph 71 | */ 72 | void updateSize(const OptimizableGraph& graph); 73 | 74 | /** 75 | * manually update with the given parameters 76 | */ 77 | void updateSize(int numVertices, int dimension); 78 | 79 | /** 80 | * return the workspace for a vertex in an edge 81 | */ 82 | double* workspaceForVertex(int vertexIndex) 83 | { 84 | assert(vertexIndex >= 0 && (size_t)vertexIndex < _workspace.size() && "Index out of bounds"); 85 | return _workspace[vertexIndex].data(); 86 | } 87 | 88 | protected: 89 | WorkspaceVector _workspace; ///< the memory pre-allocated for computing the Jacobians 90 | int _maxNumVertices; ///< the maximum number of vertices connected by a hyper-edge 91 | int _maxDimension; ///< the maximum dimension (number of elements) for a Jacobian 92 | }; 93 | 94 | } // end namespace 95 | 96 | #endif 97 | -------------------------------------------------------------------------------- /g2o/g2o/core/linear_solver.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_LINEAR_SOLVER_H 28 | #define G2O_LINEAR_SOLVER_H 29 | #include "sparse_block_matrix.h" 30 | #include "sparse_block_matrix_ccs.h" 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief basic solver for Ax = b 36 | * 37 | * basic solver for Ax = b which has to reimplemented for different linear algebra libraries. 38 | * A is assumed to be symmetric (only upper triangular block is stored) and positive-semi-definit. 39 | */ 40 | template 41 | class LinearSolver 42 | { 43 | public: 44 | LinearSolver() {}; 45 | virtual ~LinearSolver() {} 46 | 47 | /** 48 | * init for operating on matrices with a different non-zero pattern like before 49 | */ 50 | virtual bool init() = 0; 51 | 52 | /** 53 | * Assumes that A is the same matrix for several calls. 54 | * Among other assumptions, the non-zero pattern does not change! 55 | * If the matrix changes call init() before. 56 | * solve system Ax = b, x and b have to allocated beforehand!! 57 | */ 58 | virtual bool solve(const SparseBlockMatrix& A, double* x, double* b) = 0; 59 | 60 | /** 61 | * Inverts the diagonal blocks of A 62 | * @returns false if not defined. 63 | */ 64 | virtual bool solveBlocks(double**&blocks, const SparseBlockMatrix& A) { (void)blocks; (void) A; return false; } 65 | 66 | 67 | /** 68 | * Inverts the a block pattern of A in spinv 69 | * @returns false if not defined. 70 | */ 71 | virtual bool solvePattern(SparseBlockMatrix& spinv, const std::vector >& blockIndices, const SparseBlockMatrix& A){ 72 | (void) spinv; 73 | (void) blockIndices; 74 | (void) A; 75 | return false; 76 | } 77 | 78 | //! write a debug dump of the system matrix if it is not PSD in solve 79 | virtual bool writeDebug() const { return false;} 80 | virtual void setWriteDebug(bool) {} 81 | }; 82 | 83 | /** 84 | * \brief Solver with faster iterating structure for the linear matrix 85 | */ 86 | template 87 | class LinearSolverCCS : public LinearSolver 88 | { 89 | public: 90 | LinearSolverCCS() : LinearSolver(), _ccsMatrix(0) {} 91 | ~LinearSolverCCS() 92 | { 93 | delete _ccsMatrix; 94 | } 95 | 96 | protected: 97 | SparseBlockMatrixCCS* _ccsMatrix; 98 | 99 | void initMatrixStructure(const SparseBlockMatrix& A) 100 | { 101 | delete _ccsMatrix; 102 | _ccsMatrix = new SparseBlockMatrixCCS(A.rowBlockIndices(), A.colBlockIndices()); 103 | A.fillSparseBlockMatrixCCS(*_ccsMatrix); 104 | } 105 | }; 106 | 107 | } // end namespace 108 | 109 | #endif 110 | -------------------------------------------------------------------------------- /g2o/g2o/core/marginal_covariance_cholesky.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_MARGINAL_COVARIANCE_CHOLESKY_H 28 | #define G2O_MARGINAL_COVARIANCE_CHOLESKY_H 29 | 30 | #include "optimizable_graph.h" 31 | #include "sparse_block_matrix.h" 32 | 33 | #include 34 | #include 35 | 36 | #ifdef _MSC_VER 37 | #include 38 | #else 39 | #include 40 | #endif 41 | 42 | 43 | namespace g2o { 44 | 45 | /** 46 | * \brief computing the marginal covariance given a cholesky factor (lower triangle of the factor) 47 | */ 48 | class MarginalCovarianceCholesky { 49 | protected: 50 | /** 51 | * hash struct for storing the matrix elements needed to compute the covariance 52 | */ 53 | typedef std::tr1::unordered_map LookupMap; 54 | 55 | public: 56 | MarginalCovarianceCholesky(); 57 | ~MarginalCovarianceCholesky(); 58 | 59 | /** 60 | * compute the marginal cov for the given block indices, write the result to the covBlocks memory (which has to 61 | * be provided by the caller). 62 | */ 63 | void computeCovariance(double** covBlocks, const std::vector& blockIndices); 64 | 65 | 66 | /** 67 | * compute the marginal cov for the given block indices, write the result in spinv). 68 | */ 69 | void computeCovariance(SparseBlockMatrix& spinv, const std::vector& rowBlockIndices, const std::vector< std::pair >& blockIndices); 70 | 71 | 72 | /** 73 | * set the CCS representation of the cholesky factor along with the inverse permutation used to reduce the fill-in. 74 | * permInv might be 0, will then not permute the entries. 75 | * 76 | * The pointers provided by the user need to be still valid when calling computeCovariance(). The pointers 77 | * are owned by the caller, MarginalCovarianceCholesky does not free the pointers. 78 | */ 79 | void setCholeskyFactor(int n, int* Lp, int* Li, double* Lx, int* permInv); 80 | 81 | protected: 82 | // information about the cholesky factor (lower triangle) 83 | int _n; ///< L is an n X n matrix 84 | int* _Ap; ///< column pointer of the CCS storage 85 | int* _Ai; ///< row indices of the CCS storage 86 | double* _Ax; ///< values of the cholesky factor 87 | int* _perm; ///< permutation of the cholesky factor. Variable re-ordering for better fill-in 88 | 89 | LookupMap _map; ///< hash look up table for the already computed entries 90 | std::vector _diag; ///< cache 1 / H_ii to avoid recalculations 91 | 92 | //! compute the index used for hashing 93 | int computeIndex(int r, int c) const { /*assert(r <= c);*/ return r*_n + c;} 94 | /** 95 | * compute one entry in the covariance, r and c are values after applying the permutation, and upper triangular. 96 | * May issue recursive calls to itself to compute the missing values. 97 | */ 98 | double computeEntry(int r, int c); 99 | }; 100 | 101 | } 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /g2o/g2o/core/matrix_operations.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_CORE_MATRIX_OPERATIONS_H 28 | #define G2O_CORE_MATRIX_OPERATIONS_H 29 | 30 | #include 31 | 32 | namespace g2o { 33 | namespace internal { 34 | 35 | template 36 | inline void axpy(const MatrixType& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 37 | { 38 | y.segment(yoff) += A * x.segment(xoff); 39 | } 40 | 41 | template 42 | inline void axpy(const Eigen::Matrix& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 43 | { 44 | y.segment(yoff, A.rows()) += A * x.segment::ColsAtCompileTime>(xoff); 45 | } 46 | 47 | template<> 48 | inline void axpy(const Eigen::MatrixXd& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 49 | { 50 | y.segment(yoff, A.rows()) += A * x.segment(xoff, A.cols()); 51 | } 52 | 53 | template 54 | inline void atxpy(const MatrixType& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 55 | { 56 | y.segment(yoff) += A.transpose() * x.segment(xoff); 57 | } 58 | 59 | template 60 | inline void atxpy(const Eigen::Matrix& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 61 | { 62 | y.segment::ColsAtCompileTime>(yoff) += A.transpose() * x.segment(xoff, A.rows()); 63 | } 64 | 65 | template<> 66 | inline void atxpy(const Eigen::MatrixXd& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 67 | { 68 | y.segment(yoff, A.cols()) += A.transpose() * x.segment(xoff, A.rows()); 69 | } 70 | 71 | } // end namespace internal 72 | } // end namespace g2o 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /g2o/g2o/core/matrix_structure.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "matrix_structure.h" 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | using namespace std; 34 | 35 | namespace g2o { 36 | 37 | struct ColSort 38 | { 39 | bool operator()(const pair& e1, const pair& e2) const 40 | { 41 | return e1.second < e2.second || (e1.second == e2.second && e1.first < e2.first); 42 | } 43 | }; 44 | 45 | MatrixStructure::MatrixStructure() : 46 | n(0), m(0), Ap(0), Aii(0), maxN(0), maxNz(0) 47 | { 48 | } 49 | 50 | MatrixStructure::~MatrixStructure() 51 | { 52 | free(); 53 | } 54 | 55 | void MatrixStructure::alloc(int n_, int nz) 56 | { 57 | if (n == 0) { 58 | maxN = n = n_; 59 | maxNz = nz; 60 | Ap = new int[maxN + 1]; 61 | Aii = new int[maxNz]; 62 | } 63 | else { 64 | n = n_; 65 | if (maxNz < nz) { 66 | maxNz = 2 * nz; 67 | delete[] Aii; 68 | Aii = new int[maxNz]; 69 | } 70 | if (maxN < n) { 71 | maxN = 2 * n; 72 | delete[] Ap; 73 | Ap = new int[maxN + 1]; 74 | } 75 | } 76 | } 77 | 78 | void MatrixStructure::free() 79 | { 80 | n = 0; 81 | m = 0; 82 | maxN = 0; 83 | maxNz = 0; 84 | delete[] Aii; Aii = 0; 85 | delete[] Ap; Ap = 0; 86 | } 87 | 88 | bool MatrixStructure::write(const char* filename) const 89 | { 90 | const int& cols = n; 91 | const int& rows = m; 92 | 93 | string name = filename; 94 | std::string::size_type lastDot = name.find_last_of('.'); 95 | if (lastDot != std::string::npos) 96 | name = name.substr(0, lastDot); 97 | 98 | vector > entries; 99 | for (int i=0; i < cols; ++i) { 100 | const int& rbeg = Ap[i]; 101 | const int& rend = Ap[i+1]; 102 | for (int j = rbeg; j < rend; ++j) { 103 | entries.push_back(make_pair(Aii[j], i)); 104 | if (Aii[j] != i) 105 | entries.push_back(make_pair(i, Aii[j])); 106 | } 107 | } 108 | 109 | sort(entries.begin(), entries.end(), ColSort()); 110 | 111 | std::ofstream fout(filename); 112 | fout << "# name: " << name << std::endl; 113 | fout << "# type: sparse matrix" << std::endl; 114 | fout << "# nnz: " << entries.size() << std::endl; 115 | fout << "# rows: " << rows << std::endl; 116 | fout << "# columns: " << cols << std::endl; 117 | for (vector >::const_iterator it = entries.begin(); it != entries.end(); ++it) { 118 | const pair& entry = *it; 119 | fout << entry.first << " " << entry.second << " 0" << std::endl; // write a constant value of 0 120 | } 121 | 122 | return fout.good(); 123 | } 124 | 125 | } // end namespace 126 | -------------------------------------------------------------------------------- /g2o/g2o/core/matrix_structure.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_MATRIX_STRUCTURE_H 28 | #define G2O_MATRIX_STRUCTURE_H 29 | 30 | 31 | namespace g2o { 32 | 33 | /** 34 | * \brief representing the structure of a matrix in column compressed structure (only the upper triangular part of the matrix) 35 | */ 36 | class MatrixStructure 37 | { 38 | public: 39 | MatrixStructure(); 40 | ~MatrixStructure(); 41 | /** 42 | * allocate space for the Matrix Structure. You may call this on an already allocated struct, it will 43 | * then reallocate the memory + additional space (double the required space). 44 | */ 45 | void alloc(int n_, int nz); 46 | 47 | void free(); 48 | 49 | /** 50 | * Write the matrix pattern to a file. File is also loadable by octave, e.g., then use spy(matrix) 51 | */ 52 | bool write(const char* filename) const; 53 | 54 | int n; ///< A is m-by-n. n must be >= 0. 55 | int m; ///< A is m-by-n. m must be >= 0. 56 | int* Ap; ///< column pointers for A, of size n+1 57 | int* Aii; ///< row indices of A, of size nz = Ap [n] 58 | 59 | //! max number of non-zeros blocks 60 | int nzMax() const { return maxNz;} 61 | 62 | protected: 63 | int maxN; ///< size of the allocated memory 64 | int maxNz; ///< size of the allocated memory 65 | }; 66 | 67 | } // end namespace 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /g2o/g2o/core/openmp_mutex.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPENMP_MUTEX 28 | #define G2O_OPENMP_MUTEX 29 | 30 | #include "../../config.h" 31 | 32 | #ifdef G2O_OPENMP 33 | #include 34 | #else 35 | #include 36 | #endif 37 | 38 | namespace g2o { 39 | 40 | #ifdef G2O_OPENMP 41 | 42 | /** 43 | * \brief Mutex realized via OpenMP 44 | */ 45 | class OpenMPMutex 46 | { 47 | public: 48 | OpenMPMutex() { omp_init_lock(&_lock); } 49 | ~OpenMPMutex() { omp_destroy_lock(&_lock); } 50 | void lock() { omp_set_lock(&_lock); } 51 | void unlock() { omp_unset_lock(&_lock); } 52 | protected: 53 | omp_lock_t _lock; 54 | }; 55 | 56 | #else 57 | 58 | /* 59 | * dummy which does nothing in case we don't have OpenMP support. 60 | * In debug mode, the mutex allows to verify the correct lock and unlock behavior 61 | */ 62 | class OpenMPMutex 63 | { 64 | public: 65 | #ifdef NDEBUG 66 | OpenMPMutex() {} 67 | #else 68 | OpenMPMutex() : _cnt(0) {} 69 | #endif 70 | ~OpenMPMutex() { assert(_cnt == 0 && "Freeing locked mutex");} 71 | void lock() { assert(++_cnt == 1 && "Locking already locked mutex");} 72 | void unlock() { assert(--_cnt == 0 && "Trying to unlock a mutex which is not locked");} 73 | protected: 74 | #ifndef NDEBUG 75 | char _cnt; 76 | #endif 77 | }; 78 | 79 | #endif 80 | 81 | /** 82 | * \brief lock a mutex within a scope 83 | */ 84 | class ScopedOpenMPMutex 85 | { 86 | public: 87 | explicit ScopedOpenMPMutex(OpenMPMutex* mutex) : _mutex(mutex) { _mutex->lock(); } 88 | ~ScopedOpenMPMutex() { _mutex->unlock(); } 89 | private: 90 | OpenMPMutex* const _mutex; 91 | ScopedOpenMPMutex(const ScopedOpenMPMutex&); 92 | void operator=(const ScopedOpenMPMutex&); 93 | }; 94 | 95 | } 96 | 97 | #endif 98 | -------------------------------------------------------------------------------- /g2o/g2o/core/optimization_algorithm.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "optimization_algorithm.h" 28 | 29 | using namespace std; 30 | 31 | namespace g2o { 32 | 33 | OptimizationAlgorithm::OptimizationAlgorithm() : 34 | _optimizer(0) 35 | { 36 | } 37 | 38 | OptimizationAlgorithm::~OptimizationAlgorithm() 39 | { 40 | } 41 | 42 | void OptimizationAlgorithm::printProperties(std::ostream& os) const 43 | { 44 | os << "------------- Algorithm Properties -------------" << endl; 45 | for (PropertyMap::const_iterator it = _properties.begin(); it != _properties.end(); ++it) { 46 | BaseProperty* p = it->second; 47 | os << it->first << "\t" << p->toString() << endl; 48 | } 49 | os << "------------------------------------------------" << endl; 50 | } 51 | 52 | bool OptimizationAlgorithm::updatePropertiesFromString(const std::string& propString) 53 | { 54 | return _properties.updateMapFromString(propString); 55 | } 56 | 57 | void OptimizationAlgorithm::setOptimizer(SparseOptimizer* optimizer) 58 | { 59 | _optimizer = optimizer; 60 | } 61 | 62 | } // end namespace 63 | -------------------------------------------------------------------------------- /g2o/g2o/core/optimization_algorithm.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_H 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "../stuff/property.h" 35 | 36 | #include "hyper_graph.h" 37 | #include "sparse_block_matrix.h" 38 | 39 | namespace g2o { 40 | 41 | class SparseOptimizer; 42 | 43 | /** 44 | * \brief Generic interface for a non-linear solver operating on a graph 45 | */ 46 | class OptimizationAlgorithm 47 | { 48 | public: 49 | enum SolverResult {Terminate=2, OK=1, Fail=-1}; 50 | OptimizationAlgorithm(); 51 | virtual ~OptimizationAlgorithm(); 52 | 53 | /** 54 | * initialize the solver, called once before the first call to solve() 55 | */ 56 | virtual bool init(bool online = false) = 0; 57 | 58 | /** 59 | * Solve one iteration. The SparseOptimizer running on-top will call this 60 | * for the given number of iterations. 61 | * @param iteration indicates the current iteration 62 | */ 63 | virtual SolverResult solve(int iteration, bool online = false) = 0; 64 | 65 | /** 66 | * computes the block diagonal elements of the pattern specified in the input 67 | * and stores them in given SparseBlockMatrix. 68 | * If your solver does not support computing the marginals, return false. 69 | */ 70 | virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices) = 0; 71 | 72 | /** 73 | * update the structures for online processing 74 | */ 75 | virtual bool updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges) = 0; 76 | 77 | /** 78 | * called by the optimizer if verbose. re-implement, if you want to print something 79 | */ 80 | virtual void printVerbose(std::ostream& os) const {(void) os;}; 81 | 82 | public: 83 | //! return the optimizer operating on 84 | const SparseOptimizer* optimizer() const { return _optimizer;} 85 | SparseOptimizer* optimizer() { return _optimizer;} 86 | 87 | /** 88 | * specify on which optimizer the solver should work on 89 | */ 90 | void setOptimizer(SparseOptimizer* optimizer); 91 | 92 | //! return the properties of the solver 93 | const PropertyMap& properties() const { return _properties;} 94 | 95 | /** 96 | * update the properties from a string, see PropertyMap::updateMapFromString() 97 | */ 98 | bool updatePropertiesFromString(const std::string& propString); 99 | 100 | /** 101 | * print the properties to a stream in a human readable fashion 102 | */ 103 | void printProperties(std::ostream& os) const; 104 | 105 | protected: 106 | SparseOptimizer* _optimizer; ///< the optimizer the solver is working on 107 | PropertyMap _properties; ///< the properties of your solver, use this to store the parameters of your solver 108 | 109 | private: 110 | // Disable the copy constructor and assignment operator 111 | OptimizationAlgorithm(const OptimizationAlgorithm&) { } 112 | OptimizationAlgorithm& operator= (const OptimizationAlgorithm&) { return *this; } 113 | }; 114 | 115 | } // end namespace 116 | 117 | #endif 118 | -------------------------------------------------------------------------------- /g2o/g2o/core/optimization_algorithm_dogleg.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H 29 | 30 | #include "optimization_algorithm_with_hessian.h" 31 | 32 | namespace g2o { 33 | 34 | class BlockSolverBase; 35 | 36 | /** 37 | * \brief Implementation of Powell's Dogleg Algorithm 38 | */ 39 | class OptimizationAlgorithmDogleg : public OptimizationAlgorithmWithHessian 40 | { 41 | public: 42 | /** \brief type of the step to take */ 43 | enum { 44 | STEP_UNDEFINED, 45 | STEP_SD, STEP_GN, STEP_DL 46 | }; 47 | 48 | public: 49 | /** 50 | * construct the Dogleg algorithm, which will use the given Solver for solving the 51 | * linearized system. 52 | */ 53 | explicit OptimizationAlgorithmDogleg(BlockSolverBase* solver); 54 | virtual ~OptimizationAlgorithmDogleg(); 55 | 56 | virtual SolverResult solve(int iteration, bool online = false); 57 | 58 | virtual void printVerbose(std::ostream& os) const; 59 | 60 | //! return the type of the last step taken by the algorithm 61 | int lastStep() const { return _lastStep;} 62 | //! return the diameter of the trust region 63 | double trustRegion() const { return _delta;} 64 | 65 | //! convert the type into an integer 66 | static const char* stepType2Str(int stepType); 67 | 68 | protected: 69 | // parameters 70 | Property* _maxTrialsAfterFailure; 71 | Property* _userDeltaInit; 72 | // damping to enforce positive definite matrix 73 | Property* _initialLambda; 74 | Property* _lamdbaFactor; 75 | 76 | Eigen::VectorXd _hsd; ///< steepest decent step 77 | Eigen::VectorXd _hdl; ///< final dogleg step 78 | Eigen::VectorXd _auxVector; ///< auxilary vector used to perform multiplications or other stuff 79 | 80 | double _currentLambda; ///< the damping factor to force positive definite matrix 81 | double _delta; ///< trust region 82 | int _lastStep; ///< type of the step taken by the algorithm 83 | bool _wasPDInAllIterations; ///< the matrix we solve was positive definite in all iterations -> if not apply damping 84 | int _lastNumTries; 85 | }; 86 | 87 | } // end namespace 88 | 89 | #endif 90 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /g2o/g2o/core/optimization_algorithm_gauss_newton.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H 29 | 30 | #include "optimization_algorithm_with_hessian.h" 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief Implementation of the Gauss Newton Algorithm 36 | */ 37 | class OptimizationAlgorithmGaussNewton : public OptimizationAlgorithmWithHessian 38 | { 39 | public: 40 | /** 41 | * construct the Gauss Newton algorithm, which use the given Solver for solving the 42 | * linearized system. 43 | */ 44 | explicit OptimizationAlgorithmGaussNewton(Solver* solver); 45 | virtual ~OptimizationAlgorithmGaussNewton(); 46 | 47 | virtual SolverResult solve(int iteration, bool online = false); 48 | 49 | virtual void printVerbose(std::ostream& os) const; 50 | }; 51 | 52 | } // end namespace 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /g2o/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 | //new add, to access the to be estimated estimate matrix 33 | #include "../types/types_six_dof_expmap.h" 34 | 35 | 36 | namespace g2o { 37 | 38 | /** 39 | * \brief Implementation of the Levenberg Algorithm 40 | */ 41 | class OptimizationAlgorithmLevenberg : public OptimizationAlgorithmWithHessian 42 | { 43 | public: 44 | /** 45 | * construct the Levenberg algorithm, which will use the given Solver for solving the 46 | * linearized system. 47 | */ 48 | explicit OptimizationAlgorithmLevenberg(Solver* solver); 49 | virtual ~OptimizationAlgorithmLevenberg(); 50 | 51 | virtual SolverResult solve(int iteration, bool online = false); 52 | 53 | virtual void printVerbose(std::ostream& os) const; 54 | 55 | //! return the currently used damping factor 56 | double currentLambda() const { return _currentLambda;} 57 | 58 | //! the number of internal iteration if an update step increases chi^2 within Levenberg-Marquardt 59 | void setMaxTrialsAfterFailure(int max_trials); 60 | 61 | //! get the number of inner iterations for Levenberg-Marquardt 62 | int maxTrialsAfterFailure() const { return _maxTrialsAfterFailure->value();} 63 | 64 | //! return the lambda set by the user, if < 0 the SparseOptimizer will compute the initial lambda 65 | double userLambdaInit() {return _userLambdaInit->value();} 66 | //! specify the initial lambda used for the first iteraion, if not given the SparseOptimizer tries to compute a suitable value 67 | void setUserLambdaInit(double lambda); 68 | 69 | //! return the number of levenberg iterations performed in the last round 70 | int levenbergIteration() { return _levenbergIterations;} 71 | 72 | protected: 73 | // Levenberg parameters 74 | Property* _maxTrialsAfterFailure; 75 | Property* _userLambdaInit; 76 | double _currentLambda; 77 | double _tau; 78 | double _goodStepLowerScale; ///< lower bound for lambda decrease if a good LM step 79 | double _goodStepUpperScale; ///< upper bound for lambda decrease if a good LM step 80 | double _ni; 81 | int _levenbergIterations; ///< the numer of levenberg iterations performed to accept the last step 82 | //RAUL 83 | int _nBad; 84 | 85 | /** 86 | * helper for Levenberg, this function computes the initial damping factor, if the user did not 87 | * specify an own value, see setUserLambdaInit() 88 | */ 89 | double computeLambdaInit() const; 90 | double computeScale() const; 91 | 92 | }; 93 | 94 | } // end namespace 95 | 96 | #endif 97 | -------------------------------------------------------------------------------- /g2o/g2o/core/optimization_algorithm_property.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H 29 | 30 | #include 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief describe the properties of a solver 36 | */ 37 | struct OptimizationAlgorithmProperty 38 | { 39 | std::string name; ///< name of the solver, e.g., var 40 | std::string desc; ///< short description of the solver 41 | std::string type; ///< type of solver, e.g., "CSparse Cholesky", "PCG" 42 | bool requiresMarginalize; ///< whether the solver requires marginalization of landmarks 43 | int poseDim; ///< dimension of the pose vertices (-1 if variable) 44 | int landmarkDim; ///< dimension of the landmar vertices (-1 if variable) 45 | OptimizationAlgorithmProperty() : 46 | name(), desc(), type(), requiresMarginalize(false), poseDim(-1), landmarkDim(-1) 47 | { 48 | } 49 | OptimizationAlgorithmProperty(const std::string& name_, const std::string& desc_, const std::string& type_, bool requiresMarginalize_, int poseDim_, int landmarkDim_) : 50 | name(name_), desc(desc_), type(type_), requiresMarginalize(requiresMarginalize_), poseDim(poseDim_), landmarkDim(landmarkDim_) 51 | { 52 | } 53 | }; 54 | 55 | } // end namespace 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /g2o/g2o/core/optimization_algorithm_with_hessian.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "optimization_algorithm_with_hessian.h" 28 | 29 | #include "solver.h" 30 | #include "optimizable_graph.h" 31 | #include "sparse_optimizer.h" 32 | 33 | #include 34 | using namespace std; 35 | 36 | namespace g2o { 37 | 38 | OptimizationAlgorithmWithHessian::OptimizationAlgorithmWithHessian(Solver* solver) : 39 | OptimizationAlgorithm(), 40 | _solver(solver) 41 | { 42 | _writeDebug = _properties.makeProperty >("writeDebug", true); 43 | } 44 | 45 | OptimizationAlgorithmWithHessian::~OptimizationAlgorithmWithHessian() 46 | { 47 | delete _solver; 48 | } 49 | 50 | bool OptimizationAlgorithmWithHessian::init(bool online) 51 | { 52 | assert(_optimizer && "_optimizer not set"); 53 | assert(_solver && "Solver not set"); 54 | _solver->setWriteDebug(_writeDebug->value()); 55 | bool useSchur=false; 56 | for (OptimizableGraph::VertexContainer::const_iterator it=_optimizer->activeVertices().begin(); it!=_optimizer->activeVertices().end(); ++it) { 57 | OptimizableGraph::Vertex* v= *it; 58 | if (v->marginalized()){ 59 | useSchur=true; 60 | break; 61 | } 62 | } 63 | if (useSchur){ 64 | if (_solver->supportsSchur()) 65 | _solver->setSchur(true); 66 | } else { 67 | if (_solver->supportsSchur()) 68 | _solver->setSchur(false); 69 | } 70 | 71 | bool initState = _solver->init(_optimizer, online); 72 | return initState; 73 | } 74 | 75 | bool OptimizationAlgorithmWithHessian::computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices) 76 | { 77 | return _solver ? _solver->computeMarginals(spinv, blockIndices) : false; 78 | } 79 | 80 | bool OptimizationAlgorithmWithHessian::buildLinearStructure() 81 | { 82 | return _solver ? _solver->buildStructure() : false; 83 | } 84 | 85 | void OptimizationAlgorithmWithHessian::updateLinearSystem() 86 | { 87 | if (_solver) 88 | _solver->buildSystem(); 89 | } 90 | 91 | bool OptimizationAlgorithmWithHessian::updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges) 92 | { 93 | return _solver ? _solver->updateStructure(vset, edges) : false; 94 | } 95 | 96 | void OptimizationAlgorithmWithHessian::setWriteDebug(bool writeDebug) 97 | { 98 | _writeDebug->setValue(writeDebug); 99 | } 100 | 101 | } // end namespace 102 | -------------------------------------------------------------------------------- /g2o/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 | #include "computeH.cuh" 32 | 33 | namespace g2o { 34 | 35 | class Solver; 36 | 37 | /** 38 | * \brief Base for solvers operating on the approximated Hessian, e.g., Gauss-Newton, Levenberg 39 | */ 40 | class OptimizationAlgorithmWithHessian : public OptimizationAlgorithm 41 | { 42 | public: 43 | explicit OptimizationAlgorithmWithHessian(Solver* solver); 44 | virtual ~OptimizationAlgorithmWithHessian(); 45 | 46 | virtual bool init(bool online = false); 47 | 48 | virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices); 49 | 50 | virtual bool buildLinearStructure(); 51 | 52 | virtual void updateLinearSystem(); 53 | 54 | virtual bool updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges); 55 | 56 | //! return the underlying solver used to solve the linear system 57 | Solver* solver() { return _solver;} 58 | 59 | /** 60 | * write debug output of the Hessian if system is not positive definite 61 | */ 62 | virtual void setWriteDebug(bool writeDebug); 63 | virtual bool writeDebug() const { return _writeDebug->value();} 64 | 65 | protected: 66 | Solver* _solver; 67 | Property* _writeDebug; 68 | 69 | }; 70 | 71 | }// end namespace 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /g2o/g2o/core/parameter.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "parameter.h" 28 | 29 | namespace g2o { 30 | 31 | Parameter::Parameter() : _id(-1) 32 | { 33 | } 34 | 35 | void Parameter::setId(int id_) 36 | { 37 | _id = id_; 38 | } 39 | 40 | } // end namespace 41 | -------------------------------------------------------------------------------- /g2o/g2o/core/parameter.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_GRAPH_PARAMETER_HH_ 28 | #define G2O_GRAPH_PARAMETER_HH_ 29 | 30 | #include 31 | 32 | #include "hyper_graph.h" 33 | 34 | namespace g2o { 35 | 36 | class Parameter : public HyperGraph::HyperGraphElement 37 | { 38 | public: 39 | Parameter(); 40 | virtual ~Parameter() {}; 41 | //! read the data from a stream 42 | virtual bool read(std::istream& is) = 0; 43 | //! write the data to a stream 44 | virtual bool write(std::ostream& os) const = 0; 45 | int id() const {return _id;} 46 | void setId(int id_); 47 | virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_PARAMETER;} 48 | protected: 49 | int _id; 50 | }; 51 | 52 | typedef std::vector ParameterVector; 53 | 54 | } // end namespace 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /g2o/g2o/core/parameter_container.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "parameter_container.h" 28 | 29 | #include 30 | 31 | #include "factory.h" 32 | #include "parameter.h" 33 | 34 | #include "../stuff/macros.h" 35 | #include "../stuff/color_macros.h" 36 | #include "../stuff/string_tools.h" 37 | 38 | namespace g2o { 39 | 40 | using namespace std; 41 | 42 | ParameterContainer::ParameterContainer(bool isMainStorage_) : 43 | _isMainStorage(isMainStorage_) 44 | { 45 | } 46 | 47 | void ParameterContainer::clear() { 48 | if (!_isMainStorage) 49 | return; 50 | for (iterator it = begin(); it!=end(); it++){ 51 | delete it->second; 52 | } 53 | BaseClass::clear(); 54 | } 55 | 56 | ParameterContainer::~ParameterContainer(){ 57 | clear(); 58 | } 59 | 60 | bool ParameterContainer::addParameter(Parameter* p){ 61 | if (p->id()<0) 62 | return false; 63 | iterator it=find(p->id()); 64 | if (it!=end()) 65 | return false; 66 | insert(make_pair(p->id(), p)); 67 | return true; 68 | } 69 | 70 | Parameter* ParameterContainer::getParameter(int id) { 71 | iterator it=find(id); 72 | if (it==end()) 73 | return 0; 74 | return it->second; 75 | } 76 | 77 | Parameter* ParameterContainer::detachParameter(int id){ 78 | iterator it=find(id); 79 | if (it==end()) 80 | return 0; 81 | Parameter* p=it->second; 82 | erase(it); 83 | return p; 84 | } 85 | 86 | bool ParameterContainer::write(std::ostream& os) const{ 87 | Factory* factory = Factory::instance(); 88 | for (const_iterator it=begin(); it!=end(); it++){ 89 | os << factory->tag(it->second) << " "; 90 | os << it->second->id() << " "; 91 | it->second->write(os); 92 | os << endl; 93 | } 94 | return true; 95 | } 96 | 97 | bool ParameterContainer::read(std::istream& is, const std::map* _renamedTypesLookup){ 98 | stringstream currentLine; 99 | string token; 100 | 101 | Factory* factory = Factory::instance(); 102 | HyperGraph::GraphElemBitset elemBitset; 103 | elemBitset[HyperGraph::HGET_PARAMETER] = 1; 104 | 105 | while (1) { 106 | int bytesRead = readLine(is, currentLine); 107 | if (bytesRead == -1) 108 | break; 109 | currentLine >> token; 110 | if (bytesRead == 0 || token.size() == 0 || token[0] == '#') 111 | continue; 112 | if (_renamedTypesLookup && _renamedTypesLookup->size()>0){ 113 | map::const_iterator foundIt = _renamedTypesLookup->find(token); 114 | if (foundIt != _renamedTypesLookup->end()) { 115 | token = foundIt->second; 116 | } 117 | } 118 | 119 | HyperGraph::HyperGraphElement* element = factory->construct(token, elemBitset); 120 | if (! element) // not a parameter or otherwise unknown tag 121 | continue; 122 | assert(element->elementType() == HyperGraph::HGET_PARAMETER && "Should be a param"); 123 | 124 | Parameter* p = static_cast(element); 125 | int pid; 126 | currentLine >> pid; 127 | p->setId(pid); 128 | bool r = p->read(currentLine); 129 | if (! r) { 130 | cerr << __PRETTY_FUNCTION__ << ": Error reading data " << token << " for parameter " << pid << endl; 131 | delete p; 132 | } else { 133 | if (! addParameter(p) ){ 134 | cerr << __PRETTY_FUNCTION__ << ": Parameter of type:" << token << " id:" << pid << " already defined" << endl; 135 | } 136 | } 137 | } // while read line 138 | 139 | return true; 140 | } 141 | 142 | } // end namespace 143 | -------------------------------------------------------------------------------- /g2o/g2o/core/parameter_container.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_GRAPH_PARAMETER_CONTAINER_HH_ 28 | #define G2O_GRAPH_PARAMETER_CONTAINER_HH_ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace g2o { 35 | 36 | class Parameter; 37 | 38 | /** 39 | * \brief map id to parameters 40 | */ 41 | class ParameterContainer : protected std::map 42 | { 43 | public: 44 | typedef std::map BaseClass; 45 | 46 | /** 47 | * create a container for the parameters. 48 | * @param isMainStorage_ pointers to the parameters are owned by this container, i.e., freed in its constructor 49 | */ 50 | ParameterContainer(bool isMainStorage_=true); 51 | virtual ~ParameterContainer(); 52 | //! add parameter to the container 53 | bool addParameter(Parameter* p); 54 | //! return a parameter based on its ID 55 | Parameter* getParameter(int id); 56 | //! remove a parameter from the container, i.e., the user now owns the pointer 57 | Parameter* detachParameter(int id); 58 | //! read parameters from a stream 59 | virtual bool read(std::istream& is, const std::map* renamedMap =0); 60 | //! write the data to a stream 61 | virtual bool write(std::ostream& os) const; 62 | bool isMainStorage() const {return _isMainStorage;} 63 | void clear(); 64 | 65 | // stuff of the base class that should re-appear 66 | using BaseClass::size; 67 | 68 | protected: 69 | bool _isMainStorage; 70 | }; 71 | 72 | } // end namespace 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /g2o/g2o/core/robust_kernel.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "robust_kernel.h" 28 | 29 | namespace g2o { 30 | 31 | RobustKernel::RobustKernel() : 32 | _delta(1.) 33 | { 34 | } 35 | 36 | RobustKernel::RobustKernel(double delta) : 37 | _delta(delta) 38 | { 39 | } 40 | 41 | void RobustKernel::setDelta(double delta) 42 | { 43 | _delta = delta; 44 | } 45 | 46 | } // end namespace g2o 47 | -------------------------------------------------------------------------------- /g2o/g2o/core/robust_kernel.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_ROBUST_KERNEL_H 28 | #define G2O_ROBUST_KERNEL_H 29 | 30 | #ifdef _MSC_VER 31 | #include 32 | #else 33 | #include 34 | #endif 35 | #include 36 | 37 | 38 | namespace g2o { 39 | 40 | /** 41 | * \brief base for all robust cost functions 42 | * 43 | * Note in all the implementations for the other cost functions the e in the 44 | * funtions corresponds to the sqaured errors, i.e., the robustification 45 | * functions gets passed the squared error. 46 | * 47 | * e.g. the robustified least squares function is 48 | * 49 | * chi^2 = sum_{e} rho( e^T Omega e ) 50 | */ 51 | class RobustKernel 52 | { 53 | public: 54 | RobustKernel(); 55 | explicit RobustKernel(double delta); 56 | virtual ~RobustKernel() {} 57 | /** 58 | * compute the scaling factor for a error: 59 | * The error is e^T Omega e 60 | * The output rho is 61 | * rho[0]: The actual scaled error value 62 | * rho[1]: First derivative of the scaling function 63 | * rho[2]: Second derivative of the scaling function 64 | */ 65 | virtual void robustify(double squaredError, Eigen::Vector3d& rho) const = 0; 66 | 67 | /** 68 | * set the window size of the error. A squared error above delta^2 is considered 69 | * as outlier in the data. 70 | */ 71 | virtual void setDelta(double delta); 72 | double delta() const { return _delta;} 73 | 74 | protected: 75 | double _delta; 76 | }; 77 | typedef std::tr1::shared_ptr RobustKernelPtr; 78 | 79 | } // end namespace g2o 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /g2o/g2o/core/robust_kernel_factory.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "robust_kernel_factory.h" 28 | #include "robust_kernel.h" 29 | 30 | #include 31 | 32 | using namespace std; 33 | 34 | namespace g2o { 35 | 36 | RobustKernelFactory* RobustKernelFactory::factoryInstance = 0; 37 | 38 | RobustKernelFactory::RobustKernelFactory() 39 | { 40 | } 41 | 42 | RobustKernelFactory::~RobustKernelFactory() 43 | { 44 | for (CreatorMap::iterator it = _creator.begin(); it != _creator.end(); ++it) { 45 | delete it->second; 46 | } 47 | _creator.clear(); 48 | } 49 | 50 | RobustKernelFactory* RobustKernelFactory::instance() 51 | { 52 | if (factoryInstance == 0) { 53 | factoryInstance = new RobustKernelFactory; 54 | } 55 | 56 | return factoryInstance; 57 | } 58 | 59 | void RobustKernelFactory::registerRobustKernel(const std::string& tag, AbstractRobustKernelCreator* c) 60 | { 61 | CreatorMap::const_iterator foundIt = _creator.find(tag); 62 | if (foundIt != _creator.end()) { 63 | cerr << "RobustKernelFactory WARNING: Overwriting robust kernel tag " << tag << endl; 64 | assert(0); 65 | } 66 | 67 | _creator[tag] = c; 68 | } 69 | 70 | void RobustKernelFactory::unregisterType(const std::string& tag) 71 | { 72 | CreatorMap::iterator tagPosition = _creator.find(tag); 73 | if (tagPosition != _creator.end()) { 74 | AbstractRobustKernelCreator* c = tagPosition->second; 75 | delete c; 76 | _creator.erase(tagPosition); 77 | } 78 | } 79 | 80 | RobustKernel* RobustKernelFactory::construct(const std::string& tag) const 81 | { 82 | CreatorMap::const_iterator foundIt = _creator.find(tag); 83 | if (foundIt != _creator.end()) { 84 | return foundIt->second->construct(); 85 | } 86 | return 0; 87 | } 88 | 89 | AbstractRobustKernelCreator* RobustKernelFactory::creator(const std::string& tag) const 90 | { 91 | CreatorMap::const_iterator foundIt = _creator.find(tag); 92 | if (foundIt != _creator.end()) { 93 | return foundIt->second; 94 | } 95 | return 0; 96 | } 97 | 98 | void RobustKernelFactory::fillKnownKernels(std::vector& types) const 99 | { 100 | types.clear(); 101 | for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it) 102 | types.push_back(it->first); 103 | } 104 | 105 | void RobustKernelFactory::destroy() 106 | { 107 | delete factoryInstance; 108 | factoryInstance = 0; 109 | } 110 | 111 | } // end namespace 112 | -------------------------------------------------------------------------------- /g2o/g2o/core/robust_kernel_factory.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_FACTORY_H 28 | #define G2O_ROBUST_KERNEL_FACTORY_H 29 | 30 | #include "../stuff/misc.h" 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | namespace g2o { 38 | 39 | class RobustKernel; 40 | 41 | /** 42 | * \brief Abstract interface for allocating a robust kernel 43 | */ 44 | class AbstractRobustKernelCreator 45 | { 46 | public: 47 | /** 48 | * create a hyper graph element. Has to implemented in derived class. 49 | */ 50 | virtual RobustKernel* construct() = 0; 51 | virtual ~AbstractRobustKernelCreator() { } 52 | }; 53 | 54 | /** 55 | * \brief templatized creator class which creates graph elements 56 | */ 57 | template 58 | class RobustKernelCreator : public AbstractRobustKernelCreator 59 | { 60 | public: 61 | RobustKernel* construct() { return new T;} 62 | }; 63 | 64 | /** 65 | * \brief create robust kernels based on their human readable name 66 | */ 67 | class RobustKernelFactory 68 | { 69 | public: 70 | 71 | //! return the instance 72 | static RobustKernelFactory* instance(); 73 | 74 | //! free the instance 75 | static void destroy(); 76 | 77 | /** 78 | * register a tag for a specific creator 79 | */ 80 | void registerRobustKernel(const std::string& tag, AbstractRobustKernelCreator* c); 81 | 82 | /** 83 | * unregister a tag for a specific creator 84 | */ 85 | void unregisterType(const std::string& tag); 86 | 87 | /** 88 | * construct a robust kernel based on its tag 89 | */ 90 | RobustKernel* construct(const std::string& tag) const; 91 | 92 | /** 93 | * return the creator for a specific tag 94 | */ 95 | AbstractRobustKernelCreator* creator(const std::string& tag) const; 96 | 97 | /** 98 | * get a list of all known robust kernels 99 | */ 100 | void fillKnownKernels(std::vector& types) const; 101 | 102 | protected: 103 | 104 | typedef std::map CreatorMap; 105 | RobustKernelFactory(); 106 | ~RobustKernelFactory(); 107 | 108 | CreatorMap _creator; ///< look-up map for the existing creators 109 | 110 | private: 111 | static RobustKernelFactory* factoryInstance; 112 | }; 113 | 114 | template 115 | class RegisterRobustKernelProxy 116 | { 117 | public: 118 | RegisterRobustKernelProxy(const std::string& name) : _name(name) 119 | { 120 | RobustKernelFactory::instance()->registerRobustKernel(_name, new RobustKernelCreator()); 121 | } 122 | 123 | ~RegisterRobustKernelProxy() 124 | { 125 | RobustKernelFactory::instance()->unregisterType(_name); 126 | } 127 | 128 | private: 129 | std::string _name; 130 | }; 131 | 132 | #if defined _MSC_VER && defined G2O_SHARED_LIBS 133 | # define G2O_ROBUST_KERNEL_FACTORY_EXPORT __declspec(dllexport) 134 | # define G2O_ROBUST_KERNEL_FACTORY_IMPORT __declspec(dllimport) 135 | #else 136 | # define G2O_ROBUST_KERNEL_FACTORY_EXPORT 137 | # define G2O_ROBUST_KERNEL_FACTORY_IMPORT 138 | #endif 139 | 140 | // These macros are used to automate registering of robust kernels and forcing linkage 141 | #define G2O_REGISTER_ROBUST_KERNEL(name, classname) \ 142 | extern "C" void G2O_ROBUST_KERNEL_FACTORY_EXPORT g2o_robust_kernel_##classname(void) {} \ 143 | static g2o::RegisterRobustKernelProxy g_robust_kernel_proxy_##classname(#name); 144 | 145 | #define G2O_USE_ROBUST_KERNEL(classname) \ 146 | extern "C" void G2O_ROBUST_KERNEL_FACTORY_IMPORT g2o_robust_kernel_##classname(void); \ 147 | static g2o::TypeFunctionProxy proxy_##classname(g2o_robust_kernel_##classname); 148 | 149 | } // end namespace g2o 150 | 151 | #endif 152 | -------------------------------------------------------------------------------- /g2o/g2o/core/robust_kernel_impl.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_IMPL_H 28 | #define G2O_ROBUST_KERNEL_IMPL_H 29 | 30 | #include "robust_kernel.h" 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief scale a robust kernel to another delta (window size) 36 | * 37 | * Scales a robust kernel to another window size. Useful, in case if 38 | * one implements a kernel which only is designed for a fixed window 39 | * size. 40 | */ 41 | class RobustKernelScaleDelta : public RobustKernel 42 | { 43 | public: 44 | /** 45 | * construct the scaled kernel ontop of another kernel which might be shared accross 46 | * several scaled kernels 47 | */ 48 | explicit RobustKernelScaleDelta(const RobustKernelPtr& kernel, double delta = 1.); 49 | explicit RobustKernelScaleDelta(double delta = 1.); 50 | 51 | //! return the underlying kernel 52 | const RobustKernelPtr kernel() const { return _kernel;} 53 | //! use another kernel for the underlying operation 54 | void setKernel(const RobustKernelPtr& ptr); 55 | 56 | void robustify(double error, Eigen::Vector3d& rho) const; 57 | 58 | protected: 59 | RobustKernelPtr _kernel; 60 | }; 61 | 62 | /** 63 | * \brief Huber Cost Function 64 | * 65 | * Loss function as described by Huber 66 | * See http://en.wikipedia.org/wiki/Huber_loss_function 67 | * 68 | * If e^(1/2) < d 69 | * rho(e) = e 70 | * 71 | * else 72 | * 73 | * 1/2 2 74 | * rho(e) = 2 d e - d 75 | */ 76 | class RobustKernelHuber : public RobustKernel 77 | { 78 | public: 79 | virtual void setDelta(double delta); 80 | virtual void setDeltaSqr(const double &delta, const double &deltaSqr); 81 | virtual void robustify(double e2, Eigen::Vector3d& rho) const; 82 | 83 | private: 84 | float dsqr; 85 | }; 86 | 87 | /** 88 | * \brief Tukey Cost Function 89 | * 90 | * 91 | * If e^(1/2) < d 92 | * rho(e) = delta2(1-(1-e/delta2)^3) 93 | * 94 | * else 95 | * 96 | * rho(e) = delta2 97 | */ 98 | class RobustKernelTukey : public RobustKernel 99 | { 100 | public: 101 | 102 | virtual void setDeltaSqr(const double &deltaSqr, const double &inv); 103 | virtual void robustify(double e2, Eigen::Vector3d& rho) const; 104 | private: 105 | float _deltaSqr; 106 | float _invDeltaSqr; 107 | }; 108 | 109 | 110 | /** 111 | * \brief Pseudo Huber Cost Function 112 | * 113 | * The smooth pseudo huber cost function: 114 | * See http://en.wikipedia.org/wiki/Huber_loss_function 115 | * 116 | * 2 e 117 | * 2 d (sqrt(-- + 1) - 1) 118 | * 2 119 | * d 120 | */ 121 | class RobustKernelPseudoHuber : public RobustKernel 122 | { 123 | public: 124 | virtual void robustify(double e2, Eigen::Vector3d& rho) const; 125 | }; 126 | 127 | /** 128 | * \brief Cauchy cost function 129 | * 130 | * 2 e 131 | * d log(-- + 1) 132 | * 2 133 | * d 134 | */ 135 | class RobustKernelCauchy : public RobustKernel 136 | { 137 | public: 138 | virtual void robustify(double e2, Eigen::Vector3d& rho) const; 139 | }; 140 | 141 | /** 142 | * \brief Saturated cost function. 143 | * 144 | * The error is at most delta^2 145 | */ 146 | class RobustKernelSaturated : public RobustKernel 147 | { 148 | public: 149 | virtual void robustify(double e2, Eigen::Vector3d& rho) const; 150 | }; 151 | 152 | /** 153 | * \brief Dynamic covariance scaling - DCS 154 | * 155 | * See paper Robust Map Optimization from Agarwal et al. ICRA 2013 156 | * 157 | * delta is used as $phi$ 158 | */ 159 | class RobustKernelDCS : public RobustKernel 160 | { 161 | public: 162 | virtual void robustify(double e2, Eigen::Vector3d& rho) const; 163 | }; 164 | 165 | } // end namespace g2o 166 | 167 | #endif 168 | -------------------------------------------------------------------------------- /g2o/g2o/core/solver.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "solver.h" 28 | 29 | #include 30 | #include 31 | 32 | namespace g2o { 33 | 34 | Solver::Solver() : 35 | _optimizer(0), _x(0), _b(0), _xSize(0), _maxXSize(0), 36 | _isLevenberg(false), _additionalVectorSpace(0) 37 | { 38 | } 39 | 40 | Solver::~Solver() 41 | { 42 | delete[] _x; 43 | delete[] _b; 44 | } 45 | 46 | void Solver::resizeVector(size_t sx) 47 | { 48 | size_t oldSize = _xSize; 49 | _xSize = sx; 50 | sx += _additionalVectorSpace; // allocate some additional space if requested 51 | if (_maxXSize < sx) { 52 | _maxXSize = 2*sx; 53 | delete[] _x; 54 | _x = new double[_maxXSize]; 55 | #ifndef NDEBUG 56 | memset(_x, 0, _maxXSize * sizeof(double)); 57 | #endif 58 | if (_b) { // backup the former b, might still be needed for online processing 59 | memcpy(_x, _b, oldSize * sizeof(double)); 60 | delete[] _b; 61 | _b = new double[_maxXSize]; 62 | std::swap(_b, _x); 63 | } else { 64 | _b = new double[_maxXSize]; 65 | #ifndef NDEBUG 66 | memset(_b, 0, _maxXSize * sizeof(double)); 67 | #endif 68 | } 69 | } 70 | } 71 | 72 | void Solver::setOptimizer(SparseOptimizer* optimizer) 73 | { 74 | _optimizer = optimizer; 75 | } 76 | 77 | void Solver::setLevenberg(bool levenberg) 78 | { 79 | _isLevenberg = levenberg; 80 | } 81 | 82 | void Solver::setAdditionalVectorSpace(size_t s) 83 | { 84 | _additionalVectorSpace = s; 85 | } 86 | 87 | } // end namespace 88 | -------------------------------------------------------------------------------- /g2o/g2o/core/sparse_block_matrix_diagonal.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_SPARSE_BLOCK_MATRIX_DIAGONAL_H 28 | #define G2O_SPARSE_BLOCK_MATRIX_DIAGONAL_H 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "../../config.h" 35 | #include "matrix_operations.h" 36 | 37 | namespace g2o { 38 | 39 | /** 40 | * \brief Sparse matrix which uses blocks on the diagonal 41 | * 42 | * This class is used as a const view on a SparseBlockMatrix 43 | * which allows a faster iteration over the elements of the 44 | * matrix. 45 | */ 46 | template 47 | class SparseBlockMatrixDiagonal 48 | { 49 | public: 50 | //! this is the type of the elementary block, it is an Eigen::Matrix. 51 | typedef MatrixType SparseMatrixBlock; 52 | 53 | //! columns of the matrix 54 | int cols() const {return _blockIndices.size() ? _blockIndices.back() : 0;} 55 | //! rows of the matrix 56 | int rows() const {return _blockIndices.size() ? _blockIndices.back() : 0;} 57 | 58 | typedef std::vector > DiagonalVector; 59 | 60 | SparseBlockMatrixDiagonal(const std::vector& blockIndices) : 61 | _blockIndices(blockIndices) 62 | {} 63 | 64 | //! how many rows/cols does the block at block-row / block-column r has? 65 | inline int dimOfBlock(int r) const { return r ? _blockIndices[r] - _blockIndices[r-1] : _blockIndices[0] ; } 66 | 67 | //! where does the row /col at block-row / block-column r starts? 68 | inline int baseOfBlock(int r) const { return r ? _blockIndices[r-1] : 0 ; } 69 | 70 | //! the block matrices per block-column 71 | const DiagonalVector& diagonal() const { return _diagonal;} 72 | DiagonalVector& diagonal() { return _diagonal;} 73 | 74 | //! indices of the row blocks 75 | const std::vector& blockIndices() const { return _blockIndices;} 76 | 77 | void multiply(double*& dest, const double* src) const 78 | { 79 | int destSize=cols(); 80 | if (! dest) { 81 | dest=new double[destSize]; 82 | memset(dest,0, destSize*sizeof(double)); 83 | } 84 | 85 | // map the memory by Eigen 86 | Eigen::Map destVec(dest, destSize); 87 | Eigen::Map srcVec(src, rows()); 88 | 89 | # ifdef G2O_OPENMP 90 | # pragma omp parallel for default (shared) schedule(dynamic, 10) 91 | # endif 92 | for (int i=0; i < static_cast(_diagonal.size()); ++i){ 93 | int destOffset = baseOfBlock(i); 94 | int srcOffset = destOffset; 95 | const SparseMatrixBlock& A = _diagonal[i]; 96 | // destVec += *A.transpose() * srcVec (according to the sub-vector parts) 97 | internal::axpy(A, srcVec, srcOffset, destVec, destOffset); 98 | } 99 | } 100 | 101 | protected: 102 | const std::vector& _blockIndices; ///< vector of the indices of the blocks along the diagonal 103 | DiagonalVector _diagonal; 104 | }; 105 | 106 | } //end namespace 107 | 108 | #endif 109 | -------------------------------------------------------------------------------- /g2o/g2o/core/sparse_block_matrix_test.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "sparse_block_matrix.h" 28 | #include 29 | 30 | using namespace std; 31 | using namespace g2o; 32 | using namespace Eigen; 33 | 34 | typedef SparseBlockMatrix< MatrixXd > 35 | SparseBlockMatrixX; 36 | 37 | std::ostream& operator << (std::ostream& os, const SparseBlockMatrixX::SparseMatrixBlock& m) { 38 | for (int i=0; iblock(0,0, true); 55 | cerr << b->rows() << " " << b->cols() << endl; 56 | for (int i=0; irows(); ++i) 57 | for (int j=0; jcols(); ++j){ 58 | (*b)(i,j)=i*b->cols()+j; 59 | } 60 | 61 | 62 | cerr << "block access 2" << endl; 63 | b=M->block(0,2, true); 64 | cerr << b->rows() << " " << b->cols() << endl; 65 | for (int i=0; irows(); ++i) 66 | for (int j=0; jcols(); ++j){ 67 | (*b)(i,j)=i*b->cols()+j; 68 | } 69 | 70 | b=M->block(3,2, true); 71 | cerr << b->rows() << " " << b->cols() << endl; 72 | for (int i=0; irows(); ++i) 73 | for (int j=0; jcols(); ++j){ 74 | (*b)(i,j)=i*b->cols()+j; 75 | } 76 | 77 | cerr << *M << endl; 78 | 79 | cerr << "SUM" << endl; 80 | 81 | SparseBlockMatrixX* Ms=0; 82 | M->add(Ms); 83 | M->add(Ms); 84 | cerr << *Ms; 85 | 86 | SparseBlockMatrixX* Mt=0; 87 | M->transpose(Mt); 88 | cerr << *Mt << endl; 89 | 90 | SparseBlockMatrixX* Mp=0; 91 | M->multiply(Mp, Mt); 92 | cerr << *Mp << endl; 93 | 94 | int iperm[]={3,2,1,0}; 95 | SparseBlockMatrixX* PMp=0; 96 | 97 | Mp->symmPermutation(PMp,iperm, false); 98 | cerr << *PMp << endl; 99 | 100 | PMp->clear(true); 101 | Mp->block(3,0)->fill(0.); 102 | Mp->symmPermutation(PMp,iperm, true); 103 | cerr << *PMp << endl; 104 | 105 | 106 | 107 | } 108 | -------------------------------------------------------------------------------- /g2o/g2o/solvers/linear_solver_dense.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 H. Strasdat 3 | // Copyright (C) 2012 R. Kümmerle 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are 8 | // met: 9 | // 10 | // * Redistributions of source code must retain the above copyright notice, 11 | // this list of conditions and the following disclaimer. 12 | // * Redistributions in binary form must reproduce the above copyright 13 | // notice, this list of conditions and the following disclaimer in the 14 | // documentation and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 17 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 18 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 19 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 20 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 22 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 23 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 24 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 25 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | 28 | #ifndef G2O_LINEAR_SOLVER_DENSE_H 29 | #define G2O_LINEAR_SOLVER_DENSE_H 30 | 31 | #include "../core/linear_solver.h" 32 | #include "../core/batch_stats.h" 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | 40 | namespace g2o { 41 | 42 | /** 43 | * \brief linear solver using dense cholesky decomposition 44 | */ 45 | template 46 | class LinearSolverDense : public LinearSolver 47 | { 48 | public: 49 | LinearSolverDense() : 50 | LinearSolver(), 51 | _reset(true) 52 | { 53 | } 54 | 55 | virtual ~LinearSolverDense() 56 | { 57 | } 58 | 59 | virtual bool init() 60 | { 61 | _reset = true; 62 | return true; 63 | } 64 | 65 | bool solve(const SparseBlockMatrix& A, double* x, double* b) 66 | { 67 | int n = A.cols(); 68 | int m = A.cols(); 69 | 70 | Eigen::MatrixXd& H = _H; 71 | if (H.cols() != n) { 72 | H.resize(n, m); 73 | _reset = true; 74 | } 75 | if (_reset) { 76 | _reset = false; 77 | H.setZero(); 78 | } 79 | 80 | // copy the sparse block matrix into a dense matrix 81 | int c_idx = 0; 82 | for (size_t i = 0; i < A.blockCols().size(); ++i) { 83 | int c_size = A.colsOfBlock(i); 84 | assert(c_idx == A.colBaseOfBlock(i) && "mismatch in block indices"); 85 | 86 | const typename SparseBlockMatrix::IntBlockMap& col = A.blockCols()[i]; 87 | if (col.size() > 0) { 88 | typename SparseBlockMatrix::IntBlockMap::const_iterator it; 89 | for (it = col.begin(); it != col.end(); ++it) { 90 | int r_idx = A.rowBaseOfBlock(it->first); 91 | // only the upper triangular block is processed 92 | if (it->first <= (int)i) { 93 | int r_size = A.rowsOfBlock(it->first); 94 | H.block(r_idx, c_idx, r_size, c_size) = *(it->second); 95 | if (r_idx != c_idx) // write the lower triangular block 96 | H.block(c_idx, r_idx, c_size, r_size) = it->second->transpose(); 97 | } 98 | } 99 | } 100 | 101 | c_idx += c_size; 102 | } 103 | 104 | // solving via Cholesky decomposition 105 | Eigen::VectorXd::MapType xvec(x, m); 106 | Eigen::VectorXd::ConstMapType bvec(b, n); 107 | _cholesky.compute(H); 108 | if (_cholesky.isPositive()) { 109 | xvec = _cholesky.solve(bvec); 110 | return true; 111 | } 112 | return false; 113 | } 114 | 115 | protected: 116 | bool _reset; 117 | Eigen::MatrixXd _H; 118 | Eigen::LDLT _cholesky; 119 | 120 | }; 121 | 122 | 123 | }// end namespace 124 | 125 | #endif 126 | -------------------------------------------------------------------------------- /g2o/g2o/stuff/color_macros.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_COLOR_MACROS_H 28 | #define G2O_COLOR_MACROS_H 29 | 30 | // font attributes 31 | #define FT_BOLD "\033[1m" 32 | #define FT_UNDERLINE "\033[4m" 33 | 34 | //background color 35 | #define BG_BLACK "\033[40m" 36 | #define BG_RED "\033[41m" 37 | #define BG_GREEN "\033[42m" 38 | #define BG_YELLOW "\033[43m" 39 | #define BG_LIGHTBLUE "\033[44m" 40 | #define BG_MAGENTA "\033[45m" 41 | #define BG_BLUE "\033[46m" 42 | #define BG_WHITE "\033[47m" 43 | 44 | // font color 45 | #define CL_BLACK(s) "\033[30m" << s << "\033[0m" 46 | #define CL_RED(s) "\033[31m" << s << "\033[0m" 47 | #define CL_GREEN(s) "\033[32m" << s << "\033[0m" 48 | #define CL_YELLOW(s) "\033[33m" << s << "\033[0m" 49 | #define CL_LIGHTBLUE(s) "\033[34m" << s << "\033[0m" 50 | #define CL_MAGENTA(s) "\033[35m" << s << "\033[0m" 51 | #define CL_BLUE(s) "\033[36m" << s << "\033[0m" 52 | #define CL_WHITE(s) "\033[37m" << s << "\033[0m" 53 | 54 | #define FG_BLACK "\033[30m" 55 | #define FG_RED "\033[31m" 56 | #define FG_GREEN "\033[32m" 57 | #define FG_YELLOW "\033[33m" 58 | #define FG_LIGHTBLUE "\033[34m" 59 | #define FG_MAGENTA "\033[35m" 60 | #define FG_BLUE "\033[36m" 61 | #define FG_WHITE "\033[37m" 62 | 63 | #define FG_NORM "\033[0m" 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /g2o/g2o/stuff/macros.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_MACROS_H 28 | #define G2O_MACROS_H 29 | 30 | #ifndef DEG2RAD 31 | #define DEG2RAD(x) ((x) * 0.01745329251994329575) 32 | #endif 33 | 34 | #ifndef RAD2DEG 35 | #define RAD2DEG(x) ((x) * 57.29577951308232087721) 36 | #endif 37 | 38 | // GCC the one and only 39 | #if defined(__GNUC__) 40 | # define G2O_ATTRIBUTE_CONSTRUCTOR(func) \ 41 | static void func(void)__attribute__ ((constructor)); \ 42 | static void func(void) 43 | 44 | # define G2O_ATTRIBUTE_UNUSED __attribute__((unused)) 45 | # define G2O_ATTRIBUTE_FORMAT12 __attribute__ ((format (printf, 1, 2))) 46 | # define G2O_ATTRIBUTE_FORMAT23 __attribute__ ((format (printf, 2, 3))) 47 | # define G2O_ATTRIBUTE_WARNING(func) func __attribute__((warning)) 48 | # define G2O_ATTRIBUTE_DEPRECATED(func) func __attribute__((deprecated)) 49 | 50 | #ifdef ANDROID 51 | # define g2o_isnan(x) isnan(x) 52 | # define g2o_isinf(x) isinf(x) 53 | # define g2o_isfinite(x) isfinite(x) 54 | #else 55 | # define g2o_isnan(x) std::isnan(x) 56 | # define g2o_isinf(x) std::isinf(x) 57 | # define g2o_isfinite(x) std::isfinite(x) 58 | #endif 59 | 60 | // MSVC on Windows 61 | #elif defined _MSC_VER 62 | # define __PRETTY_FUNCTION__ __FUNCTION__ 63 | 64 | /** 65 | Modified by Mark Pupilli from: 66 | 67 | "Initializer/finalizer sample for MSVC and GCC. 68 | 2010 Joe Lowe. Released into the public domain." 69 | 70 | "For MSVC, places a ptr to the function in the user initializer section (.CRT$XCU), basically the same thing the compiler does for the constructor calls for static C++ objects. For GCC, uses a constructor attribute." 71 | 72 | (As posted on Stack OVerflow) 73 | */ 74 | # define G2O_ATTRIBUTE_CONSTRUCTOR(f) \ 75 | __pragma(section(".CRT$XCU",read)) \ 76 | static void __cdecl f(void); \ 77 | __declspec(allocate(".CRT$XCU")) void (__cdecl*f##_)(void) = f; \ 78 | static void __cdecl f(void) 79 | 80 | # define G2O_ATTRIBUTE_UNUSED 81 | # define G2O_ATTRIBUTE_FORMAT12 82 | # define G2O_ATTRIBUTE_FORMAT23 83 | # define G2O_ATTRIBUTE_WARNING(func) func 84 | # define G2O_ATTRIBUTE_DEPRECATED(func) func 85 | 86 | #include 87 | 88 | # define g2o_isnan(x) _isnan(x) 89 | # define g2o_isinf(x) (_finite(x) == 0) 90 | # define g2o_isfinite(x) (_finite(x) != 0) 91 | 92 | // unknown compiler 93 | #else 94 | # ifndef __PRETTY_FUNCTION__ 95 | # define __PRETTY_FUNCTION__ "" 96 | # endif 97 | # define G2O_ATTRIBUTE_CONSTRUCTOR(func) func 98 | # define G2O_ATTRIBUTE_UNUSED 99 | # define G2O_ATTRIBUTE_FORMAT12 100 | # define G2O_ATTRIBUTE_FORMAT23 101 | # define G2O_ATTRIBUTE_WARNING(func) func 102 | # define G2O_ATTRIBUTE_DEPRECATED(func) func 103 | 104 | #include 105 | #define g2o_isnan(x) isnan(x) 106 | #define g2o_isinf(x) isinf(x) 107 | #define g2o_isfinite(x) isfinite(x) 108 | 109 | #endif 110 | 111 | // some macros that are only useful for c++ 112 | #ifdef __cplusplus 113 | 114 | #define G2O_FSKIP_LINE(f) \ 115 | {char c=' ';while(c != '\n' && f.good() && !(f).eof()) (f).get(c);} 116 | 117 | #ifndef PVAR 118 | #define PVAR(s) \ 119 | #s << " = " << (s) << std::flush 120 | #endif 121 | 122 | #ifndef PVARA 123 | #define PVARA(s) \ 124 | #s << " = " << RAD2DEG(s) << "deg" << std::flush 125 | #endif 126 | 127 | #ifndef FIXED 128 | #define FIXED(s) \ 129 | std::fixed << s << std::resetiosflags(std::ios_base::fixed) 130 | #endif 131 | 132 | #endif // __cplusplus 133 | 134 | #endif 135 | -------------------------------------------------------------------------------- /g2o/g2o/stuff/misc.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_STUFF_MISC_H 28 | #define G2O_STUFF_MISC_H 29 | 30 | #include "macros.h" 31 | #include 32 | 33 | #ifndef M_PI 34 | #define M_PI 3.14159265358979323846 35 | #endif 36 | 37 | /** @addtogroup utils **/ 38 | // @{ 39 | 40 | /** \file misc.h 41 | * \brief some general case utility functions 42 | * 43 | * This file specifies some general case utility functions 44 | **/ 45 | 46 | namespace g2o { 47 | 48 | /** 49 | * return the square value 50 | */ 51 | template 52 | inline T square(T x) 53 | { 54 | return x*x; 55 | } 56 | 57 | /** 58 | * return the hypot of x and y 59 | */ 60 | template 61 | inline T hypot(T x, T y) 62 | { 63 | return (T) (sqrt(x*x + y*y)); 64 | } 65 | 66 | /** 67 | * return the squared hypot of x and y 68 | */ 69 | template 70 | inline T hypot_sqr(T x, T y) 71 | { 72 | return x*x + y*y; 73 | } 74 | 75 | /** 76 | * convert from degree to radian 77 | */ 78 | inline double deg2rad(double degree) 79 | { 80 | return degree * 0.01745329251994329576; 81 | } 82 | 83 | /** 84 | * convert from radian to degree 85 | */ 86 | inline double rad2deg(double rad) 87 | { 88 | return rad * 57.29577951308232087721; 89 | } 90 | 91 | /** 92 | * normalize the angle 93 | */ 94 | inline double normalize_theta(double theta) 95 | { 96 | if (theta >= -M_PI && theta < M_PI) 97 | return theta; 98 | 99 | double multiplier = floor(theta / (2*M_PI)); 100 | theta = theta - multiplier*2*M_PI; 101 | if (theta >= M_PI) 102 | theta -= 2*M_PI; 103 | if (theta < -M_PI) 104 | theta += 2*M_PI; 105 | 106 | return theta; 107 | } 108 | 109 | /** 110 | * inverse of an angle, i.e., +180 degree 111 | */ 112 | inline double inverse_theta(double th) 113 | { 114 | return normalize_theta(th + M_PI); 115 | } 116 | 117 | /** 118 | * average two angles 119 | */ 120 | inline double average_angle(double theta1, double theta2) 121 | { 122 | double x, y; 123 | 124 | x = cos(theta1) + cos(theta2); 125 | y = sin(theta1) + sin(theta2); 126 | if(x == 0 && y == 0) 127 | return 0; 128 | else 129 | return std::atan2(y, x); 130 | } 131 | 132 | /** 133 | * sign function. 134 | * @return the sign of x. +1 for x > 0, -1 for x < 0, 0 for x == 0 135 | */ 136 | template 137 | inline int sign(T x) 138 | { 139 | if (x > 0) 140 | return 1; 141 | else if (x < 0) 142 | return -1; 143 | else 144 | return 0; 145 | } 146 | 147 | /** 148 | * clamp x to the interval [l, u] 149 | */ 150 | template 151 | inline T clamp(T l, T x, T u) 152 | { 153 | if (x < l) 154 | return l; 155 | if (x > u) 156 | return u; 157 | return x; 158 | } 159 | 160 | /** 161 | * wrap x to be in the interval [l, u] 162 | */ 163 | template 164 | inline T wrap(T l, T x, T u) 165 | { 166 | T intervalWidth = u - l; 167 | while (x < l) 168 | x += intervalWidth; 169 | while (x > u) 170 | x -= intervalWidth; 171 | return x; 172 | } 173 | 174 | /** 175 | * tests whether there is a NaN in the array 176 | */ 177 | inline bool arrayHasNaN(const double* array, int size, int* nanIndex = 0) 178 | { 179 | for (int i = 0; i < size; ++i) 180 | if (g2o_isnan(array[i])) { 181 | if (nanIndex) 182 | *nanIndex = i; 183 | return true; 184 | } 185 | return false; 186 | } 187 | 188 | /** 189 | * The following two functions are used to force linkage with static libraries. 190 | */ 191 | extern "C" 192 | { 193 | typedef void (* ForceLinkFunction) (void); 194 | } 195 | 196 | struct ForceLinker 197 | { 198 | ForceLinker(ForceLinkFunction function) { (function)(); } 199 | }; 200 | 201 | 202 | } // end namespace 203 | 204 | // @} 205 | 206 | #endif 207 | -------------------------------------------------------------------------------- /g2o/g2o/stuff/os_specific.c: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "os_specific.h" 28 | 29 | #ifdef WINDOWS 30 | 31 | int vasprintf(char** strp, const char* fmt, va_list ap) 32 | { 33 | int n; 34 | int size = 100; 35 | char* p; 36 | char* np; 37 | 38 | if ((p = (char*)malloc(size * sizeof(char))) == NULL) 39 | return -1; 40 | 41 | while (1) { 42 | #ifdef _MSC_VER 43 | n = vsnprintf_s(p, size, size - 1, fmt, ap); 44 | #else 45 | n = vsnprintf(p, size, fmt, ap); 46 | #endif 47 | if (n > -1 && n < size) { 48 | *strp = p; 49 | return n; 50 | } 51 | if (n > -1) 52 | size = n+1; 53 | else 54 | size *= 2; 55 | if ((np = (char*)realloc (p, size * sizeof(char))) == NULL) { 56 | free(p); 57 | return -1; 58 | } else 59 | p = np; 60 | } 61 | } 62 | 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /g2o/g2o/stuff/os_specific.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OS_SPECIFIC_HH_ 28 | #define G2O_OS_SPECIFIC_HH_ 29 | 30 | #ifdef WINDOWS 31 | #include 32 | #include 33 | #include 34 | #ifndef _WINDOWS 35 | #include 36 | #endif 37 | #define drand48() ((double) rand()/(double)RAND_MAX) 38 | 39 | #ifdef __cplusplus 40 | extern "C" { 41 | #endif 42 | 43 | int vasprintf(char** strp, const char* fmt, va_list ap); 44 | 45 | #ifdef __cplusplus 46 | } 47 | #endif 48 | 49 | #endif 50 | 51 | #ifdef UNIX 52 | #include 53 | // nothing to do on real operating systems 54 | #endif 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /g2o/g2o/stuff/property.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "property.h" 28 | 29 | #include 30 | #include 31 | 32 | #include "macros.h" 33 | 34 | #include "string_tools.h" 35 | using namespace std; 36 | 37 | namespace g2o { 38 | 39 | BaseProperty::BaseProperty(const std::string name_) :_name(name_){ 40 | } 41 | 42 | BaseProperty::~BaseProperty(){} 43 | 44 | bool PropertyMap::addProperty(BaseProperty* p) { 45 | std::pair result = insert(make_pair(p->name(), p)); 46 | return result.second; 47 | } 48 | 49 | bool PropertyMap::eraseProperty(const std::string& name) { 50 | PropertyMapIterator it=find(name); 51 | if (it==end()) 52 | return false; 53 | delete it->second; 54 | erase(it); 55 | return true; 56 | } 57 | 58 | PropertyMap::~PropertyMap() { 59 | for (PropertyMapIterator it=begin(); it!=end(); it++){ 60 | if (it->second) 61 | delete it->second; 62 | } 63 | } 64 | 65 | bool PropertyMap::updatePropertyFromString(const std::string& name, const std::string& value) 66 | { 67 | PropertyMapIterator it = find(name); 68 | if (it == end()) 69 | return false; 70 | it->second->fromString(value); 71 | return true; 72 | } 73 | 74 | void PropertyMap::writeToCSV(std::ostream& os) const 75 | { 76 | for (PropertyMapConstIterator it=begin(); it!=end(); it++){ 77 | BaseProperty* p =it->second; 78 | os << p->name() << ", "; 79 | } 80 | os << std::endl; 81 | for (PropertyMapConstIterator it=begin(); it!=end(); it++){ 82 | BaseProperty* p =it->second; 83 | os << p->toString() << ", "; 84 | } 85 | os << std::endl; 86 | } 87 | 88 | bool PropertyMap::updateMapFromString(const std::string& values) 89 | { 90 | bool status = true; 91 | vector valuesMap = strSplit(values, ","); 92 | for (size_t i = 0; i < valuesMap.size(); ++i) { 93 | vector m = strSplit(valuesMap[i], "="); 94 | if (m.size() != 2) { 95 | cerr << __PRETTY_FUNCTION__ << ": unable to extract name=value pair from " << valuesMap[i] << endl; 96 | continue; 97 | } 98 | string name = trim(m[0]); 99 | string value = trim(m[1]); 100 | status = status && updatePropertyFromString(name, value); 101 | } 102 | return status; 103 | } 104 | 105 | } // end namespace 106 | -------------------------------------------------------------------------------- /g2o/g2o/stuff/timeutil.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "timeutil.h" 28 | #include 29 | 30 | #ifdef _WINDOWS 31 | #include 32 | #include 33 | #endif 34 | 35 | #ifdef UNIX 36 | #include 37 | #endif 38 | 39 | namespace g2o { 40 | 41 | #ifdef _WINDOWS 42 | #if defined(_MSC_VER) || defined(_MSC_EXTENSIONS) 43 | #define DELTA_EPOCH_IN_MICROSECS 11644473600000000Ui64 44 | #else 45 | #define DELTA_EPOCH_IN_MICROSECS 11644473600000000ULL 46 | #endif 47 | 48 | struct timezone 49 | { 50 | int tz_minuteswest; /* minutes W of Greenwich */ 51 | int tz_dsttime; /* type of dst correction */ 52 | }; 53 | 54 | int gettimeofday(struct timeval *tv, struct timezone *tz) 55 | { 56 | // Define a structure to receive the current Windows filetime 57 | FILETIME ft; 58 | 59 | // Initialize the present time to 0 and the timezone to UTC 60 | unsigned __int64 tmpres = 0; 61 | static int tzflag = 0; 62 | 63 | if (NULL != tv) 64 | { 65 | GetSystemTimeAsFileTime(&ft); 66 | 67 | // The GetSystemTimeAsFileTime returns the number of 100 nanosecond 68 | // intervals since Jan 1, 1601 in a structure. Copy the high bits to 69 | // the 64 bit tmpres, shift it left by 32 then or in the low 32 bits. 70 | tmpres |= ft.dwHighDateTime; 71 | tmpres <<= 32; 72 | tmpres |= ft.dwLowDateTime; 73 | 74 | // Convert to microseconds by dividing by 10 75 | tmpres /= 10; 76 | 77 | // The Unix epoch starts on Jan 1 1970. Need to subtract the difference 78 | // in seconds from Jan 1 1601. 79 | tmpres -= DELTA_EPOCH_IN_MICROSECS; 80 | 81 | // Finally change microseconds to seconds and place in the seconds value. 82 | // The modulus picks up the microseconds. 83 | tv->tv_sec = (long)(tmpres / 1000000UL); 84 | tv->tv_usec = (long)(tmpres % 1000000UL); 85 | } 86 | 87 | if (NULL != tz) { 88 | if (!tzflag) { 89 | _tzset(); 90 | tzflag++; 91 | } 92 | 93 | long sec; 94 | int hours; 95 | _get_timezone(&sec); 96 | _get_daylight(&hours); 97 | 98 | // Adjust for the timezone west of Greenwich 99 | tz->tz_minuteswest = sec / 60; 100 | tz->tz_dsttime = hours; 101 | } 102 | 103 | return 0; 104 | } 105 | #endif 106 | 107 | ScopeTime::ScopeTime(const char* title) : _title(title), _startTime(get_monotonic_time()) {} 108 | 109 | ScopeTime::~ScopeTime() { 110 | std::cerr << _title<<" took "<<1000*(get_monotonic_time()-_startTime)<<"ms.\n"; 111 | } 112 | 113 | double get_monotonic_time() 114 | { 115 | #if (defined(_POSIX_TIMERS) && (_POSIX_TIMERS+0 >= 0) && defined(_POSIX_MONOTONIC_CLOCK)) 116 | struct timespec ts; 117 | clock_gettime(CLOCK_MONOTONIC, &ts); 118 | return ts.tv_sec + ts.tv_nsec*1e-9; 119 | #else 120 | return get_time(); 121 | #endif 122 | } 123 | 124 | } // end namespace 125 | -------------------------------------------------------------------------------- /g2o/g2o/stuff/timeutil.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_TIMEUTIL_H 28 | #define G2O_TIMEUTIL_H 29 | 30 | #ifdef _WINDOWS 31 | #include 32 | #else 33 | #include 34 | #endif 35 | 36 | #include 37 | 38 | 39 | /** @addtogroup utils **/ 40 | // @{ 41 | 42 | /** \file timeutil.h 43 | * \brief utility functions for handling time related stuff 44 | */ 45 | 46 | /// Executes code, only if secs are gone since last exec. 47 | /// extended version, in which the current time is given, e.g., timestamp of IPC message 48 | #ifndef DO_EVERY_TS 49 | #define DO_EVERY_TS(secs, currentTime, code) \ 50 | if (1) {\ 51 | static double s_lastDone_ = (currentTime); \ 52 | double s_now_ = (currentTime); \ 53 | if (s_lastDone_ > s_now_) \ 54 | s_lastDone_ = s_now_; \ 55 | if (s_now_ - s_lastDone_ > (secs)) { \ 56 | code; \ 57 | s_lastDone_ = s_now_; \ 58 | }\ 59 | } else \ 60 | (void)0 61 | #endif 62 | 63 | /// Executes code, only if secs are gone since last exec. 64 | #ifndef DO_EVERY 65 | #define DO_EVERY(secs, code) DO_EVERY_TS(secs, g2o::get_time(), code) 66 | #endif 67 | 68 | #ifndef MEASURE_TIME 69 | #define MEASURE_TIME(text, code) \ 70 | if(1) { \ 71 | double _start_time_ = g2o::get_time(); \ 72 | code; \ 73 | fprintf(stderr, "%s took %f sec\n", text, g2o::get_time() - _start_time_); \ 74 | } else \ 75 | (void) 0 76 | #endif 77 | 78 | namespace g2o { 79 | 80 | #ifdef _WINDOWS 81 | typedef struct timeval { 82 | long tv_sec; 83 | long tv_usec; 84 | } timeval; 85 | int gettimeofday(struct timeval *tv, struct timezone *tz); 86 | #endif 87 | 88 | /** 89 | * return the current time in seconds since 1. Jan 1970 90 | */ 91 | inline double get_time() 92 | { 93 | struct timeval ts; 94 | gettimeofday(&ts,0); 95 | return ts.tv_sec + ts.tv_usec*1e-6; 96 | } 97 | 98 | /** 99 | * return a monotonic increasing time which basically does not need to 100 | * have a reference point. Consider this for measuring how long some 101 | * code fragments required to execute. 102 | * 103 | * On Linux we call clock_gettime() on other systems we currently 104 | * call get_time(). 105 | */ 106 | double get_monotonic_time(); 107 | 108 | /** 109 | * \brief Class to measure the time spent in a scope 110 | * 111 | * To use this class, e.g. to measure the time spent in a function, 112 | * just create and instance at the beginning of the function. 113 | */ 114 | class ScopeTime { 115 | public: 116 | ScopeTime(const char* title); 117 | ~ScopeTime(); 118 | private: 119 | std::string _title; 120 | double _startTime; 121 | }; 122 | 123 | } // end namespace 124 | 125 | #ifndef MEASURE_FUNCTION_TIME 126 | #define MEASURE_FUNCTION_TIME \ 127 | g2o::ScopeTime scopeTime(__PRETTY_FUNCTION__) 128 | #endif 129 | 130 | 131 | // @} 132 | #endif 133 | -------------------------------------------------------------------------------- /g2o/g2o/types/se3_ops.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 H. Strasdat 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_MATH_STUFF 28 | #define G2O_MATH_STUFF 29 | 30 | #include 31 | #include 32 | 33 | namespace g2o { 34 | using namespace Eigen; 35 | 36 | inline Matrix3d skew(const Vector3d&v); 37 | inline Vector3d deltaR(const Matrix3d& R); 38 | inline Vector2d project(const Vector3d&); 39 | inline Vector3d project(const Vector4d&); 40 | inline Vector3d unproject(const Vector2d&); 41 | inline Vector4d unproject(const Vector3d&); 42 | 43 | #include "se3_ops.hpp" 44 | 45 | } 46 | 47 | #endif //MATH_STUFF 48 | -------------------------------------------------------------------------------- /g2o/g2o/types/se3_ops.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 H. Strasdat 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | Matrix3d skew(const Vector3d&v) 28 | { 29 | Matrix3d m; 30 | m.fill(0.); 31 | m(0,1) = -v(2); 32 | m(0,2) = v(1); 33 | m(1,2) = -v(0); 34 | m(1,0) = v(2); 35 | m(2,0) = -v(1); 36 | m(2,1) = v(0); 37 | return m; 38 | } 39 | 40 | Vector3d deltaR(const Matrix3d& R) 41 | { 42 | Vector3d v; 43 | v(0)=R(2,1)-R(1,2); 44 | v(1)=R(0,2)-R(2,0); 45 | v(2)=R(1,0)-R(0,1); 46 | return v; 47 | } 48 | 49 | Vector2d project(const Vector3d& v) 50 | { 51 | Vector2d res; 52 | res(0) = v(0)/v(2); 53 | res(1) = v(1)/v(2); 54 | return res; 55 | } 56 | 57 | Vector3d project(const Vector4d& v) 58 | { 59 | Vector3d res; 60 | res(0) = v(0)/v(3); 61 | res(1) = v(1)/v(3); 62 | res(2) = v(2)/v(3); 63 | return res; 64 | } 65 | 66 | Vector3d unproject(const Vector2d& v) 67 | { 68 | Vector3d res; 69 | res(0) = v(0); 70 | res(1) = v(1); 71 | res(2) = 1; 72 | return res; 73 | } 74 | 75 | Vector4d unproject(const Vector3d& v) 76 | { 77 | Vector4d res; 78 | res(0) = v(0); 79 | res(1) = v(1); 80 | res(2) = v(2); 81 | res(3) = 1; 82 | return res; 83 | } 84 | 85 | 86 | -------------------------------------------------------------------------------- /g2o/g2o/types/types_sba.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 Kurt Konolige 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "types_sba.h" 28 | #include 29 | 30 | namespace g2o { 31 | 32 | using namespace std; 33 | 34 | 35 | VertexSBAPointXYZ::VertexSBAPointXYZ() : BaseVertex<3, Vector3d>() 36 | { 37 | } 38 | 39 | bool VertexSBAPointXYZ::read(std::istream& is) 40 | { 41 | Vector3d lv; 42 | for (int i=0; i<3; i++) 43 | is >> _estimate[i]; 44 | return true; 45 | } 46 | 47 | bool VertexSBAPointXYZ::write(std::ostream& os) const 48 | { 49 | Vector3d lv=estimate(); 50 | for (int i=0; i<3; i++){ 51 | os << lv[i] << " "; 52 | } 53 | return os.good(); 54 | } 55 | 56 | } // end namespace 57 | -------------------------------------------------------------------------------- /g2o/g2o/types/types_sba.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 Kurt Konolige 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_SBA_TYPES 28 | #define G2O_SBA_TYPES 29 | 30 | #include "../core/base_vertex.h" 31 | 32 | #include 33 | #include 34 | 35 | namespace g2o { 36 | 37 | /** 38 | * \brief Point vertex, XYZ 39 | */ 40 | class VertexSBAPointXYZ : public BaseVertex<3, Vector3d> 41 | { 42 | public: 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 44 | VertexSBAPointXYZ(); 45 | virtual bool read(std::istream& is); 46 | virtual bool write(std::ostream& os) const; 47 | 48 | virtual void setToOriginImpl() { 49 | _estimate.fill(0.); 50 | } 51 | 52 | virtual void oplusImpl(const double* update) 53 | { 54 | Eigen::Map v(update); 55 | _estimate += v; 56 | } 57 | }; 58 | 59 | } // end namespace 60 | 61 | #endif // SBA_TYPES 62 | -------------------------------------------------------------------------------- /g2o/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 | -------------------------------------------------------------------------------- /run_test_eth_cvg.sh: -------------------------------------------------------------------------------- 1 | ./bin/NID_pose_estimation config_eth_cvg.yaml --------------------------------------------------------------------------------