├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake └── FindSuiteParse.cmake ├── package.xml ├── src ├── FullSystem │ ├── CoarseInitializer.cpp │ ├── CoarseInitializer.h │ ├── CoarseTracker.cpp │ ├── CoarseTracker.h │ ├── FullSystem.cpp │ ├── FullSystem.h │ ├── FullSystemDebugStuff.cpp │ ├── FullSystemMarginalize.cpp │ ├── FullSystemOptPoint.cpp │ ├── FullSystemOptimize.cpp │ ├── HessianBlocks.cpp │ ├── HessianBlocks.h │ ├── ImmaturePoint.cpp │ ├── ImmaturePoint.h │ ├── PixelSelector.h │ ├── PixelSelector2.cpp │ ├── PixelSelector2.h │ ├── ResidualProjections.h │ ├── Residuals.cpp │ ├── Residuals.h │ ├── ScaleOptimizer.cpp │ └── ScaleOptimizer.h ├── IOWrapper │ ├── ImageDisplay.h │ ├── ImageDisplay_dummy.cpp │ ├── ImageRW.h │ ├── ImageRW_dummy.cpp │ ├── OpenCV │ │ ├── ImageDisplay_OpenCV.cpp │ │ └── ImageRW_OpenCV.cpp │ ├── Output3DWrapper.h │ └── Pangolin │ │ ├── KeyFrameDisplay.cpp │ │ ├── KeyFrameDisplay.h │ │ ├── PangolinSOSVIOViewer.cpp │ │ └── PangolinSOSVIOViewer.h ├── LoopClosure │ ├── LoopHandler.cpp │ ├── LoopHandler.h │ ├── PoseEstimator.cpp │ ├── PoseEstimator.h │ ├── ScanContext.cpp │ └── ScanContext.h ├── OptimizationBackend │ ├── AccumulatedSCHessian.cpp │ ├── AccumulatedSCHessian.h │ ├── AccumulatedTopHessian.cpp │ ├── AccumulatedTopHessian.h │ ├── EnergyFunctional.cpp │ ├── EnergyFunctional.h │ ├── EnergyFunctionalStructs.cpp │ ├── EnergyFunctionalStructs.h │ ├── MatrixAccumulators.h │ ├── RawResidualJacobian.h │ └── ScaleAccumulator.h ├── SlamNode.cpp ├── SlamNode.h ├── main.cpp └── util │ ├── FrameShell.h │ ├── ImageAndExposure.h │ ├── IndexThreadReduce.h │ ├── MinimalImage.h │ ├── NumType.h │ ├── Undistort.cpp │ ├── Undistort.h │ ├── globalCalib.cpp │ ├── globalCalib.h │ ├── globalFuncs.h │ ├── nanoflann.h │ ├── settings.cpp │ └── settings.h ├── tests ├── EuRoC │ ├── calib.yaml │ ├── camera0.txt │ ├── camera1.txt │ └── euroc.launch ├── KITTI │ ├── 0_2 │ │ ├── calib.yaml │ │ ├── camera0.txt │ │ └── camera1.txt │ ├── 4_12 │ │ ├── calib.yaml │ │ ├── camera0.txt │ │ └── camera1.txt │ └── kitti.launch ├── Malaga │ ├── calib.yaml │ ├── camera0.txt │ ├── camera1.txt │ └── malaga.launch ├── RobotCar │ ├── calib.yaml │ ├── camera0.txt │ ├── camera1.txt │ └── robotcar.launch └── TUMVI │ ├── calib.yaml │ ├── camera0.txt │ ├── camera1.txt │ ├── pcalib0.txt │ ├── pcalib1.txt │ ├── tumvi.launch │ ├── vignette0.png │ └── vignette1.png └── thirdparty ├── Sophus ├── CMakeLists.txt ├── FindEigen3.cmake ├── README ├── SophusConfig.cmake.in ├── cmake_modules │ └── FindEigen3.cmake └── sophus │ ├── rxso3.hpp │ ├── se2.hpp │ ├── se3.hpp │ ├── sim3.hpp │ ├── so2.hpp │ ├── so3.hpp │ ├── sophus.hpp │ ├── test_rxso3.cpp │ ├── test_se2.cpp │ ├── test_se3.cpp │ ├── test_sim3.cpp │ ├── test_so2.cpp │ ├── test_so3.cpp │ └── tests.hpp └── g2o ├── CMakeLists.txt ├── README.md ├── build.sh ├── cmake_modules ├── FindBLAS.cmake ├── FindEigen3.cmake └── FindLAPACK.cmake ├── config.h.in └── g2o ├── config.h ├── core ├── CMakeLists.txt ├── base_binary_edge.h ├── base_binary_edge.hpp ├── base_dynamic_vertex.h ├── base_edge.h ├── base_multi_edge.h ├── base_multi_edge.hpp ├── base_unary_edge.h ├── base_unary_edge.hpp ├── base_vertex.h ├── base_vertex.hpp ├── batch_stats.cpp ├── batch_stats.h ├── block_solver.h ├── block_solver.hpp ├── cache.cpp ├── cache.h ├── creators.h ├── dynamic_aligned_buffer.hpp ├── eigen_types.h ├── estimate_propagator.cpp ├── estimate_propagator.h ├── factory.cpp ├── factory.h ├── g2o_core_api.h ├── hyper_dijkstra.cpp ├── hyper_dijkstra.h ├── hyper_graph.cpp ├── hyper_graph.h ├── hyper_graph_action.cpp ├── hyper_graph_action.h ├── io_helper.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 ├── ownership.h ├── parameter.cpp ├── parameter.h ├── parameter_container.cpp ├── parameter_container.h ├── robust_kernel.cpp ├── robust_kernel.h ├── robust_kernel_factory.cpp ├── robust_kernel_factory.h ├── robust_kernel_impl.cpp ├── robust_kernel_impl.h ├── solver.cpp ├── solver.h ├── sparse_block_matrix.h ├── sparse_block_matrix.hpp ├── sparse_block_matrix_ccs.h ├── sparse_block_matrix_diagonal.h ├── sparse_block_matrix_test.cpp ├── sparse_optimizer.cpp ├── sparse_optimizer.h ├── sparse_optimizer_terminate_action.cpp └── sparse_optimizer_terminate_action.h ├── solvers └── eigen │ ├── CMakeLists.txt │ ├── linear_solver_eigen.h │ └── solver_eigen.cpp ├── stuff ├── CMakeLists.txt ├── color_macros.h ├── command_args.cpp ├── command_args.h ├── filesys_tools.cpp ├── filesys_tools.h ├── g2o_stuff_api.h ├── macros.h ├── misc.h ├── opengl_primitives.cpp ├── opengl_primitives.h ├── opengl_wrapper.h ├── os_specific.c ├── os_specific.h ├── property.cpp ├── property.h ├── sampler.cpp ├── sampler.h ├── scoped_pointer.h ├── sparse_helper.cpp ├── sparse_helper.h ├── string_tools.cpp ├── string_tools.h ├── tictoc.cpp ├── tictoc.h ├── timeutil.cpp ├── timeutil.h └── unscented.h └── types └── slam3d ├── CMakeLists.txt ├── dquat2mat.cpp ├── dquat2mat.h ├── dquat2mat_maxima_generated.cpp ├── edge_se3.cpp ├── edge_se3.h ├── g2o_types_slam3d_api.h ├── isometry3d_gradients.cpp ├── isometry3d_gradients.h ├── isometry3d_mappings.cpp ├── isometry3d_mappings.h ├── se3_ops.h ├── se3_ops.hpp ├── se3quat.h ├── types_slam3d.cpp ├── types_slam3d.h ├── vertex_se3.cpp └── vertex_se3.h /.gitignore: -------------------------------------------------------------------------------- 1 | thirdparty/g2o/build 2 | thirdparty/g2o/lib 3 | 4 | tests/Aqua 5 | tests/Gazebo -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | PROJECT(sos_slam) 2 | CMAKE_MINIMUM_REQUIRED(VERSION 2.8.3) 3 | 4 | set(BUILD_TYPE Release) 5 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 6 | 7 | find_package(SuiteParse REQUIRED) 8 | find_package(Eigen3 REQUIRED) 9 | find_package(Boost COMPONENTS system thread) 10 | find_package(Pangolin 0.2 REQUIRED) 11 | find_package(OpenCV REQUIRED) 12 | find_package(PCL REQUIRED) 13 | find_package(catkin REQUIRED COMPONENTS 14 | geometry_msgs 15 | roscpp 16 | rosbag 17 | sensor_msgs 18 | cv_bridge 19 | image_transport 20 | message_filters 21 | ) 22 | 23 | catkin_package() 24 | 25 | add_definitions("-DENABLE_SSE") 26 | set(CMAKE_CXX_FLAGS "${SSE_FLAGS} -O3 -g -march=native") 27 | 28 | include_directories( 29 | src 30 | thirdparty/Sophus 31 | thirdparty/sse2neon 32 | thirdparty/g2o 33 | ${EIGEN3_INCLUDE_DIR} 34 | ${catkin_INCLUDE_DIRS} 35 | ${Pangolin_INCLUDE_DIRS} 36 | ${OpenCV_INCLUDE_DIRS} 37 | ${PCL_INCLUDE_DIRS} 38 | ${CSPARSE_INCLUDE_DIR} 39 | ${CHOLMOD_INCLUDE_DIR} 40 | ) 41 | 42 | link_directories(${PCL_LIBRARY_DIRS}) 43 | add_definitions(${PCL_DEFINITIONS}) 44 | 45 | add_library(sos_slam_lib 46 | src/SlamNode.cpp 47 | src/FullSystem/FullSystem.cpp 48 | src/FullSystem/FullSystemOptimize.cpp 49 | src/FullSystem/FullSystemOptPoint.cpp 50 | src/FullSystem/FullSystemDebugStuff.cpp 51 | src/FullSystem/FullSystemMarginalize.cpp 52 | src/FullSystem/Residuals.cpp 53 | src/FullSystem/ScaleOptimizer.cpp 54 | src/FullSystem/CoarseInitializer.cpp 55 | src/FullSystem/CoarseTracker.cpp 56 | src/FullSystem/ImmaturePoint.cpp 57 | src/FullSystem/HessianBlocks.cpp 58 | src/FullSystem/PixelSelector2.cpp 59 | src/OptimizationBackend/EnergyFunctional.cpp 60 | src/OptimizationBackend/AccumulatedTopHessian.cpp 61 | src/OptimizationBackend/AccumulatedSCHessian.cpp 62 | src/OptimizationBackend/EnergyFunctionalStructs.cpp 63 | src/util/settings.cpp 64 | src/util/Undistort.cpp 65 | src/util/globalCalib.cpp 66 | src/IOWrapper/Pangolin/KeyFrameDisplay.cpp 67 | src/IOWrapper/Pangolin/PangolinSOSVIOViewer.cpp 68 | src/IOWrapper/OpenCV/ImageDisplay_OpenCV.cpp 69 | src/IOWrapper/OpenCV/ImageRW_OpenCV.cpp 70 | src/LoopClosure/LoopHandler.cpp 71 | src/LoopClosure/ScanContext.cpp 72 | src/LoopClosure/PoseEstimator.cpp 73 | ) 74 | 75 | add_executable(sos_slam_node src/main.cpp) 76 | 77 | target_link_libraries(sos_slam_node 78 | sos_slam_lib 79 | ${catkin_LIBRARIES} 80 | ${BOOST_THREAD_LIBRARY} 81 | ${Pangolin_LIBRARIES} 82 | ${OpenCV_LIBS} 83 | ${PCL_LIBRARIES} 84 | ${PROJECT_SOURCE_DIR}/thirdparty/g2o/lib/libg2o_core.so 85 | ${PROJECT_SOURCE_DIR}/thirdparty/g2o/lib/libg2o_types_slam3d.so 86 | ${PROJECT_SOURCE_DIR}/thirdparty/g2o/lib/libg2o_solver_eigen.so 87 | boost_system boost_thread cxsparse) 88 | 89 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Scale Optimized Spline SLAM (SOS-SLAM) 2 | 3 | ## Related Publications 4 | - **Direct Sparse Odometry**, J. Engel, V. Koltun, D. Cremers, IEEE Transactions on Pattern Analysis and Machine Intelligence, 2018. 5 | - **Extending Monocular Visual Odometry to Stereo Camera Systems by Scale Optimization**, J. Mo and J. Sattar, IEEE/RSJ International Conference on Intelligent Robots and Systems, 2019. 6 | - **A Fast and Robust Place Recognition Approach for Stereo Visual Odometry Using LiDAR Descriptors**, J. Mo and J. Sattar, IEEE/RSJ International Conference on Intelligent Robots and Systems, 2020. 7 | - **Fast Direct Stereo Visual SLAM**, J. Mo, Md Jahidul Islam, and J. Sattar, IEEE Robotics and Automation Letters, 2022. 8 | - **Continuous-Time Spline Visual-Inertial Odometry**, J. Mo and J. Sattar, IEEE International Conference on Robotics and Automation, 2022. 9 | 10 | 11 | ## Installation 12 | 0. Dependencies: [ROS](https://www.ros.org/) (middleware), [DSO dependencies](https://github.com/JakobEngel/dso#21-required-dependencies) ([OpenCV](https://opencv.org/), [Pangolin](https://github.com/stevenlovegrove/Pangolin)), and [PCL](https://pointclouds.org/) (for 3D loop closure). 13 | 14 | 1. Build g2o library (for global pose optimization when loop closure is enabled): 15 | ``` 16 | cd thirdparty/g2o 17 | bash build.sh 18 | ``` 19 | 20 | 2. Build SOS-SLAM: 21 | ``` 22 | cd catkin_ws/src 23 | git clone https://github.com/IRVLab/SOS-SLAM.git 24 | cd .. 25 | catkin_make 26 | ``` 27 | 28 | ## Usage 29 | 1. Preparation (examples of popular datasets are [provided](https://github.com/IRVLab/SOS-SLAM/tree/main/tests)) 30 | - [Calibrate](https://github.com/ethz-asl/kalibr) your stereo visual-inertial rig and convert to [this format](https://github.com/IRVLab/SOS-SLAM/tree/main/tests/EuRoC): 31 | - Refer to [DSO](https://github.com/JakobEngel/dso) for more details of intrisic parameters ([cameraX.txt](https://github.com/IRVLab/SOS-SLAM/blob/main/tests/EuRoC/camera0.txt)). 32 | - Put a small number in [T_cam1_cam0[2,2]](https://github.com/IRVLab/SOS-SLAM/blob/main/tests/EuRoC/calib.yaml#L15) for numerical stability if images are stereo pre-rectified. 33 | - Create a [launch file](https://github.com/IRVLab/SOS-SLAM/blob/main/tests/EuRoC/euroc.launch): 34 | - **scale_opt_thres**: scale optimization accept threshold (e.g., 15.0), set -1 to disable scale optimization. 35 | - **weight_imu_dso**: relative weight between inertial and visual systems (e.g., 6.0), set -1 to ignore IMU measurements. 36 | - **loop_lidar_range**: imitated LiDAR scan range (e.g., 40.0 meters), set -1 to disable place recognition. 37 | - check ROS parameter acquisition (*getParam()* and *param()*) in [main.cpp](https://github.com/IRVLab/SOS-SLAM/blob/main/src/main.cpp) and [settings.cpp](https://github.com/IRVLab/SOS-SLAM/blob/main/src/util/settings.cpp) for more optional parameters. 38 | 39 | 2. Run 40 | ``` 41 | roslaunch sos_slam [YOUR_LAUNCH_FILE] 42 | ``` 43 | 44 | ### Outputs 45 | - Published ROS Topics: 46 | - **pose_cam0_in_world/current**: pose of the most recent frame (not final, minimal lagging). 47 | - **pose_cam0_in_world/marginalized**: pose of the most recently marginalized frame (final, several keyframes behind). 48 | - **poses.txt** will also be written to ~/.ros for pose evaluation once done. -------------------------------------------------------------------------------- /cmake/FindSuiteParse.cmake: -------------------------------------------------------------------------------- 1 | FIND_PATH(CHOLMOD_INCLUDE_DIR NAMES cholmod.h amd.h camd.h 2 | PATHS 3 | ${SUITE_SPARSE_ROOT}/include 4 | /usr/include/suitesparse 5 | /usr/include/ufsparse 6 | /opt/local/include/ufsparse 7 | /usr/local/include/ufsparse 8 | /sw/include/ufsparse 9 | ) 10 | 11 | FIND_LIBRARY(CHOLMOD_LIBRARY NAMES cholmod 12 | PATHS 13 | ${SUITE_SPARSE_ROOT}/lib 14 | /usr/lib 15 | /usr/local/lib 16 | /opt/local/lib 17 | /sw/lib 18 | ) 19 | 20 | FIND_LIBRARY(AMD_LIBRARY NAMES SHARED NAMES amd 21 | PATHS 22 | ${SUITE_SPARSE_ROOT}/lib 23 | /usr/lib 24 | /usr/local/lib 25 | /opt/local/lib 26 | /sw/lib 27 | ) 28 | 29 | FIND_LIBRARY(CAMD_LIBRARY NAMES camd 30 | PATHS 31 | ${SUITE_SPARSE_ROOT}/lib 32 | /usr/lib 33 | /usr/local/lib 34 | /opt/local/lib 35 | /sw/lib 36 | ) 37 | 38 | FIND_LIBRARY(SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig 39 | PATHS 40 | ${SUITE_SPARSE_ROOT}/lib 41 | /usr/lib 42 | /usr/local/lib 43 | /opt/local/lib 44 | /sw/lib 45 | ) 46 | 47 | 48 | # Different platforms seemingly require linking against different sets of libraries 49 | IF(CYGWIN) 50 | FIND_PACKAGE(PkgConfig) 51 | FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd 52 | PATHS 53 | /usr/lib 54 | /usr/local/lib 55 | /opt/local/lib 56 | /sw/lib 57 | ) 58 | PKG_CHECK_MODULES(LAPACK lapack) 59 | 60 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${LAPACK_LIBRARIES}) 61 | 62 | # MacPorts build of the SparseSuite requires linking against extra libraries 63 | 64 | ELSEIF(APPLE) 65 | 66 | FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd 67 | PATHS 68 | /usr/lib 69 | /usr/local/lib 70 | /opt/local/lib 71 | /sw/lib 72 | ) 73 | 74 | FIND_LIBRARY(CCOLAMD_LIBRARY NAMES ccolamd 75 | PATHS 76 | /usr/lib 77 | /usr/local/lib 78 | /opt/local/lib 79 | /sw/lib 80 | ) 81 | 82 | FIND_LIBRARY(METIS_LIBRARY NAMES metis 83 | PATHS 84 | /usr/lib 85 | /usr/local/lib 86 | /opt/local/lib 87 | /sw/lib 88 | ) 89 | 90 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${METIS_LIBRARY} "-framework Accelerate") 91 | ELSE(APPLE) 92 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY}) 93 | ENDIF(CYGWIN) 94 | 95 | IF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 96 | SET(CHOLMOD_FOUND TRUE) 97 | ELSE(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 98 | SET(CHOLMOD_FOUND FALSE) 99 | ENDIF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 100 | 101 | # Look for csparse; note the difference in the directory specifications! 102 | FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h 103 | PATHS 104 | /usr/include/suitesparse 105 | /usr/include 106 | /opt/local/include 107 | /usr/local/include 108 | /sw/include 109 | /usr/include/ufsparse 110 | /opt/local/include/ufsparse 111 | /usr/local/include/ufsparse 112 | /sw/include/ufsparse 113 | ) 114 | 115 | FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse 116 | PATHS 117 | /usr/lib 118 | /usr/local/lib 119 | /opt/local/lib 120 | /sw/lib 121 | ) 122 | 123 | IF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 124 | SET(CSPARSE_FOUND TRUE) 125 | ELSE(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 126 | SET(CSPARSE_FOUND FALSE) 127 | ENDIF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 128 | 129 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sos_slam 4 | 0.0.0 5 | The sos_slam package 6 | 7 | Jiawei Mo 8 | 9 | GPLv3 10 | catkin 11 | gazebo_ros 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/FullSystem/CoarseTracker.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) <2022> 2 | 3 | // This program is free software: you can redistribute it and/or modify 4 | // it under the terms of the GNU General Public License as published by 5 | // the Free Software Foundation, either version 3 of the License, or 6 | // (at your option) any later version. 7 | 8 | // This program is distributed in the hope that it will be useful, 9 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | // GNU General Public License for more details. 12 | 13 | // You should have received a copy of the GNU General Public License 14 | // along with this program. If not, see . 15 | 16 | // This file is modified from 17 | 18 | #pragma once 19 | 20 | #include "ScaleOptimizer.h" 21 | 22 | namespace dso { 23 | class CoarseTracker : public ScaleOptimizer { 24 | public: 25 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 26 | 27 | CoarseTracker(int w, int h, const std::vector &tfm_vec, 28 | const Mat33f &K1); 29 | 30 | void setCoarseTrackingRef(std::vector frameHessians); 31 | 32 | void scaleCoarseDepthL0(float scale); 33 | 34 | void debugPlotIDepthMap(float *minID, float *maxID, 35 | std::vector &wraps); 36 | void debugPlotIDepthMapFloat(std::vector &wraps); 37 | 38 | bool trackNewestCoarse(FrameHessian *newFrameHessian, SE3 &lastToNew_out, 39 | AffLight &aff_g2l_out, int coarsestLvl, 40 | Vec5 minResForAbort, Vec5 &lastResiduals, 41 | IOWrap::Output3DWrapper *wrap = 0); 42 | 43 | // act as pure ouptut 44 | int refFrameID; 45 | FrameHessian *lastRef; 46 | AffLight lastRef_aff_g2l; 47 | Vec3 lastFlowIndicators; 48 | double firstCoarseRMSE; 49 | 50 | private: 51 | void makeCoarseDepthL0(std::vector frameHessians); 52 | 53 | Vec6 calcResPose(int lvl, const SE3 &refToNew, AffLight aff_g2l, 54 | float cutoffTH, bool plot_img = false); 55 | void calcGSSSEPose(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, 56 | AffLight aff_g2l); 57 | 58 | // warped buffers 59 | float *poseBufWarped_idepth; 60 | float *poseBufWarped_u; 61 | float *poseBufWarped_v; 62 | float *poseBufWarped_dx; 63 | float *poseBufWarped_dy; 64 | float *poseBufWarped_residual; 65 | float *poseBufWarped_weight; 66 | float *poseBufWarped_refColor; 67 | int poseBufWarped_n; 68 | 69 | FrameHessian *newFrame; 70 | Accumulator9 poseAcc; 71 | }; 72 | 73 | class CoarseDistanceMap { 74 | public: 75 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 76 | 77 | Mat33f K[PYR_LEVELS]; 78 | Mat33f Ki[PYR_LEVELS]; 79 | 80 | CoarseDistanceMap(int w, int h); 81 | ~CoarseDistanceMap(); 82 | 83 | void makeDistanceMap(std::vector frameHessians, 84 | FrameHessian *frame); 85 | 86 | void makeInlierVotes(std::vector frameHessians); 87 | 88 | void makeK(CalibHessian *HCalib); 89 | 90 | float *fwdWarpedIDDistFinal; 91 | 92 | void addIntoDistFinal(int u, int v); 93 | 94 | private: 95 | int w[PYR_LEVELS]; 96 | int h[PYR_LEVELS]; 97 | 98 | PointFrameResidual **coarseProjectionGrid; 99 | int *coarseProjectionGridnum; 100 | Eigen::Vector2i *bfsList1; 101 | Eigen::Vector2i *bfsList2; 102 | 103 | void growDistBFS(int bfsNum); 104 | }; 105 | 106 | } // namespace dso 107 | -------------------------------------------------------------------------------- /src/FullSystem/ImmaturePoint.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | 26 | #include "util/NumType.h" 27 | 28 | #include "HessianBlocks.h" 29 | namespace dso { 30 | 31 | struct ImmaturePointTemporaryResidual { 32 | public: 33 | ResState state_state; 34 | double state_energy; 35 | ResState state_NewState; 36 | double state_NewEnergy; 37 | FrameHessian *target; 38 | }; 39 | 40 | enum ImmaturePointStatus { 41 | IPS_GOOD = 0, // traced well and good 42 | IPS_OOB, // OOB: end tracking & marginalize! 43 | IPS_OUTLIER, // energy too high: if happens again: outlier! 44 | IPS_SKIPPED, // traced well and good (but not actually traced). 45 | IPS_BADCONDITION, // not traced because of bad condition. 46 | IPS_UNINITIALIZED 47 | }; // not even traced once. 48 | 49 | class ImmaturePoint { 50 | public: 51 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 52 | // static values 53 | float color[MAX_RES_PER_POINT]; 54 | float weights[MAX_RES_PER_POINT]; 55 | 56 | Mat22f gradH; 57 | Vec2f gradH_ev; 58 | Mat22f gradH_eig; 59 | float energyTH; 60 | float u, v; 61 | FrameHessian *host; 62 | int idxInImmaturePoints; 63 | 64 | float quality; 65 | 66 | float my_type; 67 | 68 | float idepth_min; 69 | float idepth_max; 70 | ImmaturePoint(int u_, int v_, FrameHessian *host_, float type, 71 | CalibHessian *HCalib); 72 | ~ImmaturePoint(); 73 | 74 | ImmaturePointStatus traceOn(FrameHessian *frame, 75 | const Mat33f &hostToFrame_KRKi, 76 | const Vec3f &hostToFrame_Kt, 77 | const Vec2f &hostToFrame_affine, 78 | CalibHessian *HCalib, bool debugPrint = false); 79 | 80 | ImmaturePointStatus lastTraceStatus; 81 | Vec2f lastTraceUV; 82 | float lastTracePixelInterval; 83 | 84 | float idepth_GT; 85 | 86 | double linearizeResidual(CalibHessian *HCalib, const float outlierTHSlack, 87 | ImmaturePointTemporaryResidual *tmpRes, float &Hdd, 88 | float &bd, float idepth); 89 | float getdPixdd(CalibHessian *HCalib, ImmaturePointTemporaryResidual *tmpRes, 90 | float idepth); 91 | 92 | float calcResidual(CalibHessian *HCalib, const float outlierTHSlack, 93 | ImmaturePointTemporaryResidual *tmpRes, float idepth); 94 | 95 | private: 96 | }; 97 | 98 | } // namespace dso 99 | -------------------------------------------------------------------------------- /src/FullSystem/PixelSelector2.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | 26 | #include "util/NumType.h" 27 | 28 | namespace dso { 29 | 30 | enum PixelSelectorStatus { PIXSEL_VOID = 0, PIXSEL_1, PIXSEL_2, PIXSEL_3 }; 31 | 32 | class FrameHessian; 33 | 34 | class PixelSelector { 35 | public: 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 37 | int makeMaps(const FrameHessian *const fh, float *map_out, float density, 38 | int recursionsLeft = 1, bool plot = false, float thFactor = 1); 39 | 40 | PixelSelector(int w, int h); 41 | ~PixelSelector(); 42 | int currentPotential; 43 | 44 | bool allowFast; 45 | void makeHists(const FrameHessian *const fh); 46 | 47 | private: 48 | Eigen::Vector3i select(const FrameHessian *const fh, float *map_out, int pot, 49 | float thFactor = 1); 50 | 51 | unsigned char *randomPattern; 52 | 53 | int *gradHist; 54 | float *ths; 55 | float *thsSmoothed; 56 | int thsStep; 57 | const FrameHessian *gradHistFrame; 58 | }; 59 | 60 | } // namespace dso 61 | -------------------------------------------------------------------------------- /src/FullSystem/ResidualProjections.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | 26 | #include "FullSystem.h" 27 | #include "HessianBlocks.h" 28 | #include "util/NumType.h" 29 | #include "util/settings.h" 30 | 31 | namespace dso { 32 | 33 | EIGEN_STRONG_INLINE float derive_idepth(const Vec3f &t, const float &u, 34 | const float &v, const int &dx, 35 | const int &dy, const float &dxInterp, 36 | const float &dyInterp, 37 | const float &drescale) { 38 | return (dxInterp * drescale * (t[0] - t[2] * u) + 39 | dyInterp * drescale * (t[1] - t[2] * v)) * 40 | SCALE_IDEPTH; 41 | } 42 | 43 | EIGEN_STRONG_INLINE bool projectPoint(const float &u_pt, const float &v_pt, 44 | const float &idepth, const Mat33f &KRKi, 45 | const Vec3f &Kt, float &Ku, float &Kv) { 46 | Vec3f ptp = KRKi * Vec3f(u_pt, v_pt, 1) + Kt * idepth; 47 | Ku = ptp[0] / ptp[2]; 48 | Kv = ptp[1] / ptp[2]; 49 | return Ku > 1.1f && Kv > 1.1f && Ku < wM3G && Kv < hM3G; 50 | } 51 | 52 | EIGEN_STRONG_INLINE bool 53 | projectPoint(const float &u_pt, const float &v_pt, const float &idepth, 54 | const int &dx, const int &dy, CalibHessian *const &HCalib, 55 | const Mat33f &R, const Vec3f &t, float &drescale, float &u, 56 | float &v, float &Ku, float &Kv, Vec3f &KliP, float &new_idepth) { 57 | KliP = Vec3f((u_pt + dx - HCalib->cxl()) * HCalib->fxli(), 58 | (v_pt + dy - HCalib->cyl()) * HCalib->fyli(), 1); 59 | 60 | Vec3f ptp = R * KliP + t * idepth; 61 | drescale = 1.0f / ptp[2]; 62 | new_idepth = idepth * drescale; 63 | 64 | if (!(drescale > 0)) 65 | return false; 66 | 67 | u = ptp[0] * drescale; 68 | v = ptp[1] * drescale; 69 | Ku = u * HCalib->fxl() + HCalib->cxl(); 70 | Kv = v * HCalib->fyl() + HCalib->cyl(); 71 | 72 | return Ku > 1.1f && Kv > 1.1f && Ku < wM3G && Kv < hM3G; 73 | } 74 | 75 | } // namespace dso 76 | -------------------------------------------------------------------------------- /src/FullSystem/Residuals.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | 26 | #include "util/globalCalib.h" 27 | #include "vector" 28 | 29 | #include "OptimizationBackend/RawResidualJacobian.h" 30 | #include "util/NumType.h" 31 | #include "util/globalFuncs.h" 32 | #include 33 | #include 34 | 35 | namespace dso { 36 | class PointHessian; 37 | class FrameHessian; 38 | class CalibHessian; 39 | 40 | class EFResidual; 41 | 42 | enum ResLocation { ACTIVE = 0, LINEARIZED, MARGINALIZED, NONE }; 43 | enum ResState { IN = 0, OOB, OUTLIER }; 44 | 45 | struct FullJacRowT { 46 | Eigen::Vector2f projectedTo[MAX_RES_PER_POINT]; 47 | }; 48 | 49 | class PointFrameResidual { 50 | public: 51 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 52 | 53 | EFResidual *efResidual; 54 | 55 | static int instanceCounter; 56 | 57 | ResState state_state; 58 | double state_energy; 59 | ResState state_NewState; 60 | double state_NewEnergy; 61 | double state_NewEnergyWithOutlier; 62 | 63 | void setState(ResState s) { state_state = s; } 64 | 65 | PointHessian *point; 66 | FrameHessian *host; 67 | FrameHessian *target; 68 | RawResidualJacobian *J; 69 | 70 | bool isNew; 71 | 72 | Eigen::Vector2f projectedTo[MAX_RES_PER_POINT]; 73 | Vec3f centerProjectedTo; 74 | 75 | ~PointFrameResidual(); 76 | PointFrameResidual(); 77 | PointFrameResidual(PointHessian *point_, FrameHessian *host_, 78 | FrameHessian *target_); 79 | double linearize(CalibHessian *HCalib); 80 | 81 | void resetOOB() { 82 | state_NewEnergy = state_energy = 0; 83 | state_NewState = ResState::OUTLIER; 84 | 85 | setState(ResState::IN); 86 | }; 87 | void applyRes(bool copyJacobians); 88 | 89 | void debugPlot(); 90 | 91 | void printRows(std::vector &v, VecX &r, int nFrames, int nPoints, int M, 92 | int res); 93 | }; 94 | } // namespace dso 95 | -------------------------------------------------------------------------------- /src/FullSystem/ScaleOptimizer.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) <2022> 2 | 3 | // This program is free software: you can redistribute it and/or modify 4 | // it under the terms of the GNU General Public License as published by 5 | // the Free Software Foundation, either version 3 of the License, or 6 | // (at your option) any later version. 7 | 8 | // This program is distributed in the hope that it will be useful, 9 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | // GNU General Public License for more details. 12 | 13 | // You should have received a copy of the GNU General Public License 14 | // along with this program. If not, see . 15 | 16 | // This file is modified from 17 | 18 | #pragma once 19 | 20 | #include "IOWrapper/Output3DWrapper.h" 21 | #include "OptimizationBackend/MatrixAccumulators.h" 22 | #include "OptimizationBackend/ScaleAccumulator.h" 23 | #include "util/NumType.h" 24 | #include "util/settings.h" 25 | 26 | #include 27 | #include 28 | 29 | namespace dso { 30 | struct CalibHessian; 31 | struct FrameHessian; 32 | struct PointFrameResidual; 33 | 34 | template 35 | T *allocAligned(int size, std::vector &rawPtrVec) { 36 | const int padT = 1 + ((1 << b) / sizeof(T)); 37 | T *ptr = new T[size + padT]; 38 | rawPtrVec.push_back(ptr); 39 | T *alignedPtr = (T *)((((uintptr_t)(ptr + padT)) >> b) << b); 40 | return alignedPtr; 41 | } 42 | 43 | class ScaleOptimizer { 44 | public: 45 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 46 | 47 | ScaleOptimizer(int w, int h, const std::vector &tfm_cam1_cam0, 48 | const Mat33f &K1); 49 | ~ScaleOptimizer(); 50 | 51 | void makeK(CalibHessian *HCalib); 52 | 53 | float optimizeScale(FrameHessian *fh1, float &scale, int coarsestLvl); 54 | 55 | protected: 56 | std::vector ptrToDelete; 57 | 58 | float *idepth[PYR_LEVELS]; 59 | float *weight_sums[PYR_LEVELS]; 60 | float *weight_sums_bak[PYR_LEVELS]; 61 | 62 | Mat33f Ki[PYR_LEVELS]; 63 | float fx[PYR_LEVELS]; 64 | float fy[PYR_LEVELS]; 65 | float cx[PYR_LEVELS]; 66 | float cy[PYR_LEVELS]; 67 | int w[PYR_LEVELS]; 68 | int h[PYR_LEVELS]; 69 | 70 | // pc buffers 71 | float *pc_u[PYR_LEVELS]; 72 | float *pc_v[PYR_LEVELS]; 73 | float *pc_idepth[PYR_LEVELS]; 74 | float *pc_color[PYR_LEVELS]; 75 | int pc_n[PYR_LEVELS]; 76 | 77 | /**************************Scale Optimization***************************/ 78 | void calcGSSSEScale(int lvl, float &H_out, float &b_out, float scale); 79 | Vec6 calcResScale(int lvl, float scale, float cutoffTH, 80 | bool plot_img = false); 81 | 82 | // Transformation from frame0 to frame1 83 | SE3 tfmF0ToF1; 84 | 85 | // cam1 params 86 | float fx1[PYR_LEVELS]; 87 | float fy1[PYR_LEVELS]; 88 | float cx1[PYR_LEVELS]; 89 | float cy1[PYR_LEVELS]; 90 | 91 | // warped buffers 92 | float *scaleBufWarped_rx1; 93 | float *scaleBufWarped_rx2; 94 | float *scaleBufWarped_rx3; 95 | float *scaleBufWarped_dx; 96 | float *scaleBufWarped_dy; 97 | float *scaleBufWarped_residual; 98 | float *scaleBufWarped_weight; 99 | float *scaleBufWarped_ref_color; 100 | int scaleBufWarped_n; 101 | 102 | FrameHessian *fhStereo; 103 | ScaleAccumulator scaleAcc; 104 | }; 105 | 106 | } // namespace dso 107 | -------------------------------------------------------------------------------- /src/IOWrapper/ImageDisplay.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | #include "util/MinimalImage.h" 26 | #include "util/NumType.h" 27 | #include 28 | 29 | namespace dso { 30 | 31 | namespace IOWrap { 32 | 33 | void displayImage(const char *windowName, const MinimalImageB *img, 34 | bool autoSize = false); 35 | void displayImage(const char *windowName, const MinimalImageB3 *img, 36 | bool autoSize = false); 37 | void displayImage(const char *windowName, const MinimalImageF *img, 38 | bool autoSize = false); 39 | void displayImage(const char *windowName, const MinimalImageF3 *img, 40 | bool autoSize = false); 41 | void displayImage(const char *windowName, const MinimalImageB16 *img, 42 | bool autoSize = false); 43 | 44 | void displayImageStitch(const char *windowName, 45 | const std::vector images, int cc = 0, 46 | int rc = 0); 47 | void displayImageStitch(const char *windowName, 48 | const std::vector images, int cc = 0, 49 | int rc = 0); 50 | void displayImageStitch(const char *windowName, 51 | const std::vector images, int cc = 0, 52 | int rc = 0); 53 | void displayImageStitch(const char *windowName, 54 | const std::vector images, int cc = 0, 55 | int rc = 0); 56 | 57 | int waitKey(int milliseconds); 58 | void closeAllWindows(); 59 | 60 | } // namespace IOWrap 61 | 62 | } // namespace dso 63 | -------------------------------------------------------------------------------- /src/IOWrapper/ImageDisplay_dummy.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #include "IOWrapper/ImageDisplay.h" 25 | 26 | namespace dso { 27 | 28 | namespace IOWrap { 29 | void displayImage(const char *windowName, const MinimalImageB *img, 30 | bool autoSize){}; 31 | void displayImage(const char *windowName, const MinimalImageB3 *img, 32 | bool autoSize){}; 33 | void displayImage(const char *windowName, const MinimalImageF *img, 34 | bool autoSize){}; 35 | void displayImage(const char *windowName, const MinimalImageF3 *img, 36 | bool autoSize){}; 37 | void displayImage(const char *windowName, const MinimalImageB16 *img, 38 | bool autoSize){}; 39 | 40 | void displayImageStitch(const char *windowName, 41 | const std::vector images, int cc, 42 | int rc){}; 43 | void displayImageStitch(const char *windowName, 44 | const std::vector images, int cc, 45 | int rc){}; 46 | void displayImageStitch(const char *windowName, 47 | const std::vector images, int cc, 48 | int rc){}; 49 | void displayImageStitch(const char *windowName, 50 | const std::vector images, int cc, 51 | int rc){}; 52 | 53 | int waitKey(int milliseconds) { return 0; }; 54 | void closeAllWindows(){}; 55 | } // namespace IOWrap 56 | 57 | } // namespace dso 58 | -------------------------------------------------------------------------------- /src/IOWrapper/ImageRW.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | #include "util/MinimalImage.h" 26 | #include "util/NumType.h" 27 | 28 | namespace dso { 29 | namespace IOWrap { 30 | 31 | MinimalImageB *readImageBW_8U(std::string filename); 32 | MinimalImageB3 *readImageRGB_8U(std::string filename); 33 | MinimalImage *readImageBW_16U(std::string filename); 34 | 35 | MinimalImageB *readStreamBW_8U(char *data, int numBytes); 36 | 37 | void writeImage(std::string filename, MinimalImageB *img); 38 | void writeImage(std::string filename, MinimalImageB3 *img); 39 | void writeImage(std::string filename, MinimalImageF *img); 40 | void writeImage(std::string filename, MinimalImageF3 *img); 41 | 42 | } // namespace IOWrap 43 | } // namespace dso 44 | -------------------------------------------------------------------------------- /src/IOWrapper/ImageRW_dummy.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #include "IOWrapper/ImageRW.h" 25 | 26 | namespace dso { 27 | 28 | namespace IOWrap { 29 | 30 | MinimalImageB *readImageBW_8U(std::string filename) { 31 | printf("not implemented. bye!\n"); 32 | return 0; 33 | }; 34 | MinimalImageB3 *readImageRGB_8U(std::string filename) { 35 | printf("not implemented. bye!\n"); 36 | return 0; 37 | }; 38 | MinimalImage *readImageBW_16U(std::string filename) { 39 | printf("not implemented. bye!\n"); 40 | return 0; 41 | }; 42 | MinimalImageB *readStreamBW_8U(char *data, int numBytes) { 43 | printf("not implemented. bye!\n"); 44 | return 0; 45 | }; 46 | void writeImage(std::string filename, MinimalImageB *img){}; 47 | void writeImage(std::string filename, MinimalImageB3 *img){}; 48 | void writeImage(std::string filename, MinimalImageF *img){}; 49 | void writeImage(std::string filename, MinimalImageF3 *img){}; 50 | 51 | } // namespace IOWrap 52 | 53 | } // namespace dso 54 | -------------------------------------------------------------------------------- /src/IOWrapper/Pangolin/KeyFrameDisplay.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) <2022> 2 | 3 | // This program is free software: you can redistribute it and/or modify 4 | // it under the terms of the GNU General Public License as published by 5 | // the Free Software Foundation, either version 3 of the License, or 6 | // (at your option) any later version. 7 | 8 | // This program is distributed in the hope that it will be useful, 9 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | // GNU General Public License for more details. 12 | 13 | // You should have received a copy of the GNU General Public License 14 | // along with this program. If not, see . 15 | 16 | // This file is modified from 17 | 18 | #pragma once 19 | 20 | #undef Success 21 | #include "util/NumType.h" 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | namespace dso { 29 | class CalibHessian; 30 | class FrameHessian; 31 | class FrameShell; 32 | 33 | namespace IOWrap { 34 | 35 | template struct InputPointSparse { 36 | float u; 37 | float v; 38 | float idepth; 39 | float idepth_hessian; 40 | float relObsBaseline; 41 | int numGoodRes; 42 | unsigned char color[ppp]; 43 | unsigned char status; 44 | }; 45 | 46 | struct MyVertex { 47 | float point[3]; 48 | unsigned char color[4]; 49 | }; 50 | 51 | // stores a pointcloud associated to a Keyframe. 52 | class KeyFrameDisplay { 53 | 54 | public: 55 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 56 | KeyFrameDisplay(); 57 | ~KeyFrameDisplay(); 58 | 59 | // copies points from KF over to internal buffer, 60 | // keeping some additional information so we can render it differently. 61 | void setFromKF(FrameHessian *fh, CalibHessian *HCalib); 62 | 63 | // copies & filters internal data to GL buffer for rendering. if nothing to 64 | // do: does nothing. 65 | void refreshPC(); 66 | 67 | // renders cam & pointcloud. 68 | void drawCam(float lineWidth = 1, float *color = 0, float sizeFactor = 1); 69 | void drawPC(float pointSize); 70 | 71 | int id; 72 | bool active; 73 | SE3 tfmCToW; 74 | bool needRefresh; 75 | 76 | inline bool operator<(const KeyFrameDisplay &other) const { 77 | return (id < other.id); 78 | } 79 | 80 | private: 81 | float fx, fy, cx, cy; 82 | float fxi, fyi, cxi, cyi; 83 | int width, height; 84 | 85 | float myScaledTh, myAbsTh, myScale; 86 | int mySparsifyFactor; 87 | int myDisplayMode; 88 | float myMinRelBs; 89 | 90 | int numSparsePoints; 91 | int numSparseBufferSize; 92 | InputPointSparse *originalInputSparse; 93 | 94 | bool bufferValid; 95 | int numGlBufferPoints; 96 | int numGlBufferGoodPoints; 97 | pangolin::GlBuffer vertexBuffer; 98 | pangolin::GlBuffer colorBuffer; 99 | }; 100 | 101 | } // namespace IOWrap 102 | } // namespace dso 103 | -------------------------------------------------------------------------------- /src/IOWrapper/Pangolin/PangolinSOSVIOViewer.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) <2022> 2 | 3 | // This program is free software: you can redistribute it and/or modify 4 | // it under the terms of the GNU General Public License as published by 5 | // the Free Software Foundation, either version 3 of the License, or 6 | // (at your option) any later version. 7 | 8 | // This program is distributed in the hope that it will be useful, 9 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | // GNU General Public License for more details. 12 | 13 | // You should have received a copy of the GNU General Public License 14 | // along with this program. If not, see . 15 | 16 | // This file is modified from 17 | 18 | #pragma once 19 | #include "IOWrapper/Output3DWrapper.h" 20 | #include "boost/thread.hpp" 21 | #include "util/MinimalImage.h" 22 | #include 23 | #include 24 | #include 25 | 26 | namespace dso { 27 | 28 | class FrameHessian; 29 | class CalibHessian; 30 | class FrameShell; 31 | 32 | namespace IOWrap { 33 | 34 | class KeyFrameDisplay; 35 | 36 | class PangolinSOSVIOViewer : public Output3DWrapper { 37 | public: 38 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 39 | PangolinSOSVIOViewer(int w_, int h_, bool startRunThread = true); 40 | virtual ~PangolinSOSVIOViewer(); 41 | 42 | void run(); 43 | void close(); 44 | 45 | void addImageToDisplay(std::string name, MinimalImageB3 *image); 46 | void clearAllImagesToDisplay(); 47 | 48 | // ==================== Output3DWrapper Functionality ====================== 49 | virtual void publishKeyframes(std::vector &frames, bool final, 50 | CalibHessian *HCalib) override; 51 | 52 | virtual void modifyKeyframePoseByKFID(int id, const SE3 &poseCamToWorld); 53 | 54 | void refreshLidarData(const std::vector &pts, size_t cur_sz); 55 | 56 | void pushLoopClosure(std::pair new_loop_pair); 57 | 58 | virtual void pushLiveFrame(FrameHessian *image) override; 59 | 60 | virtual void pushDepthImage(MinimalImageB3 *image) override; 61 | 62 | virtual void join() override; 63 | 64 | private: 65 | void drawConstraints(); 66 | 67 | void drawLoopClosures(); 68 | 69 | void drawLidar(); 70 | 71 | boost::thread runThread; 72 | bool running; 73 | int w, h; 74 | 75 | // frame info 76 | size_t frameID; 77 | double scaleScale; 78 | double scaleErrInit; 79 | double imuBa; 80 | double imuBg; 81 | 82 | // 3D model rendering 83 | boost::mutex model3dMutex; 84 | std::vector keyframesIdSorted; 85 | std::map keyframesById; 86 | 87 | // lidar rendering 88 | boost::mutex modelLidarMutex; 89 | std::vector lidarPts; 90 | size_t lidarCurSize; 91 | 92 | // images rendering 93 | boost::mutex openImagesMutex; 94 | MinimalImageB3 *internalVideoImg; 95 | bool videoImgChanged; 96 | MinimalImageB3 *internalKFImg; 97 | bool KFImgChanged; 98 | 99 | // loop closure rendering 100 | boost::mutex loopClosureMutex; 101 | std::vector> loopClosures; 102 | }; 103 | 104 | } // namespace IOWrap 105 | 106 | } // namespace dso 107 | -------------------------------------------------------------------------------- /src/LoopClosure/PoseEstimator.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) <2022> 2 | 3 | // This program is free software: you can redistribute it and/or modify 4 | // it under the terms of the GNU General Public License as published by 5 | // the Free Software Foundation, either version 3 of the License, or 6 | // (at your option) any later version. 7 | 8 | // This program is distributed in the hope that it will be useful, 9 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | // GNU General Public License for more details. 12 | 13 | // You should have received a copy of the GNU General Public License 14 | // along with this program. If not, see . 15 | 16 | // This file is modified from 17 | 18 | #pragma once 19 | 20 | #include "OptimizationBackend/MatrixAccumulators.h" 21 | #include "util/NumType.h" 22 | #include "util/settings.h" 23 | 24 | #include "LoopClosure/LoopHandler.h" 25 | 26 | #include "vector" 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | #define INNER_PERCENT 90 34 | // #define TRANS_THRES 3.0 35 | // #define ROT_THRES 0.2 36 | 37 | namespace dso { 38 | struct FrameHessian; 39 | struct PointFrameResidual; 40 | 41 | struct LoopFrame; 42 | 43 | class PoseEstimator { 44 | public: 45 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 46 | 47 | PoseEstimator(int w, int h); 48 | ~PoseEstimator(); 49 | 50 | bool estimate(const LoopFrame *cur_frame, const LoopFrame *matched_frame, 51 | int coarsest_lvl, Eigen::Matrix4d &ref_to_new, 52 | float &pose_error); 53 | 54 | bool icp(const std::vector &pts_source, 55 | const std::vector &pts_target, 56 | Eigen::Matrix4d &tfm_target_source, float &icp_score); 57 | 58 | private: 59 | void makeK(const std::vector &cam); 60 | Vec6 calcRes(int lvl, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH, 61 | bool plot_img = false); 62 | void calcGSSSE(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, 63 | AffLight aff_g2l); 64 | 65 | float fx[PYR_LEVELS]; 66 | float fy[PYR_LEVELS]; 67 | float cx[PYR_LEVELS]; 68 | float cy[PYR_LEVELS]; 69 | int w[PYR_LEVELS]; 70 | int h[PYR_LEVELS]; 71 | 72 | // pc buffers 73 | std::vector> pts; 74 | 75 | // warped buffers 76 | float *bufWarped_idepth; 77 | float *bufWarped_u; 78 | float *bufWarped_v; 79 | float *bufWarped_dx; 80 | float *bufWarped_dy; 81 | float *bufWarped_residual; 82 | float *bufWarped_weight; 83 | float *bufWarped_ref_color; 84 | int bufWarped_n; 85 | 86 | Accumulator9 acc; 87 | 88 | std::vector ptrToDelete; 89 | 90 | // photometric terms 91 | AffLight refAffGToL; 92 | float refAbExposure; 93 | FrameHessian *newFrame; 94 | 95 | pcl::PointCloud 96 | transformPoints(const std::vector &pts_input, 97 | const Eigen::Matrix4d &tfm_target_source); 98 | }; 99 | 100 | } // namespace dso -------------------------------------------------------------------------------- /src/LoopClosure/ScanContext.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) <2022> 2 | 3 | // This program is free software: you can redistribute it and/or modify 4 | // it under the terms of the GNU General Public License as published by 5 | // the Free Software Foundation, either version 3 of the License, or 6 | // (at your option) any later version. 7 | 8 | // This program is distributed in the hope that it will be useful, 9 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | // GNU General Public License for more details. 12 | 13 | // You should have received a copy of the GNU General Public License 14 | // along with this program. If not, see . 15 | 16 | #pragma once 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "LoopClosure/LoopHandler.h" 24 | #include "util/settings.h" 25 | 26 | namespace dso { 27 | 28 | struct LoopFrame; 29 | 30 | class ScanContext { 31 | public: 32 | ScanContext(); 33 | 34 | unsigned int getHeight(); 35 | unsigned int getWidth(); 36 | 37 | void process_scan(int cur_frame_id, const dso::SE3 &cur_wc, 38 | std::vector &pts_spherical, 39 | g2o::SE3Quat &tfm_sc_rig); 40 | 41 | bool generate(LoopFrame *frame, flann::Matrix &ringkey); 42 | 43 | void search_ringkey(const flann::Matrix &ringkey, 44 | flann::Index> *ringkeys, 45 | std::vector &candidates); 46 | 47 | void search_sc(std::vector> &signature, 48 | const std::vector &loop_frames, 49 | const std::vector &candidates, int &res_idx, 50 | float &res_diff); 51 | 52 | private: 53 | void getAlignTfmByPCA(const std::vector &pts, 54 | Mat44 &tfm_ned_cam); 55 | 56 | void process_scan_forward(int cur_frame_id, const dso::SE3 &cur_wc, 57 | std::vector &pts_spherical, 58 | g2o::SE3Quat &tfm_sc_rig); 59 | 60 | void process_scan_downward(const dso::SE3 &cur_wc, 61 | std::vector &pts_scan, 62 | g2o::SE3Quat &tfm_sc_rig); 63 | 64 | // variables for forward-facing camera only 65 | std::unordered_map> id2pose_wc; 66 | std::vector> ptsNearby; 67 | }; 68 | 69 | } // namespace dso 70 | -------------------------------------------------------------------------------- /src/OptimizationBackend/RawResidualJacobian.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | 26 | #include "util/NumType.h" 27 | 28 | namespace dso { 29 | struct RawResidualJacobian { 30 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 31 | // ================== new structure: save independently =============. 32 | VecNRf resF; 33 | 34 | // the two rows of d[x,y]/d[xi]. 35 | Vec6f Jpdxi[2]; // 2x6 36 | 37 | // the two rows of d[x,y]/d[C]. 38 | VecCf Jpdc[2]; // 2x4 39 | 40 | // the two rows of d[x,y]/d[idepth]. 41 | Vec2f Jpdd; // 2x1 42 | 43 | // the two columns of d[r]/d[x,y]. 44 | VecNRf JIdx[2]; // 9x2 45 | 46 | // = the two columns of d[r] / d[ab] 47 | VecNRf JabF[2]; // 9x2 48 | 49 | // = JIdx^T * JIdx (inner product). Only as a shorthand. 50 | Mat22f JIdx2; // 2x2 51 | // = Jab^T * JIdx (inner product). Only as a shorthand. 52 | Mat22f JabJIdx; // 2x2 53 | // = Jab^T * Jab (inner product). Only as a shorthand. 54 | Mat22f Jab2; // 2x2 55 | }; 56 | } // namespace dso 57 | -------------------------------------------------------------------------------- /src/OptimizationBackend/ScaleAccumulator.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) <2022> 2 | 3 | // This program is free software: you can redistribute it and/or modify 4 | // it under the terms of the GNU General Public License as published by 5 | // the Free Software Foundation, either version 3 of the License, or 6 | // (at your option) any later version. 7 | 8 | // This program is distributed in the hope that it will be useful, 9 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | // GNU General Public License for more details. 12 | 13 | // You should have received a copy of the GNU General Public License 14 | // along with this program. If not, see . 15 | 16 | // This file is modified from 17 | 18 | #pragma once 19 | #include "util/NumType.h" 20 | 21 | #if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) 22 | #include "SSE2NEON.h" 23 | #endif 24 | 25 | namespace dso { 26 | 27 | class ScaleAccumulator { 28 | public: 29 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 30 | Mat22f hessian; 31 | Vec2f b; 32 | size_t num; 33 | 34 | inline void initialize() { 35 | hessian.setZero(); 36 | b.setZero(); 37 | memset(sseData, 0, sizeof(float) * 4 * 3); 38 | memset(sseData1k, 0, sizeof(float) * 4 * 3); 39 | memset(sseData1m, 0, sizeof(float) * 4 * 3); 40 | num = numIn1 = numIn1k = numIn1m = 0; 41 | } 42 | 43 | inline void finish() { 44 | hessian.setZero(); 45 | shiftUp(true); 46 | assert(numIn1 == 0); 47 | assert(numIn1k == 0); 48 | 49 | int idx = 0; 50 | for (int r = 0; r < 2; r++) 51 | for (int c = r; c < 2; c++) { 52 | float d = sseData1m[idx + 0] + sseData1m[idx + 1] + 53 | sseData1m[idx + 2] + sseData1m[idx + 3]; 54 | hessian(r, c) = hessian(c, r) = d; 55 | idx += 4; 56 | } 57 | assert(idx == 4 * 3); 58 | } 59 | 60 | inline void updateSSE_oneed(const __m128 J0, const __m128 J1, 61 | const __m128 w) { 62 | float *pt = sseData; 63 | 64 | __m128 J0w = _mm_mul_ps(J0, w); 65 | _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt), _mm_mul_ps(J0w, J0))); 66 | pt += 4; 67 | _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt), _mm_mul_ps(J0w, J1))); 68 | pt += 4; 69 | 70 | __m128 J1w = _mm_mul_ps(J1, w); 71 | _mm_store_ps(pt, _mm_add_ps(_mm_load_ps(pt), _mm_mul_ps(J1w, J1))); 72 | pt += 4; 73 | 74 | num += 4; 75 | numIn1++; 76 | shiftUp(false); 77 | } 78 | 79 | private: 80 | EIGEN_ALIGN16 float sseData[4 * 3]; 81 | EIGEN_ALIGN16 float sseData1k[4 * 3]; 82 | EIGEN_ALIGN16 float sseData1m[4 * 3]; 83 | float numIn1, numIn1k, numIn1m; 84 | 85 | void shiftUp(bool force) { 86 | if (numIn1 > 1000 || force) { 87 | for (int i = 0; i < 3; i++) 88 | _mm_store_ps(sseData1k + 4 * i, 89 | _mm_add_ps(_mm_load_ps(sseData + 4 * i), 90 | _mm_load_ps(sseData1k + 4 * i))); 91 | numIn1k += numIn1; 92 | numIn1 = 0; 93 | memset(sseData, 0, sizeof(float) * 4 * 3); 94 | } 95 | 96 | if (numIn1k > 1000 || force) { 97 | for (int i = 0; i < 3; i++) 98 | _mm_store_ps(sseData1m + 4 * i, 99 | _mm_add_ps(_mm_load_ps(sseData1k + 4 * i), 100 | _mm_load_ps(sseData1m + 4 * i))); 101 | numIn1m += numIn1k; 102 | numIn1k = 0; 103 | memset(sseData1k, 0, sizeof(float) * 4 * 3); 104 | } 105 | } 106 | }; 107 | 108 | } // namespace dso 109 | -------------------------------------------------------------------------------- /src/SlamNode.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) <2022> 2 | 3 | // This program is free software: you can redistribute it and/or modify 4 | // it under the terms of the GNU General Public License as published by 5 | // the Free Software Foundation, either version 3 of the License, or 6 | // (at your option) any later version. 7 | 8 | // This program is distributed in the hope that it will be useful, 9 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | // GNU General Public License for more details. 12 | 13 | // You should have received a copy of the GNU General Public License 14 | // along with this program. If not, see . 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include "FullSystem/FullSystem.h" 21 | #include "util/Undistort.h" 22 | 23 | using namespace dso; 24 | 25 | class SlamNode { 26 | private: 27 | int startFrame; 28 | int incomingId; 29 | FullSystem *fullSystem; 30 | Undistort *undistorter0; 31 | std::queue> imgQueue; 32 | boost::mutex imgQueueMutex; 33 | 34 | // scale optimization 35 | std::vector tfmCam1Cam0; 36 | Undistort *undistorter1; 37 | 38 | // IMU 39 | double tdCamImu; 40 | std::queue imuQueue; 41 | boost::mutex imuQueueMutex; 42 | 43 | public: 44 | bool isLost; 45 | SlamNode(int start_frame, double td_cam_imu, 46 | const std::vector &tfm_cam1_cam0, const std::string &calib0, 47 | const std::string &calib1, const std::string &vignette0, 48 | const std::string &vignette1, const std::string &gamma0, 49 | const std::string &gamma1); 50 | ~SlamNode(); 51 | 52 | void imuMessageCallback(const sensor_msgs::ImuConstPtr &msg); 53 | 54 | void imageMessageCallback(const sensor_msgs::ImageConstPtr &msg0, 55 | const sensor_msgs::ImageConstPtr &msg1); 56 | 57 | void process(ImageAndExposure *img0, ImageAndExposure *img1, 58 | const std::vector &imu_data); 59 | }; 60 | -------------------------------------------------------------------------------- /src/util/FrameShell.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | 26 | #include "NumType.h" 27 | #include "algorithm" 28 | 29 | namespace dso { 30 | 31 | class FrameShell { 32 | public: 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 34 | int id; // INTERNAL ID, starting at zero. 35 | int incoming_id; // ID passed into DSO 36 | double timestamp; // timestamp passed into DSO. 37 | 38 | SE3 camToTrackingRef; 39 | FrameShell *trackingRef; 40 | 41 | // constantly adapted. 42 | double scale; 43 | SE3 camToWorldScaled; 44 | Vec3 velInWorld; 45 | SE3 camToWorld; // Write: TRACKING, while frame is still fresh; MAPPING: only 46 | // when locked [shellPoseMutex]. 47 | AffLight aff_g2l; 48 | bool poseValid; 49 | 50 | // statisitcs 51 | int statistics_outlierResOnThis; 52 | int statistics_goodResOnThis; 53 | int marginalizedAt; 54 | double movedByOpt; 55 | 56 | inline FrameShell() { 57 | id = 0; 58 | poseValid = true; 59 | camToWorld = SE3(); 60 | timestamp = 0; 61 | marginalizedAt = -1; 62 | movedByOpt = 0; 63 | statistics_outlierResOnThis = statistics_goodResOnThis = 0; 64 | trackingRef = 0; 65 | camToTrackingRef = SE3(); 66 | } 67 | }; 68 | 69 | } // namespace dso 70 | -------------------------------------------------------------------------------- /src/util/ImageAndExposure.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | #include 26 | #include 27 | 28 | namespace dso { 29 | 30 | class ImageAndExposure { 31 | public: 32 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 33 | float *image; // irradiance. between 0 and 256 34 | int w, h; // width and height; 35 | double timestamp; 36 | float exposure_time; // exposure time in ms. 37 | inline ImageAndExposure(int w_, int h_, double timestamp_ = 0) 38 | : w(w_), h(h_), timestamp(timestamp_) { 39 | image = new float[w * h]; 40 | exposure_time = 1; 41 | } 42 | inline ~ImageAndExposure() { delete[] image; } 43 | 44 | inline void copyMetaTo(ImageAndExposure &other) { 45 | other.exposure_time = exposure_time; 46 | } 47 | 48 | inline ImageAndExposure *getDeepCopy() { 49 | ImageAndExposure *img = new ImageAndExposure(w, h, timestamp); 50 | img->exposure_time = exposure_time; 51 | memcpy(img->image, image, w * h * sizeof(float)); 52 | return img; 53 | } 54 | }; 55 | 56 | } // namespace dso 57 | -------------------------------------------------------------------------------- /src/util/globalCalib.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #include "globalCalib.h" 25 | #include "stdio.h" 26 | #include 27 | 28 | namespace dso { 29 | int wG[PYR_LEVELS], hG[PYR_LEVELS]; 30 | float fxG[PYR_LEVELS], fyG[PYR_LEVELS], cxG[PYR_LEVELS], cyG[PYR_LEVELS]; 31 | 32 | float fxiG[PYR_LEVELS], fyiG[PYR_LEVELS], cxiG[PYR_LEVELS], cyiG[PYR_LEVELS]; 33 | 34 | Eigen::Matrix3f KG[PYR_LEVELS], KiG[PYR_LEVELS]; 35 | 36 | float wM3G; 37 | float hM3G; 38 | 39 | void setGlobalCalib(int w, int h, const Eigen::Matrix3f &K) { 40 | int wlvl = w; 41 | int hlvl = h; 42 | pyrLevelsUsed = 1; 43 | while (wlvl % 2 == 0 && hlvl % 2 == 0 && wlvl * hlvl > 5000 && 44 | pyrLevelsUsed < PYR_LEVELS) { 45 | wlvl /= 2; 46 | hlvl /= 2; 47 | pyrLevelsUsed++; 48 | } 49 | printf("using pyramid levels 0 to %d. coarsest resolution: %d x %d!\n", 50 | pyrLevelsUsed - 1, wlvl, hlvl); 51 | if (wlvl > 100 && hlvl > 100) { 52 | printf("\n\n===============WARNING!===================\n " 53 | "using not enough pyramid levels.\n" 54 | "Consider scaling to a resolution that is a multiple of a power of " 55 | "2.\n"); 56 | } 57 | if (pyrLevelsUsed < 3) { 58 | printf("\n\n===============WARNING!===================\n " 59 | "I need higher resolution.\n" 60 | "I will probably segfault.\n"); 61 | } 62 | 63 | wM3G = w - 3; 64 | hM3G = h - 3; 65 | 66 | wG[0] = w; 67 | hG[0] = h; 68 | KG[0] = K; 69 | fxG[0] = K(0, 0); 70 | fyG[0] = K(1, 1); 71 | cxG[0] = K(0, 2); 72 | cyG[0] = K(1, 2); 73 | KiG[0] = KG[0].inverse(); 74 | fxiG[0] = KiG[0](0, 0); 75 | fyiG[0] = KiG[0](1, 1); 76 | cxiG[0] = KiG[0](0, 2); 77 | cyiG[0] = KiG[0](1, 2); 78 | 79 | for (int level = 1; level < pyrLevelsUsed; ++level) { 80 | wG[level] = w >> level; 81 | hG[level] = h >> level; 82 | 83 | fxG[level] = fxG[level - 1] * 0.5; 84 | fyG[level] = fyG[level - 1] * 0.5; 85 | cxG[level] = (cxG[0] + 0.5) / ((int)1 << level) - 0.5; 86 | cyG[level] = (cyG[0] + 0.5) / ((int)1 << level) - 0.5; 87 | 88 | KG[level] << fxG[level], 0.0, cxG[level], 0.0, fyG[level], cyG[level], 0.0, 89 | 0.0, 1.0; // synthetic 90 | KiG[level] = KG[level].inverse(); 91 | 92 | fxiG[level] = KiG[level](0, 0); 93 | fyiG[level] = KiG[level](1, 1); 94 | cxiG[level] = KiG[level](0, 2); 95 | cyiG[level] = KiG[level](1, 2); 96 | } 97 | } 98 | 99 | } // namespace dso 100 | -------------------------------------------------------------------------------- /src/util/globalCalib.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | #include "NumType.h" 26 | #include "settings.h" 27 | 28 | namespace dso { 29 | extern int wG[PYR_LEVELS], hG[PYR_LEVELS]; 30 | extern float fxG[PYR_LEVELS], fyG[PYR_LEVELS], cxG[PYR_LEVELS], cyG[PYR_LEVELS]; 31 | 32 | extern float fxiG[PYR_LEVELS], fyiG[PYR_LEVELS], cxiG[PYR_LEVELS], 33 | cyiG[PYR_LEVELS]; 34 | 35 | extern Eigen::Matrix3f KG[PYR_LEVELS], KiG[PYR_LEVELS]; 36 | 37 | extern float wM3G; 38 | extern float hM3G; 39 | 40 | void setGlobalCalib(int w, int h, const Eigen::Matrix3f &K); 41 | } // namespace dso 42 | -------------------------------------------------------------------------------- /tests/EuRoC/calib.yaml: -------------------------------------------------------------------------------- 1 | # ROS topics 2 | imu_topic: /imu0 3 | cam0_topic: /cam0/image_raw 4 | cam1_topic: /cam1/image_raw 5 | 6 | # transformation of IMU frame in the main camera 7 | T_cam0_imu: [ 0.0149, 0.9996, -0.0258, 0.0652, 8 | -0.9999, 0.0150, 0.0038, -0.0207, 9 | 0.0041, 0.0257, 0.9997, -0.0081, 10 | 0.0000, 0.0000, 0.0000, 1.0000] 11 | 12 | # transformation of the main camera in the other camera frame 13 | T_cam1_cam0: [ 1.0000, 0.0023, 0.0004, -0.1101, 14 | -0.0023, 0.9999, 0.0141, 0.0004, 15 | -0.0003, -0.0141, 0.9999, -0.0009, 16 | 0.0000, 0.0000, 0.0000, 1.0000] 17 | 18 | # IMU configuration 19 | rate_hz: 200 20 | accelerometer_noise_density: 2.0000e-3 # [ m / s^2 / sqrt(Hz) ] 21 | accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) 22 | gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] 23 | gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) 24 | -------------------------------------------------------------------------------- /tests/EuRoC/camera0.txt: -------------------------------------------------------------------------------- 1 | RadTan 0.609912234 0.9527 0.488982713 0.518489583 -0.28340811 0.07395907 0.00019359 1.76187114e-05 2 | 752 480 3 | crop 4 | 752 480 5 | -------------------------------------------------------------------------------- /tests/EuRoC/camera1.txt: -------------------------------------------------------------------------------- 1 | RadTan 0.608493351 0.950279167 0.505982713 0.5327875 -0.28368365 0.07451284 -0.00010473 -3.55590700e-05 2 | 752 480 3 | crop 4 | 752 480 5 | -------------------------------------------------------------------------------- /tests/EuRoC/euroc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /tests/KITTI/0_2/calib.yaml: -------------------------------------------------------------------------------- 1 | # ROS topics 2 | cam0_topic: /left/image_raw 3 | cam1_topic: /right/image_raw 4 | 5 | T_cam1_cam0: [ 1.0000, 0.0000, 0.0000, -0.5372, 6 | 0.0000, 1.0000, 0.0000, 0.0000, 7 | 0.0000, 0.0000, 1.0000, 0.0000001, 8 | 0.0000, 0.0000, 0.0000, 1.0000] 9 | -------------------------------------------------------------------------------- /tests/KITTI/0_2/camera0.txt: -------------------------------------------------------------------------------- 1 | Pinhole 718.8560 718.8560 607.1928 185.2157 0 2 | 1241 376 3 | crop 4 | 1232 368 -------------------------------------------------------------------------------- /tests/KITTI/0_2/camera1.txt: -------------------------------------------------------------------------------- 1 | Pinhole 718.8560 718.8560 607.1928 185.2157 0 2 | 1241 376 3 | crop 4 | 1232 368 -------------------------------------------------------------------------------- /tests/KITTI/4_12/calib.yaml: -------------------------------------------------------------------------------- 1 | # ROS topics 2 | cam0_topic: /left/image_raw 3 | cam1_topic: /right/image_raw 4 | 5 | T_cam1_cam0: [ 1.0000, 0.0000, 0.0000, -0.5372, 6 | 0.0000, 1.0000, 0.0000, 0.0000, 7 | 0.0000, 0.0000, 1.0000, 0.0000001, 8 | 0.0000, 0.0000, 0.0000, 1.0000] 9 | -------------------------------------------------------------------------------- /tests/KITTI/4_12/camera0.txt: -------------------------------------------------------------------------------- 1 | Pinhole 707.0912 707.0912 601.8873 183.1104 0 2 | 1226 370 3 | crop 4 | 1216 368 5 | -------------------------------------------------------------------------------- /tests/KITTI/4_12/camera1.txt: -------------------------------------------------------------------------------- 1 | Pinhole 707.0912 707.0912 601.8873 183.1104 0 2 | 1226 370 3 | crop 4 | 1216 368 5 | -------------------------------------------------------------------------------- /tests/KITTI/kitti.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /tests/Malaga/calib.yaml: -------------------------------------------------------------------------------- 1 | # ROS topics 2 | imu_topic: /imu 3 | cam0_topic: /left/image_raw 4 | cam1_topic: /right/image_raw 5 | 6 | T_cam1_cam0: [ 1.0000, 0.0000, 0.0000, -0.119471, 7 | 0.0000, 1.0000, 0.0000, 0.0000, 8 | 0.0000, 0.0000, 1.0000, 0.0000001, 9 | 0.0000, 0.0000, 0.0000, 1.0000] 10 | -------------------------------------------------------------------------------- /tests/Malaga/camera0.txt: -------------------------------------------------------------------------------- 1 | Pinhole 795.11588 795.11588 517.12973 395.59665 0 2 | 1024 768 3 | crop 4 | 1024 768 5 | -------------------------------------------------------------------------------- /tests/Malaga/camera1.txt: -------------------------------------------------------------------------------- 1 | Pinhole 795.11588 795.11588 517.12973 395.59665 0 2 | 1024 768 3 | crop 4 | 1024 768 5 | -------------------------------------------------------------------------------- /tests/Malaga/malaga.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /tests/RobotCar/calib.yaml: -------------------------------------------------------------------------------- 1 | # ROS topics 2 | cam0_topic: camera/left/image_raw 3 | cam1_topic: camera/right/image_raw 4 | 5 | T_cam1_cam0: [ 1.0000, 0.0000, 0.0000, -0.239983, 6 | 0.0000, 1.0000, 0.0000, 0.0000, 7 | 0.0000, 0.0000, 1.0000, 0.0000001, 8 | 0.0000, 0.0000, 0.0000, 1.0000] -------------------------------------------------------------------------------- /tests/RobotCar/camera0.txt: -------------------------------------------------------------------------------- 1 | Pinhole 983.044006 983.044006 643.646973 493.378998 0 2 | 1280 760 3 | crop 4 | 1280 760 5 | -------------------------------------------------------------------------------- /tests/RobotCar/camera1.txt: -------------------------------------------------------------------------------- 1 | Pinhole 983.044006 983.044006 643.646973 493.378998 0 2 | 1280 760 3 | crop 4 | 1280 760 5 | -------------------------------------------------------------------------------- /tests/RobotCar/robotcar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /tests/TUMVI/calib.yaml: -------------------------------------------------------------------------------- 1 | # ROS topics 2 | imu_topic: /imu0 3 | cam0_topic: /cam0/image_raw 4 | cam1_topic: /cam1/image_raw 5 | 6 | # transformation of IMU frame in the main camera 7 | T_cam0_imu: [-0.9995, 0.0296, -0.0085, 0.0473, 8 | 0.0075, -0.0344, -0.9994, -0.0474, 9 | -0.0299, -0.9990, 0.0342, -0.0682, 10 | 0.0000, 0.0000, 0.0000, 1.0000] 11 | 12 | # transformation of the main camera in the other camera frame 13 | T_cam1_cam0: [ 1.0000, -0.0008, -0.0007, -0.1011, 14 | 0.0008, 0.9989, -0.0469, -0.0020, 15 | 0.0007, 0.0469, 0.9989, -0.0012, 16 | 0.0000, 0.0000, 0.0000, 1.0000] 17 | 18 | # IMU configuration 19 | rate_hz: 200 20 | accelerometer_noise_density: 0.0028 # m/s^1.5 21 | accelerometer_random_walk: 0.00086 # m/s^2.5 22 | gyroscope_noise_density: 0.00016 # rad/s^0.5 23 | gyroscope_random_walk: 0.000022 # rad/s^1.5 -------------------------------------------------------------------------------- /tests/TUMVI/camera0.txt: -------------------------------------------------------------------------------- 1 | EquiDistant 0.373004838186 0.372994740336 0.498890050897 0.502729380663 0.00348238940225 0.000715034845216 -0.00205323614187 0.000202936735918 2 | 512 512 3 | crop 4 | 512 512 5 | -------------------------------------------------------------------------------- /tests/TUMVI/camera1.txt: -------------------------------------------------------------------------------- 1 | EquiDistant 0.371957753309 0.371942262641 0.494334955407 0.498861778606 0.00340031707904 0.00176627815347 -0.00266312569782 0.000329951742393 2 | 512 512 3 | crop 4 | 512 512 5 | -------------------------------------------------------------------------------- /tests/TUMVI/pcalib0.txt: -------------------------------------------------------------------------------- 1 | 0.0 1.0 2.0 3.0 4.0 5.0 6.0 7.0 8.0 9.0 10.0 11.0 12.0 13.0 14.0 15.0 16.0 17.0 18.0 19.0 20.0 21.0 22.0 23.0 24.0 25.0 26.0 27.0 28.0 29.0 30.0 31.0 32.0 33.0 34.0 35.0 36.0 37.0 38.0 39.0 40.0 41.0 42.0 43.0 44.0 45.0 46.0 47.0 48.0 49.0 50.0 51.0 52.0 53.0 54.0 55.0 56.0 57.0 58.0 59.0 60.0 61.0 62.0 63.0 64.0 65.0 66.0 67.0 68.0 69.0 70.0 71.0 72.0 73.0 74.0 75.0 76.0 77.0 78.0 79.0 80.0 81.0 82.0 83.0 84.0 85.0 86.0 87.0 88.0 89.0 90.0 91.0 92.0 93.0 94.0 95.0 96.0 97.0 98.0 99.0 100.0 101.0 102.0 103.0 104.0 105.0 106.0 107.0 108.0 109.0 110.0 111.0 112.0 113.0 114.0 115.0 116.0 117.0 118.0 119.0 120.0 121.0 122.0 123.0 124.0 125.0 126.0 127.0 128.0 129.0 130.0 131.0 132.0 133.0 134.0 135.0 136.0 137.0 138.0 139.0 140.0 141.0 142.0 143.0 144.0 145.0 146.0 147.0 148.0 149.0 150.0 151.0 152.0 153.0 154.0 155.0 156.0 157.0 158.0 159.0 160.0 161.0 162.0 163.0 164.0 165.0 166.0 167.0 168.0 169.0 170.0 171.0 172.0 173.0 174.0 175.0 176.0 177.0 178.0 179.0 180.0 181.0 182.0 183.0 184.0 185.0 186.0 187.0 188.0 189.0 190.0 191.0 192.0 193.0 194.0 195.0 196.0 197.0 198.0 199.0 200.0 201.0 202.0 203.0 204.0 205.0 206.0 207.0 208.0 209.0 210.0 211.0 212.0 213.0 214.0 215.0 216.0 217.0 218.0 219.0 220.0 221.0 222.0 223.0 224.0 225.0 226.0 227.0 228.0 229.0 230.0 231.0 232.0 233.0 234.0 235.0 236.0 237.0 238.0 239.0 240.0 241.0 242.0 243.0 244.0 245.0 246.0 247.0 248.0 249.0 250.0 251.0 252.0 253.0 254.0 255.0 -------------------------------------------------------------------------------- /tests/TUMVI/pcalib1.txt: -------------------------------------------------------------------------------- 1 | 0.0 1.0 2.0 3.0 4.0 5.0 6.0 7.0 8.0 9.0 10.0 11.0 12.0 13.0 14.0 15.0 16.0 17.0 18.0 19.0 20.0 21.0 22.0 23.0 24.0 25.0 26.0 27.0 28.0 29.0 30.0 31.0 32.0 33.0 34.0 35.0 36.0 37.0 38.0 39.0 40.0 41.0 42.0 43.0 44.0 45.0 46.0 47.0 48.0 49.0 50.0 51.0 52.0 53.0 54.0 55.0 56.0 57.0 58.0 59.0 60.0 61.0 62.0 63.0 64.0 65.0 66.0 67.0 68.0 69.0 70.0 71.0 72.0 73.0 74.0 75.0 76.0 77.0 78.0 79.0 80.0 81.0 82.0 83.0 84.0 85.0 86.0 87.0 88.0 89.0 90.0 91.0 92.0 93.0 94.0 95.0 96.0 97.0 98.0 99.0 100.0 101.0 102.0 103.0 104.0 105.0 106.0 107.0 108.0 109.0 110.0 111.0 112.0 113.0 114.0 115.0 116.0 117.0 118.0 119.0 120.0 121.0 122.0 123.0 124.0 125.0 126.0 127.0 128.0 129.0 130.0 131.0 132.0 133.0 134.0 135.0 136.0 137.0 138.0 139.0 140.0 141.0 142.0 143.0 144.0 145.0 146.0 147.0 148.0 149.0 150.0 151.0 152.0 153.0 154.0 155.0 156.0 157.0 158.0 159.0 160.0 161.0 162.0 163.0 164.0 165.0 166.0 167.0 168.0 169.0 170.0 171.0 172.0 173.0 174.0 175.0 176.0 177.0 178.0 179.0 180.0 181.0 182.0 183.0 184.0 185.0 186.0 187.0 188.0 189.0 190.0 191.0 192.0 193.0 194.0 195.0 196.0 197.0 198.0 199.0 200.0 201.0 202.0 203.0 204.0 205.0 206.0 207.0 208.0 209.0 210.0 211.0 212.0 213.0 214.0 215.0 216.0 217.0 218.0 219.0 220.0 221.0 222.0 223.0 224.0 225.0 226.0 227.0 228.0 229.0 230.0 231.0 232.0 233.0 234.0 235.0 236.0 237.0 238.0 239.0 240.0 241.0 242.0 243.0 244.0 245.0 246.0 247.0 248.0 249.0 250.0 251.0 252.0 253.0 254.0 255.0 -------------------------------------------------------------------------------- /tests/TUMVI/tumvi.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /tests/TUMVI/vignette0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IRVLab/SOS-SLAM/a4849ab67d024c9f2dd110e0de70dd1f3dbfa66b/tests/TUMVI/vignette0.png -------------------------------------------------------------------------------- /tests/TUMVI/vignette1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IRVLab/SOS-SLAM/a4849ab67d024c9f2dd110e0de70dd1f3dbfa66b/tests/TUMVI/vignette1.png -------------------------------------------------------------------------------- /thirdparty/Sophus/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET(PROJECT_NAME Sophus) 2 | 3 | PROJECT(${PROJECT_NAME}) 4 | CMAKE_MINIMUM_REQUIRED(VERSION 2.6) 5 | 6 | SET (CMAKE_VERBOSE_MAKEFILE ON) 7 | 8 | # Release by default 9 | # Turn on Debug with "-DCMAKE_BUILD_TYPE=Debug" 10 | IF( NOT CMAKE_BUILD_TYPE ) 11 | SET( CMAKE_BUILD_TYPE Release ) 12 | ENDIF() 13 | 14 | IF (CMAKE_COMPILER_IS_GNUCXX ) 15 | SET(CMAKE_CXX_FLAGS_DEBUG "-O0 -g") 16 | SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG ") 17 | 18 | ADD_DEFINITIONS("-Wall -Werror -Wno-unused-variable 19 | -Wno-unused-but-set-variable -Wno-unknown-pragmas ") 20 | ENDIF() 21 | 22 | ################################################################################ 23 | # Add local path for finding packages, set the local version first 24 | set( CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake_modules" ) 25 | list( APPEND CMAKE_MODULE_PATH "${CMAKE_ROOT}/Modules" ) 26 | 27 | ################################################################################ 28 | # Create variables used for exporting in SophusConfig.cmake 29 | set( Sophus_LIBRARIES "" ) 30 | set( Sophus_INCLUDE_DIR ${PROJECT_SOURCE_DIR} ) 31 | 32 | ################################################################################ 33 | 34 | include(FindEigen3.cmake) 35 | #find_package( Eigen3 REQUIRED ) 36 | INCLUDE_DIRECTORIES( ${EIGEN3_INCLUDE_DIR} ) 37 | SET( Sophus_INCLUDE_DIR ${Sophus_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} ) 38 | 39 | SET (SOURCE_DIR "sophus") 40 | 41 | SET (TEMPLATES tests 42 | so2 43 | se2 44 | so3 45 | se3 46 | rxso3 47 | sim3 48 | ) 49 | 50 | SET (SOURCES ${SOURCE_DIR}/sophus.hpp) 51 | 52 | FOREACH(templ ${TEMPLATES}) 53 | LIST(APPEND SOURCES ${SOURCE_DIR}/${templ}.hpp) 54 | ENDFOREACH(templ) 55 | 56 | 57 | INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) 58 | 59 | # Added ${SOURCES} to executables so they show up in QtCreator (and possibly 60 | # other IDEs). 61 | # ADD_EXECUTABLE(test_so2 sophus/test_so2.cpp ${SOURCES}) 62 | # ADD_EXECUTABLE(test_se2 sophus/test_se2.cpp ${SOURCES}) 63 | # ADD_EXECUTABLE(test_so3 sophus/test_so3.cpp ${SOURCES}) 64 | # ADD_EXECUTABLE(test_se3 sophus/test_se3.cpp ${SOURCES}) 65 | # ADD_EXECUTABLE(test_rxso3 sophus/test_rxso3.cpp ${SOURCES}) 66 | # ADD_EXECUTABLE(test_sim3 sophus/test_sim3.cpp ${SOURCES}) 67 | # ENABLE_TESTING() 68 | # 69 | # ADD_TEST(test_so2 test_so2) 70 | # ADD_TEST(test_se2 test_se2) 71 | # ADD_TEST(test_so3 test_so3) 72 | # ADD_TEST(test_se3 test_se3) 73 | # ADD_TEST(test_rxso3 test_rxso3) 74 | # ADD_TEST(test_sim3 test_sim3) 75 | 76 | ################################################################################ 77 | # Create the SophusConfig.cmake file for other cmake projects. 78 | CONFIGURE_FILE( ${CMAKE_CURRENT_SOURCE_DIR}/SophusConfig.cmake.in 79 | ${CMAKE_CURRENT_BINARY_DIR}/SophusConfig.cmake @ONLY IMMEDIATE ) 80 | export( PACKAGE Sophus ) 81 | 82 | INSTALL(DIRECTORY sophus DESTINATION ${CMAKE_INSTALL_PREFIX}/include 83 | FILES_MATCHING PATTERN "*.hpp" ) -------------------------------------------------------------------------------- /thirdparty/Sophus/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 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN3_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN3_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 79 | 80 | endif(EIGEN3_INCLUDE_DIR) 81 | 82 | -------------------------------------------------------------------------------- /thirdparty/Sophus/README: -------------------------------------------------------------------------------- 1 | Sophus (version 0.9a) 2 | 3 | C++ implementation of Lie Groups using Eigen. 4 | 5 | Thanks to Steven Lovegrove, Sophus is now fully templated - using the Curiously Recurring Template Pattern (CRTP). 6 | 7 | (In order to go back to the non-templated/double-only version "git checkout a621ff".) 8 | 9 | Installation guide: 10 | 11 | >>> 12 | cd Sophus 13 | mkdir build 14 | cd build 15 | cmake .. 16 | make 17 | <<< 18 | 19 | 20 | -------------------------------------------------------------------------------- /thirdparty/Sophus/SophusConfig.cmake.in: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Sophus source dir 3 | set( Sophus_SOURCE_DIR "@CMAKE_CURRENT_SOURCE_DIR@") 4 | 5 | ################################################################################ 6 | # Sophus build dir 7 | set( Sophus_DIR "@CMAKE_CURRENT_BINARY_DIR@") 8 | 9 | ################################################################################ 10 | set( Sophus_INCLUDE_DIR "@Sophus_INCLUDE_DIR@" ) 11 | set( Sophus_INCLUDE_DIRS "@Sophus_INCLUDE_DIR@" ) -------------------------------------------------------------------------------- /thirdparty/Sophus/cmake_modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # Once done this will define 3 | # 4 | # EIGEN3_FOUND - system has eigen lib 5 | # EIGEN3_INCLUDE_DIR - the eigen include directory 6 | 7 | # Copyright (c) 2006, 2007 Montel Laurent, 8 | # Redistribution and use is allowed according to the terms of the BSD license. 9 | # For details see the accompanying COPYING-CMAKE-SCRIPTS file. 10 | 11 | if( EIGEN3_INCLUDE_DIR ) 12 | # in cache already 13 | set( EIGEN3_FOUND TRUE ) 14 | else (EIGEN3_INCLUDE_DIR) 15 | find_path( EIGEN3_INCLUDE_DIR NAMES Eigen/Core 16 | PATH_SUFFIXES eigen3/ 17 | HINTS 18 | ${INCLUDE_INSTALL_DIR} 19 | /usr/local/include 20 | ${KDE4_INCLUDE_DIR} 21 | ) 22 | include( FindPackageHandleStandardArgs ) 23 | find_package_handle_standard_args( Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR ) 24 | mark_as_advanced( EIGEN3_INCLUDE_DIR ) 25 | endif(EIGEN3_INCLUDE_DIR) 26 | 27 | -------------------------------------------------------------------------------- /thirdparty/Sophus/sophus/sophus.hpp: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2013 Hauke Strasdat 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #ifndef SOPHUS_HPP 24 | #define SOPHUS_HPP 25 | 26 | #include 27 | 28 | // fix log1p not being found on Android in Eigen 29 | #if defined( ANDROID ) 30 | #include 31 | namespace std { 32 | using ::log1p; 33 | } 34 | #endif 35 | 36 | #include 37 | #include 38 | 39 | namespace Sophus { 40 | using namespace Eigen; 41 | 42 | template 43 | struct SophusConstants { 44 | EIGEN_ALWAYS_INLINE static 45 | const Scalar epsilon() { 46 | return static_cast(1e-10); 47 | } 48 | 49 | EIGEN_ALWAYS_INLINE static 50 | const Scalar pi() { 51 | return static_cast(M_PI); 52 | } 53 | }; 54 | 55 | template<> 56 | struct SophusConstants { 57 | EIGEN_ALWAYS_INLINE static 58 | float epsilon() { 59 | return static_cast(1e-5); 60 | } 61 | 62 | EIGEN_ALWAYS_INLINE static 63 | float pi() { 64 | return static_cast(M_PI); 65 | } 66 | }; 67 | 68 | class SophusException : public std::runtime_error { 69 | public: 70 | SophusException (const std::string& str) 71 | : runtime_error("Sophus exception: " + str) { 72 | } 73 | }; 74 | 75 | } 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /thirdparty/Sophus/sophus/test_rxso3.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2012-2013 Hauke Strasdat 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #include 24 | #include 25 | 26 | 27 | #include "rxso3.hpp" 28 | #include "tests.hpp" 29 | 30 | using namespace Sophus; 31 | using namespace std; 32 | 33 | template 34 | void tests() { 35 | 36 | typedef RxSO3Group RxSO3Type; 37 | typedef typename RxSO3Group::Point Point; 38 | typedef typename RxSO3Group::Tangent Tangent; 39 | 40 | vector rxso3_vec; 41 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.2, 0.5, 0.0, 1.))); 42 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.2, 0.5, -1.0, 1.1))); 43 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0., 1.1))); 44 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0.00001, 0.))); 45 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0.00001, 0.00001))); 46 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0.00001, 0))); 47 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(M_PI, 0, 0, 0.9))); 48 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.2, 0.5, 0.0,0)) 49 | *RxSO3Type::exp(Tangent(M_PI, 0, 0,0.0)) 50 | *RxSO3Type::exp(Tangent(-0.2, -0.5, -0.0,0))); 51 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.3, 0.5, 0.1,0)) 52 | *RxSO3Type::exp(Tangent(M_PI, 0, 0,0)) 53 | *RxSO3Type::exp(Tangent(-0.3, -0.5, -0.1,0))); 54 | 55 | vector tangent_vec; 56 | Tangent tmp; 57 | tmp << 0,0,0,0; 58 | tangent_vec.push_back(tmp); 59 | tmp << 1,0,0,0; 60 | tangent_vec.push_back(tmp); 61 | tmp << 1,0,0,0.1; 62 | tangent_vec.push_back(tmp); 63 | tmp << 0,1,0,0.1; 64 | tangent_vec.push_back(tmp); 65 | tmp << 0,0,1,-0.1; 66 | tangent_vec.push_back(tmp); 67 | tmp << -1,1,0,-0.1; 68 | tangent_vec.push_back(tmp); 69 | tmp << 20,-1,0,2; 70 | tangent_vec.push_back(tmp); 71 | 72 | vector point_vec; 73 | point_vec.push_back(Point(1,2,4)); 74 | 75 | Tests tests; 76 | tests.setGroupElements(rxso3_vec); 77 | tests.setTangentVectors(tangent_vec); 78 | tests.setPoints(point_vec); 79 | 80 | tests.runAllTests(); 81 | } 82 | 83 | int main() { 84 | cerr << "Test RxSO3" << endl << endl; 85 | 86 | cerr << "Double tests: " << endl; 87 | tests(); 88 | 89 | cerr << "Float tests: " << endl; 90 | tests(); 91 | return 0; 92 | } 93 | -------------------------------------------------------------------------------- /thirdparty/Sophus/sophus/test_se2.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2012-2013 Hauke Strasdat 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include "se2.hpp" 28 | #include "tests.hpp" 29 | 30 | using namespace Sophus; 31 | using namespace std; 32 | 33 | template 34 | void tests() { 35 | 36 | typedef SO2Group SO2Type; 37 | typedef SE2Group SE2Type; 38 | typedef typename SE2Group::Point Point; 39 | typedef typename SE2Group::Tangent Tangent; 40 | 41 | vector se2_vec; 42 | se2_vec.push_back(SE2Type(SO2Type(0.0),Point(0,0))); 43 | se2_vec.push_back(SE2Type(SO2Type(0.2),Point(10,0))); 44 | se2_vec.push_back(SE2Type(SO2Type(0.),Point(0,100))); 45 | se2_vec.push_back(SE2Type(SO2Type(-1.),Point(20,-1))); 46 | se2_vec.push_back(SE2Type(SO2Type(0.00001), 47 | Point(-0.00000001,0.0000000001))); 48 | se2_vec.push_back(SE2Type(SO2Type(0.2),Point(0,0)) 49 | *SE2Type(SO2Type(M_PI),Point(0,0)) 50 | *SE2Type(SO2Type(-0.2),Point(0,0))); 51 | se2_vec.push_back(SE2Type(SO2Type(0.3),Point(2,0)) 52 | *SE2Type(SO2Type(M_PI),Point(0,0)) 53 | *SE2Type(SO2Type(-0.3),Point(0,6))); 54 | 55 | vector tangent_vec; 56 | Tangent tmp; 57 | tmp << 0,0,0; 58 | tangent_vec.push_back(tmp); 59 | tmp << 1,0,0; 60 | tangent_vec.push_back(tmp); 61 | tmp << 0,1,1; 62 | tangent_vec.push_back(tmp); 63 | tmp << -1,1,0; 64 | tangent_vec.push_back(tmp); 65 | tmp << 20,-1,-1; 66 | tangent_vec.push_back(tmp); 67 | tmp << 30,5,20; 68 | tangent_vec.push_back(tmp); 69 | 70 | vector point_vec; 71 | point_vec.push_back(Point(1,2)); 72 | 73 | Tests tests; 74 | tests.setGroupElements(se2_vec); 75 | tests.setTangentVectors(tangent_vec); 76 | tests.setPoints(point_vec); 77 | 78 | tests.runAllTests(); 79 | } 80 | 81 | int main() { 82 | cerr << "Test SE2" << endl << endl; 83 | 84 | cerr << "Double tests: " << endl; 85 | tests(); 86 | 87 | cerr << "Float tests: " << endl; 88 | tests(); 89 | return 0; 90 | } 91 | -------------------------------------------------------------------------------- /thirdparty/Sophus/sophus/test_so2.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2012-2013 Hauke Strasdat 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | 24 | #include 25 | #include 26 | 27 | #include "so2.hpp" 28 | #include "tests.hpp" 29 | 30 | using namespace Sophus; 31 | using namespace std; 32 | 33 | template 34 | void tests() { 35 | 36 | typedef SO2Group SO2Type; 37 | typedef typename SO2Group::Point Point; 38 | typedef typename SO2Group::Tangent Tangent; 39 | 40 | vector so2_vec; 41 | so2_vec.push_back(SO2Type::exp(0.0)); 42 | so2_vec.push_back(SO2Type::exp(0.2)); 43 | so2_vec.push_back(SO2Type::exp(10.)); 44 | so2_vec.push_back(SO2Type::exp(0.00001)); 45 | so2_vec.push_back(SO2Type::exp(M_PI)); 46 | so2_vec.push_back(SO2Type::exp(0.2) 47 | *SO2Type::exp(M_PI) 48 | *SO2Type::exp(-0.2)); 49 | so2_vec.push_back(SO2Type::exp(-0.3) 50 | *SO2Type::exp(M_PI) 51 | *SO2Type::exp(0.3)); 52 | 53 | vector tangent_vec; 54 | tangent_vec.push_back(Tangent(0)); 55 | tangent_vec.push_back(Tangent(1)); 56 | tangent_vec.push_back(Tangent(M_PI_2)); 57 | tangent_vec.push_back(Tangent(-1)); 58 | tangent_vec.push_back(Tangent(20)); 59 | tangent_vec.push_back(Tangent(M_PI_2+0.0001)); 60 | 61 | vector point_vec; 62 | point_vec.push_back(Point(1,2)); 63 | 64 | Tests tests; 65 | tests.setGroupElements(so2_vec); 66 | tests.setTangentVectors(tangent_vec); 67 | tests.setPoints(point_vec); 68 | 69 | tests.runAllTests(); 70 | 71 | cerr << "Exception test: "; 72 | try { 73 | SO2Type so2(0., 0.); 74 | } catch(SophusException & e) { 75 | cerr << "passed." << endl << endl; 76 | return; 77 | } 78 | cerr << "failed!" << endl << endl; 79 | exit(-1); 80 | } 81 | 82 | int main() { 83 | cerr << "Test SO2" << endl << endl; 84 | 85 | cerr << "Double tests: " << endl; 86 | tests(); 87 | 88 | cerr << "Float tests: " << endl; 89 | tests(); 90 | return 0; 91 | } 92 | -------------------------------------------------------------------------------- /thirdparty/Sophus/sophus/test_so3.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2012-2013 Hauke Strasdat 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #include 24 | #include 25 | 26 | #include "so3.hpp" 27 | #include "tests.hpp" 28 | 29 | using namespace Sophus; 30 | using namespace std; 31 | 32 | template 33 | void tests() { 34 | 35 | typedef SO3Group SO3Type; 36 | typedef typename SO3Group::Point Point; 37 | typedef typename SO3Group::Tangent Tangent; 38 | 39 | vector so3_vec; 40 | 41 | so3_vec.push_back(SO3Type(Quaternion(0.1e-11, 0., 1., 0.))); 42 | so3_vec.push_back(SO3Type(Quaternion(-1,0.00001,0.0,0.0))); 43 | so3_vec.push_back(SO3Type::exp(Point(0.2, 0.5, 0.0))); 44 | so3_vec.push_back(SO3Type::exp(Point(0.2, 0.5, -1.0))); 45 | so3_vec.push_back(SO3Type::exp(Point(0., 0., 0.))); 46 | so3_vec.push_back(SO3Type::exp(Point(0., 0., 0.00001))); 47 | so3_vec.push_back(SO3Type::exp(Point(M_PI, 0, 0))); 48 | so3_vec.push_back(SO3Type::exp(Point(0.2, 0.5, 0.0)) 49 | *SO3Type::exp(Point(M_PI, 0, 0)) 50 | *SO3Type::exp(Point(-0.2, -0.5, -0.0))); 51 | so3_vec.push_back(SO3Type::exp(Point(0.3, 0.5, 0.1)) 52 | *SO3Type::exp(Point(M_PI, 0, 0)) 53 | *SO3Type::exp(Point(-0.3, -0.5, -0.1))); 54 | 55 | vector tangent_vec; 56 | tangent_vec.push_back(Tangent(0,0,0)); 57 | tangent_vec.push_back(Tangent(1,0,0)); 58 | tangent_vec.push_back(Tangent(0,1,0)); 59 | tangent_vec.push_back(Tangent(M_PI_2,M_PI_2,0.0)); 60 | tangent_vec.push_back(Tangent(-1,1,0)); 61 | tangent_vec.push_back(Tangent(20,-1,0)); 62 | tangent_vec.push_back(Tangent(30,5,-1)); 63 | 64 | vector point_vec; 65 | point_vec.push_back(Point(1,2,4)); 66 | 67 | Tests tests; 68 | tests.setGroupElements(so3_vec); 69 | tests.setTangentVectors(tangent_vec); 70 | tests.setPoints(point_vec); 71 | 72 | tests.runAllTests(); 73 | } 74 | 75 | int main() { 76 | cerr << "Test SO3" << endl << endl; 77 | 78 | cerr << "Double tests: " << endl; 79 | tests(); 80 | 81 | cerr << "Float tests: " << endl; 82 | tests(); 83 | return 0; 84 | } 85 | -------------------------------------------------------------------------------- /thirdparty/g2o/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | set(CMAKE_LEGACY_CYGWIN_WIN32 0) 3 | 4 | project(g2o) 5 | 6 | set(g2o_C_FLAGS) 7 | set(g2o_CXX_FLAGS) 8 | 9 | set(LIB_PREFIX g2o_) 10 | 11 | if(NOT CMAKE_BUILD_TYPE) 12 | set(CMAKE_BUILD_TYPE Release) 13 | endif(NOT CMAKE_BUILD_TYPE) 14 | 15 | set(G2O_LIB_TYPE SHARED) 16 | 17 | set(g2o_LIBRARY_OUTPUT_DIRECTORY ${g2o_SOURCE_DIR}/lib CACHE PATH "Target for the libraries") 18 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${g2o_LIBRARY_OUTPUT_DIRECTORY}) 19 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${g2o_LIBRARY_OUTPUT_DIRECTORY}) 20 | 21 | list(APPEND CMAKE_MODULE_PATH ${g2o_SOURCE_DIR}/cmake_modules) 22 | 23 | if(UNIX) 24 | add_definitions(-DUNIX) 25 | message(STATUS "Compiling on Unix") 26 | endif(UNIX) 27 | 28 | set(G2O_USE_OPENMP OFF CACHE BOOL "Build g2o with OpenMP support (EXPERIMENTAL)") 29 | if(G2O_USE_OPENMP) 30 | find_package(OpenMP) 31 | if(OPENMP_FOUND) 32 | set (G2O_OPENMP 1) 33 | set(g2o_C_FLAGS "${g2o_C_FLAGS} ${OpenMP_C_FLAGS}") 34 | set(g2o_CXX_FLAGS "${g2o_CXX_FLAGS} -DEIGEN_DONT_PARALLELIZE ${OpenMP_CXX_FLAGS}") 35 | message(STATUS "Compiling with OpenMP support") 36 | endif(OPENMP_FOUND) 37 | endif(G2O_USE_OPENMP) 38 | 39 | set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native") 40 | set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -march=native") 41 | 42 | set(g2o_C_FLAGS "${g2o_C_FLAGS} -Wall -W") 43 | set(g2o_CXX_FLAGS "${g2o_CXX_FLAGS} -Wall -W") 44 | 45 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${g2o_CXX_FLAGS}") 46 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${g2o_C_FLAGS}") 47 | 48 | find_package(Eigen3 3.3 REQUIRED) 49 | if (TARGET Eigen3::Eigen) 50 | set(G2O_EIGEN3_EIGEN_TARGET Eigen3::Eigen) 51 | else() 52 | include_directories(${EIGEN3_INCLUDE_DIR}) 53 | endif () 54 | 55 | include_directories(${g2o_SOURCE_DIR}) 56 | 57 | set(G2O_CXX_COMPILER "${CMAKE_CXX_COMPILER_ID} ${CMAKE_CXX_COMPILER}") 58 | configure_file(config.h.in "${g2o_SOURCE_DIR}/g2o/config.h") 59 | 60 | add_subdirectory(g2o/core) 61 | add_subdirectory(g2o/solvers/eigen) 62 | add_subdirectory(g2o/stuff) 63 | add_subdirectory(g2o/types/slam3d) 64 | -------------------------------------------------------------------------------- /thirdparty/g2o/README.md: -------------------------------------------------------------------------------- 1 | You should have received this g2o version along with [SOS-SLAM](https://github.com/IRVLab/SOS-SLAM). 2 | See the original [g2o library](https://github.com/RainerKuemmerle/g2o). -------------------------------------------------------------------------------- /thirdparty/g2o/build.sh: -------------------------------------------------------------------------------- 1 | mkdir build 2 | cd build 3 | cmake .. 4 | make -j4 -------------------------------------------------------------------------------- /thirdparty/g2o/cmake_modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | # specific additional paths for some OS 65 | if (WIN32) 66 | set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include") 67 | endif(WIN32) 68 | 69 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 70 | PATHS 71 | include 72 | ${EIGEN_ADDITIONAL_SEARCH_PATHS} 73 | ${KDE4_INCLUDE_DIR} 74 | PATH_SUFFIXES eigen3 eigen 75 | ) 76 | 77 | if(EIGEN3_INCLUDE_DIR) 78 | _eigen3_check_version() 79 | endif(EIGEN3_INCLUDE_DIR) 80 | 81 | include(FindPackageHandleStandardArgs) 82 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 83 | 84 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 85 | 86 | endif(EIGEN3_INCLUDE_DIR) 87 | 88 | -------------------------------------------------------------------------------- /thirdparty/g2o/config.h.in: -------------------------------------------------------------------------------- 1 | #ifndef G2O_CONFIG_H 2 | #define G2O_CONFIG_H 3 | 4 | 5 | #cmakedefine G2O_HAVE_OPENGL 1 6 | #cmakedefine G2O_OPENGL_FOUND 1 7 | #cmakedefine G2O_OPENMP 1 8 | #cmakedefine G2O_SHARED_LIBS 1 9 | #cmakedefine G2O_LGPL_SHARED_LIBS 1 10 | 11 | // available sparse matrix libraries 12 | #cmakedefine G2O_HAVE_CHOLMOD 1 13 | #cmakedefine G2O_HAVE_CSPARSE 1 14 | 15 | #cmakedefine G2O_NO_IMPLICIT_OWNERSHIP_OF_OBJECTS 16 | 17 | #ifdef G2O_NO_IMPLICIT_OWNERSHIP_OF_OBJECTS 18 | #define G2O_DELETE_IMPLICITLY_OWNED_OBJECTS 0 19 | #else 20 | #define G2O_DELETE_IMPLICITLY_OWNED_OBJECTS 1 21 | #endif 22 | 23 | #cmakedefine G2O_SINGLE_PRECISION_MATH 24 | #ifdef G2O_SINGLE_PRECISION_MATH 25 | #define G2O_NUMBER_FORMAT_STR "%g" 26 | 27 | #ifdef __cplusplus 28 | using number_t = float; 29 | #else 30 | typedef float number_t; 31 | #endif 32 | #else 33 | #define G2O_NUMBER_FORMAT_STR "%lg" 34 | 35 | #ifdef __cplusplus 36 | using number_t = double; 37 | #else 38 | typedef double number_t; 39 | #endif 40 | #endif 41 | 42 | #cmakedefine G2O_CXX_COMPILER "@G2O_CXX_COMPILER@" 43 | 44 | #ifdef __cplusplus 45 | #include 46 | #endif 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/config.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_CONFIG_H 2 | #define G2O_CONFIG_H 3 | 4 | 5 | /* #undef G2O_HAVE_OPENGL */ 6 | /* #undef G2O_OPENGL_FOUND */ 7 | /* #undef G2O_OPENMP */ 8 | /* #undef G2O_SHARED_LIBS */ 9 | /* #undef G2O_LGPL_SHARED_LIBS */ 10 | 11 | // available sparse matrix libraries 12 | /* #undef G2O_HAVE_CHOLMOD */ 13 | /* #undef G2O_HAVE_CSPARSE */ 14 | 15 | /* #undef G2O_NO_IMPLICIT_OWNERSHIP_OF_OBJECTS */ 16 | 17 | #ifdef G2O_NO_IMPLICIT_OWNERSHIP_OF_OBJECTS 18 | #define G2O_DELETE_IMPLICITLY_OWNED_OBJECTS 0 19 | #else 20 | #define G2O_DELETE_IMPLICITLY_OWNED_OBJECTS 1 21 | #endif 22 | 23 | /* #undef G2O_SINGLE_PRECISION_MATH */ 24 | #ifdef G2O_SINGLE_PRECISION_MATH 25 | #define G2O_NUMBER_FORMAT_STR "%g" 26 | 27 | #ifdef __cplusplus 28 | using number_t = float; 29 | #else 30 | typedef float number_t; 31 | #endif 32 | #else 33 | #define G2O_NUMBER_FORMAT_STR "%lg" 34 | 35 | #ifdef __cplusplus 36 | using number_t = double; 37 | #else 38 | typedef double number_t; 39 | #endif 40 | #endif 41 | 42 | #define G2O_CXX_COMPILER "GNU /usr/bin/c++" 43 | 44 | #ifdef __cplusplus 45 | #include 46 | #endif 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(core ${G2O_LIB_TYPE} 2 | dynamic_aligned_buffer.hpp 3 | ownership.h 4 | base_edge.h 5 | base_binary_edge.h hyper_graph_action.cpp 6 | base_binary_edge.hpp hyper_graph_action.h 7 | base_multi_edge.h hyper_graph.cpp 8 | base_multi_edge.hpp hyper_graph.h 9 | base_unary_edge.h linear_solver.h 10 | base_unary_edge.hpp marginal_covariance_cholesky.cpp 11 | base_vertex.h marginal_covariance_cholesky.h 12 | base_vertex.hpp matrix_structure.cpp 13 | batch_stats.cpp matrix_structure.h 14 | batch_stats.h openmp_mutex.h 15 | block_solver.h block_solver.hpp 16 | parameter.cpp parameter.h 17 | cache.cpp cache.h 18 | optimizable_graph.cpp optimizable_graph.h 19 | solver.cpp solver.h 20 | creators.h optimization_algorithm_factory.cpp 21 | estimate_propagator.cpp optimization_algorithm_factory.h 22 | estimate_propagator.h 23 | factory.cpp optimization_algorithm_property.h 24 | factory.h sparse_block_matrix.h 25 | sparse_optimizer.cpp sparse_block_matrix.hpp 26 | sparse_optimizer.h 27 | hyper_dijkstra.cpp hyper_dijkstra.h 28 | parameter_container.cpp parameter_container.h 29 | optimization_algorithm.cpp optimization_algorithm.h 30 | optimization_algorithm_with_hessian.cpp optimization_algorithm_with_hessian.h 31 | optimization_algorithm_gauss_newton.cpp optimization_algorithm_gauss_newton.h 32 | optimization_algorithm_levenberg.cpp optimization_algorithm_levenberg.h 33 | optimization_algorithm_dogleg.cpp optimization_algorithm_dogleg.h 34 | sparse_optimizer_terminate_action.cpp sparse_optimizer_terminate_action.h 35 | jacobian_workspace.cpp jacobian_workspace.h 36 | robust_kernel.cpp robust_kernel.h 37 | robust_kernel_impl.cpp robust_kernel_impl.h 38 | robust_kernel_factory.cpp robust_kernel_factory.h 39 | io_helper.h 40 | g2o_core_api.h 41 | ) 42 | 43 | set_target_properties(core PROPERTIES OUTPUT_NAME ${LIB_PREFIX}core) 44 | target_link_libraries(core PUBLIC stuff ${G2O_EIGEN3_EIGEN_TARGET}) 45 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/base_dynamic_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_DYNAMIC_VERTEX_H 28 | #define G2O_BASE_DYNAMIC_VERTEX_H 29 | 30 | #include "base_vertex.h" 31 | 32 | namespace g2o { 33 | template 34 | class BaseDynamicVertex : public BaseVertex<-1, T> 35 | { 36 | public: 37 | 38 | inline virtual bool setDimension(int newDimension); 39 | 40 | protected: 41 | 42 | // This method is responsible for actually changing the dimension of the state 43 | virtual bool setDimensionImpl(int newDimension) = 0; 44 | 45 | using BaseVertex<-1, T>::_graph; 46 | using BaseVertex<-1, T>::_dimension; 47 | using BaseVertex<-1, T>::_b; 48 | using BaseVertex<-1, T>::_edges; 49 | using BaseVertex<-1, T>::setHessianIndex; 50 | using BaseVertex<-1, T>::mapHessianMemory; 51 | using BaseVertex<-1, T>::updateCache; 52 | 53 | }; 54 | 55 | template 56 | bool BaseDynamicVertex::setDimension(int newDimension) { 57 | // Check the dimension is non-negative. 58 | assert(newDimension >= 0); 59 | if (newDimension < 0) 60 | return false; 61 | 62 | // Nothing to do if the dimension is unchanged. 63 | if (newDimension == _dimension) 64 | return true; 65 | 66 | // Change the state to the requested dimension 67 | if (setDimensionImpl(newDimension) == false) 68 | return false; 69 | 70 | // Store the old dimension and assign the new 71 | int oldDimension = _dimension; 72 | _dimension = newDimension; 73 | 74 | // Reset the allocation associated with this vertex and update the cache 75 | setHessianIndex(-1); 76 | mapHessianMemory(nullptr); 77 | _b.resize(_dimension); 78 | updateCache(); 79 | 80 | // If the dimension is being increased and this vertex is in a 81 | // graph, update the size of the Jacobian workspace just in case it 82 | // needs to grow. 83 | if ((newDimension > oldDimension) && (_graph != nullptr)) { 84 | JacobianWorkspace& jacobianWorkspace = _graph->jacobianWorkspace(); 85 | for (auto e : _edges) 86 | jacobianWorkspace.updateSize(e); 87 | } 88 | 89 | return true; 90 | } 91 | } 92 | #endif 93 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/base_vertex.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | template 28 | BaseVertex::BaseVertex() : 29 | OptimizableGraph::Vertex(), 30 | _hessian(nullptr, D, D) 31 | { 32 | _dimension = D; 33 | } 34 | 35 | template 36 | number_t BaseVertex::solveDirect(number_t lambda) { 37 | Eigen::Matrix tempA=_hessian + Eigen::Matrix::Identity(G2O_VERTEX_DIM, G2O_VERTEX_DIM)*lambda; 38 | number_t det=tempA.determinant(); 39 | if (g2o_isnan(det) || det < std::numeric_limits::epsilon()) 40 | return det; 41 | Eigen::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(number_t* d) 53 | { 54 | const int vertexDim = G2O_VERTEX_DIM; 55 | new (&_hessian) HessianBlockType(d, vertexDim, vertexDim); 56 | } 57 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/batch_stats.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "batch_stats.h" 28 | #include 29 | 30 | namespace g2o { 31 | using namespace std; 32 | 33 | G2OBatchStatistics* G2OBatchStatistics::_globalStats=0; 34 | 35 | #ifndef PTHING 36 | #define PTHING(s) \ 37 | #s << "= " << (st.s) << "\t " 38 | #endif 39 | 40 | G2OBatchStatistics::G2OBatchStatistics(){ 41 | // zero all. 42 | memset (this, 0, sizeof(G2OBatchStatistics)); 43 | 44 | // set the iteration to -1 to show that it isn't valid 45 | iteration = -1; 46 | } 47 | 48 | std::ostream& operator << (std::ostream& os , const G2OBatchStatistics& st) 49 | { 50 | os << PTHING(iteration); 51 | 52 | os << PTHING( numVertices ); // how many vertices are involved 53 | os << PTHING( numEdges ); // hoe many edges 54 | os << PTHING( chi2 ); // total chi2 55 | 56 | /** timings **/ 57 | // nonlinear part 58 | os << PTHING( timeResiduals ); 59 | os << PTHING( timeLinearize ); // jacobians 60 | os << PTHING( timeQuadraticForm ); // construct the quadratic form in the graph 61 | 62 | // block_solver (constructs Ax=b, plus maybe schur); 63 | os << PTHING( timeSchurComplement ); // compute schur complement (0 if not done); 64 | 65 | // linear solver (computes Ax=b); ); 66 | os << PTHING( timeSymbolicDecomposition ); // symbolic decomposition (0 if not done); 67 | os << PTHING( timeNumericDecomposition ); // numeric decomposition (0 if not done); 68 | os << PTHING( timeLinearSolution ); // total time for solving Ax=b 69 | os << PTHING( iterationsLinearSolver ); // iterations of PCG 70 | os << PTHING( timeUpdate ); // oplus 71 | os << PTHING( timeIteration ); // total time ); 72 | 73 | os << PTHING( levenbergIterations ); 74 | os << PTHING( timeLinearSolver); 75 | 76 | os << PTHING(hessianDimension); 77 | os << PTHING(hessianPoseDimension); 78 | os << PTHING(hessianLandmarkDimension); 79 | os << PTHING(choleskyNNZ); 80 | os << PTHING(timeMarginals); 81 | 82 | return os; 83 | }; 84 | 85 | void G2OBatchStatistics::setGlobalStats(G2OBatchStatistics* b) 86 | { 87 | _globalStats = b; 88 | } 89 | 90 | } // end namespace 91 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/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 G2O_CORE_API AbstractHyperGraphElementCreator 42 | { 43 | public: 44 | /** 45 | * create a hyper graph element. Has to implemented in derived class. 46 | */ 47 | virtual HyperGraph::HyperGraphElement* construct() = 0; 48 | /** 49 | * name of the class to be created. Has to implemented in derived class. 50 | */ 51 | virtual const std::string& name() const = 0; 52 | 53 | virtual ~AbstractHyperGraphElementCreator() { } 54 | }; 55 | 56 | /** 57 | * \brief templatized creator class which creates graph elements 58 | */ 59 | template 60 | class HyperGraphElementCreator : public AbstractHyperGraphElementCreator 61 | { 62 | public: 63 | HyperGraphElementCreator() : _name(typeid(T).name()) {} 64 | #if defined (WINDOWS) && defined(__GNUC__) // force stack alignment on Windows with GCC 65 | __attribute__((force_align_arg_pointer)) 66 | #endif 67 | HyperGraph::HyperGraphElement* construct() { return new T;} 68 | virtual const std::string& name() const { return _name;} 69 | protected: 70 | std::string _name; 71 | }; 72 | 73 | } // end namespace 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/dynamic_aligned_buffer.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "Eigen/Core" 6 | 7 | namespace g2o 8 | { 9 | // 16 byte aligned allocation functions 10 | template 11 | Type* allocate_aligned(size_t n) 12 | { 13 | return (Type*)Eigen::internal::aligned_malloc(n * sizeof(Type)); 14 | } 15 | 16 | template 17 | Type* reallocate_aligned(Type* ptr, size_t newSize, size_t oldSize) 18 | { 19 | return (Type*)Eigen::internal::aligned_realloc(ptr, newSize * sizeof(Type), oldSize * sizeof(Type)); 20 | } 21 | 22 | template 23 | void free_aligned(Type* block) 24 | { 25 | Eigen::internal::aligned_free(block); 26 | } 27 | 28 | template 29 | struct dynamic_aligned_buffer 30 | { 31 | explicit dynamic_aligned_buffer(size_t size) 32 | : m_size{ 0 }, m_ptr{ nullptr } 33 | { 34 | allocate(size); 35 | } 36 | 37 | ~dynamic_aligned_buffer() 38 | { 39 | free(); 40 | } 41 | 42 | Type* request(size_t n) 43 | { 44 | if (n <= m_size) 45 | return m_ptr; 46 | 47 | m_ptr = reallocate_aligned(m_ptr, n, m_size); 48 | m_size = m_ptr ? n : 0; 49 | 50 | return m_ptr; 51 | } 52 | 53 | private: 54 | void allocate(size_t size) 55 | { 56 | m_ptr = allocate_aligned(size); 57 | if (m_ptr != nullptr) 58 | m_size = size; 59 | } 60 | 61 | void free() 62 | { 63 | if (m_ptr != nullptr) 64 | { 65 | free_aligned(m_ptr); 66 | m_size = 0; 67 | m_ptr = nullptr; 68 | } 69 | } 70 | 71 | std::size_t m_size; 72 | Type* m_ptr; 73 | }; 74 | 75 | template 76 | struct aligned_deleter 77 | { 78 | void operator()(T* block) 79 | { 80 | free_aligned(block); 81 | } 82 | }; 83 | } 84 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/g2o_core_api.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Description: import/export macros for creating DLLS with Microsoft 3 | * compiler. Any exported function needs to be declared with the 4 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 5 | * for each DLL (arrrrrgh!!!) 6 | * 7 | * 17 Jan 2012 8 | * Email: pupilli@cs.bris.ac.uk 9 | ****************************************************************************/ 10 | #ifndef G2O_CORE_API_H 11 | #define G2O_CORE_API_H 12 | 13 | #include "g2o/config.h" 14 | 15 | #ifdef _MSC_VER 16 | // We are using a Microsoft compiler: 17 | #ifdef G2O_SHARED_LIBS 18 | #ifdef core_EXPORTS 19 | #define G2O_CORE_API __declspec(dllexport) 20 | #else 21 | #define G2O_CORE_API __declspec(dllimport) 22 | #endif 23 | #else 24 | #define G2O_CORE_API 25 | #endif 26 | 27 | #else 28 | // Not Microsoft compiler so set empty definition: 29 | #define G2O_CORE_API 30 | #endif 31 | 32 | #endif // G2O_CORE_API_H 33 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/io_helper.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2014 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_IO_HELPER_H 28 | #define G2O_CORE_IO_HELPER_H 29 | 30 | #include 31 | 32 | namespace g2o { 33 | namespace internal { 34 | template 35 | bool writeVector(std::ostream& os, const Eigen::DenseBase& b) { 36 | for (int i = 0; i < b.size(); i++) os << b(i) << " "; 37 | return os.good(); 38 | } 39 | 40 | template 41 | bool readVector(std::istream& is, Eigen::DenseBase& b) { 42 | for (int i = 0; i < b.size() && is.good(); i++) is >> b(i); 43 | return is.good() || is.eof(); 44 | } 45 | } // namespace internal 46 | } // namespace g2o 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/matrix_operations.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_CORE_MATRIX_OPERATIONS_H 28 | #define G2O_CORE_MATRIX_OPERATIONS_H 29 | 30 | #include 31 | 32 | namespace g2o { 33 | namespace internal { 34 | 35 | template 36 | inline void axpy(const MatrixType& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 37 | { 38 | y.segment(yoff) += A * x.segment(xoff); 39 | } 40 | 41 | template 42 | inline void axpy(const Eigen::Matrix& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 43 | { 44 | y.segment(yoff, A.rows()) += A * x.segment::ColsAtCompileTime>(xoff); 45 | } 46 | 47 | template<> 48 | inline void axpy(const MatrixX& 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 MatrixX& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 67 | { 68 | y.segment(yoff, A.cols()) += A.transpose() * x.segment(xoff, A.rows()); 69 | } 70 | 71 | } // end namespace internal 72 | } // end namespace g2o 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/matrix_structure.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 | #include "g2o_core_api.h" 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief representing the structure of a matrix in column compressed structure (only the upper triangular part of the matrix) 36 | */ 37 | class G2O_CORE_API MatrixStructure 38 | { 39 | public: 40 | MatrixStructure(); 41 | ~MatrixStructure(); 42 | /** 43 | * allocate space for the Matrix Structure. You may call this on an already allocated struct, it will 44 | * then reallocate the memory + additional space (number_t the required space). 45 | */ 46 | void alloc(int n_, int nz); 47 | 48 | void free(); 49 | 50 | /** 51 | * Write the matrix pattern to a file. File is also loadable by octave, e.g., then use spy(matrix) 52 | */ 53 | bool write(const char* filename) const; 54 | 55 | int n; ///< A is m-by-n. n must be >= 0. 56 | int m; ///< A is m-by-n. m must be >= 0. 57 | int* Ap; ///< column pointers for A, of size n+1 58 | int* Aii; ///< row indices of A, of size nz = Ap [n] 59 | 60 | //! max number of non-zeros blocks 61 | int nzMax() const { return maxNz;} 62 | 63 | protected: 64 | int maxN; ///< size of the allocated memory 65 | int maxNz; ///< size of the allocated memory 66 | }; 67 | 68 | } // end namespace 69 | 70 | #endif 71 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/openmp_mutex.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPENMP_MUTEX 28 | #define G2O_OPENMP_MUTEX 29 | 30 | #include "g2o/config.h" 31 | 32 | #ifdef G2O_OPENMP 33 | #include 34 | #else 35 | #include 36 | #endif 37 | 38 | namespace g2o { 39 | 40 | #ifdef G2O_OPENMP 41 | 42 | /** 43 | * \brief Mutex realized via OpenMP 44 | */ 45 | class OpenMPMutex 46 | { 47 | public: 48 | OpenMPMutex() { omp_init_lock(&_lock); } 49 | ~OpenMPMutex() { omp_destroy_lock(&_lock); } 50 | void lock() { omp_set_lock(&_lock); } 51 | void unlock() { omp_unset_lock(&_lock); } 52 | protected: 53 | omp_lock_t _lock; 54 | }; 55 | 56 | #else 57 | 58 | /* 59 | * dummy which does nothing in case we don't have OpenMP support. 60 | * In debug mode, the mutex allows to verify the correct lock and unlock behavior 61 | */ 62 | class OpenMPMutex 63 | { 64 | public: 65 | #ifdef NDEBUG 66 | OpenMPMutex() {} 67 | #else 68 | OpenMPMutex() : _cnt(0) {} 69 | #endif 70 | ~OpenMPMutex() { assert(_cnt == 0 && "Freeing locked mutex");} 71 | void lock() { assert(++_cnt == 1 && "Locking already locked mutex");} 72 | void unlock() { assert(--_cnt == 0 && "Trying to unlock a mutex which is not locked");} 73 | protected: 74 | #ifndef NDEBUG 75 | char _cnt; 76 | #endif 77 | }; 78 | 79 | #endif 80 | 81 | /** 82 | * \brief lock a mutex within a scope 83 | */ 84 | class ScopedOpenMPMutex 85 | { 86 | public: 87 | explicit ScopedOpenMPMutex(OpenMPMutex* mutex) : _mutex(mutex) { _mutex->lock(); } 88 | ~ScopedOpenMPMutex() { _mutex->unlock(); } 89 | private: 90 | OpenMPMutex* const _mutex; 91 | ScopedOpenMPMutex(const ScopedOpenMPMutex&); 92 | void operator=(const ScopedOpenMPMutex&); 93 | }; 94 | 95 | } 96 | 97 | #endif 98 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/optimization_algorithm.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "optimization_algorithm.h" 28 | 29 | using namespace std; 30 | 31 | namespace g2o { 32 | 33 | OptimizationAlgorithm::OptimizationAlgorithm() 34 | : _optimizer(nullptr) 35 | {} 36 | 37 | OptimizationAlgorithm::~OptimizationAlgorithm() 38 | {} 39 | 40 | void OptimizationAlgorithm::printProperties(std::ostream& os) const 41 | { 42 | os << "------------- Algorithm Properties -------------" << endl; 43 | for (PropertyMap::const_iterator it = _properties.begin(); it != _properties.end(); ++it) { 44 | BaseProperty* p = it->second; 45 | os << it->first << "\t" << p->toString() << endl; 46 | } 47 | os << "------------------------------------------------" << endl; 48 | } 49 | 50 | bool OptimizationAlgorithm::updatePropertiesFromString(const std::string& propString) 51 | { 52 | return _properties.updateMapFromString(propString); 53 | } 54 | 55 | void OptimizationAlgorithm::setOptimizer(SparseOptimizer* optimizer) 56 | { 57 | _optimizer = optimizer; 58 | } 59 | 60 | } // end namespace 61 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H 29 | 30 | #include "g2o_core_api.h" 31 | #include "optimization_algorithm_with_hessian.h" 32 | #include 33 | 34 | namespace g2o { 35 | 36 | /** 37 | * \brief Implementation of the Gauss Newton Algorithm 38 | */ 39 | class G2O_CORE_API OptimizationAlgorithmGaussNewton : public OptimizationAlgorithmWithHessian 40 | { 41 | public: 42 | /** 43 | * construct the Gauss Newton algorithm, which use the given Solver for solving the 44 | * linearized system. 45 | */ 46 | explicit OptimizationAlgorithmGaussNewton(std::unique_ptr solver); 47 | virtual ~OptimizationAlgorithmGaussNewton(); 48 | 49 | virtual SolverResult solve(int iteration, bool online = false); 50 | 51 | virtual void printVerbose(std::ostream& os) const; 52 | 53 | private: 54 | std::unique_ptr m_solver; 55 | }; 56 | 57 | } // end namespace 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/optimization_algorithm_property.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H 29 | 30 | #include 31 | 32 | #include "g2o_core_api.h" 33 | 34 | namespace g2o { 35 | 36 | /** 37 | * \brief describe the properties of a solver 38 | */ 39 | struct G2O_CORE_API OptimizationAlgorithmProperty 40 | { 41 | std::string name; ///< name of the solver, e.g., var 42 | std::string desc; ///< short description of the solver 43 | std::string type; ///< type of solver, e.g., "CSparse Cholesky", "PCG" 44 | bool requiresMarginalize; ///< whether the solver requires marginalization of landmarks 45 | int poseDim; ///< dimension of the pose vertices (-1 if variable) 46 | int landmarkDim; ///< dimension of the landmar vertices (-1 if variable) 47 | OptimizationAlgorithmProperty() : 48 | name(), desc(), type(), requiresMarginalize(false), poseDim(-1), landmarkDim(-1) 49 | { 50 | } 51 | OptimizationAlgorithmProperty(const std::string& name_, const std::string& desc_, const std::string& type_, bool requiresMarginalize_, int poseDim_, int landmarkDim_) : 52 | name(name_), desc(desc_), type(type_), requiresMarginalize(requiresMarginalize_), poseDim(poseDim_), landmarkDim(landmarkDim_) 53 | { 54 | } 55 | }; 56 | 57 | } // end namespace 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H 29 | 30 | #include "optimization_algorithm.h" 31 | #include "g2o_core_api.h" 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 G2O_CORE_API 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 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/ownership.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_OWNERSHIP_H 2 | #define G2O_OWNERSHIP_H 3 | 4 | #include 5 | 6 | namespace g2o 7 | { 8 | template 9 | void release(T* obj) 10 | { 11 | (void)obj; 12 | #if G2O_DELETE_IMPLICITLY_OWNED_OBJECTS 13 | delete obj; 14 | #endif 15 | } 16 | } 17 | 18 | #endif -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/parameter.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "parameter.h" 28 | 29 | namespace g2o { 30 | 31 | Parameter::Parameter() : _id(-1) 32 | { 33 | } 34 | 35 | void Parameter::setId(int id_) 36 | { 37 | _id = id_; 38 | } 39 | 40 | } // end namespace 41 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/parameter.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_GRAPH_PARAMETER_HH_ 28 | #define G2O_GRAPH_PARAMETER_HH_ 29 | 30 | #include 31 | 32 | #include "hyper_graph.h" 33 | 34 | namespace g2o { 35 | 36 | class G2O_CORE_API Parameter : public HyperGraph::HyperGraphElement 37 | { 38 | public: 39 | Parameter(); 40 | virtual ~Parameter() {}; 41 | //! read the data from a stream 42 | virtual bool read(std::istream& is) = 0; 43 | //! write the data to a stream 44 | virtual bool write(std::ostream& os) const = 0; 45 | int id() const {return _id;} 46 | void setId(int id_); 47 | virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_PARAMETER;} 48 | protected: 49 | int _id; 50 | }; 51 | 52 | typedef std::vector ParameterVector; 53 | 54 | } // end namespace 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/parameter_container.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_GRAPH_PARAMETER_CONTAINER_HH_ 28 | #define G2O_GRAPH_PARAMETER_CONTAINER_HH_ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace g2o { 35 | 36 | class Parameter; 37 | 38 | /** 39 | * \brief map id to parameters 40 | */ 41 | class ParameterContainer : protected std::map 42 | { 43 | public: 44 | typedef std::map BaseClass; 45 | 46 | /** 47 | * create a container for the parameters. 48 | * @param isMainStorage_ pointers to the parameters are owned by this container, i.e., freed in its constructor 49 | */ 50 | ParameterContainer(bool isMainStorage_=true); 51 | virtual ~ParameterContainer(); 52 | //! add parameter to the container 53 | bool addParameter(Parameter* p); 54 | //! return a parameter based on its ID 55 | Parameter* getParameter(int id); 56 | //! return a parameter based on its ID 57 | const Parameter* getParameter(int id) const; 58 | //! remove a parameter from the container, i.e., the user now owns the pointer 59 | Parameter* detachParameter(int id); 60 | //! read parameters from a stream 61 | virtual bool read(std::istream& is, const std::map* renamedMap =0); 62 | //! write the data to a stream 63 | virtual bool write(std::ostream& os) const; 64 | bool isMainStorage() const {return _isMainStorage;} 65 | void clear(); 66 | 67 | // stuff of the base class that should re-appear 68 | using BaseClass::size; 69 | 70 | protected: 71 | bool _isMainStorage; 72 | }; 73 | 74 | } // end namespace 75 | 76 | #endif 77 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/robust_kernel.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "robust_kernel.h" 28 | 29 | namespace g2o { 30 | 31 | RobustKernel::RobustKernel() : 32 | _delta(1.) 33 | { 34 | } 35 | 36 | RobustKernel::RobustKernel(number_t delta) : 37 | _delta(delta) 38 | { 39 | } 40 | 41 | void RobustKernel::setDelta(number_t delta) 42 | { 43 | _delta = delta; 44 | } 45 | 46 | } // end namespace g2o 47 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/robust_kernel.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_ROBUST_KERNEL_H 28 | #define G2O_ROBUST_KERNEL_H 29 | 30 | #include 31 | 32 | #include "g2o_core_api.h" 33 | 34 | namespace g2o { 35 | 36 | /** 37 | * \brief base for all robust cost functions 38 | * 39 | * Note in all the implementations for the other cost functions the e in the 40 | * funtions corresponds to the sqaured errors, i.e., the robustification 41 | * functions gets passed the squared error. 42 | * 43 | * e.g. the robustified least squares function is 44 | * 45 | * chi^2 = sum_{e} rho( e^T Omega e ) 46 | */ 47 | class G2O_CORE_API RobustKernel 48 | { 49 | public: 50 | RobustKernel(); 51 | explicit RobustKernel(number_t delta); 52 | virtual ~RobustKernel() {} 53 | /** 54 | * compute the scaling factor for a error: 55 | * The error is e^T Omega e 56 | * The output rho is 57 | * rho[0]: The actual scaled error value 58 | * rho[1]: First derivative of the scaling function 59 | * rho[2]: Second derivative of the scaling function 60 | */ 61 | virtual void robustify(number_t squaredError, Vector3& rho) const = 0; 62 | 63 | /** 64 | * set the window size of the error. A squared error above delta^2 is considered 65 | * as outlier in the data. 66 | */ 67 | virtual void setDelta(number_t delta); 68 | number_t delta() const { return _delta;} 69 | 70 | protected: 71 | number_t _delta; 72 | }; 73 | typedef std::shared_ptr RobustKernelPtr; 74 | 75 | } // end namespace g2o 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/solver.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "solver.h" 28 | #include "dynamic_aligned_buffer.hpp" 29 | 30 | #include 31 | #include 32 | 33 | namespace g2o { 34 | 35 | Solver::Solver() : 36 | _optimizer(0), _x(0), _b(0), _xSize(0), _maxXSize(0), 37 | _isLevenberg(false), _additionalVectorSpace(0) 38 | { 39 | } 40 | 41 | Solver::~Solver() 42 | { 43 | free_aligned(_x); 44 | free_aligned(_b); 45 | } 46 | 47 | void Solver::resizeVector(size_t sx) 48 | { 49 | size_t oldSize = _xSize; 50 | _xSize = sx; 51 | sx += _additionalVectorSpace; // allocate some additional space if requested 52 | if (_maxXSize < sx) { 53 | _maxXSize = 2*sx; 54 | free_aligned(_x); 55 | _x = allocate_aligned(_maxXSize); 56 | #ifndef NDEBUG 57 | memset(_x, 0, _maxXSize * sizeof(number_t)); 58 | #endif 59 | if (_b) { // backup the former b, might still be needed for online processing 60 | memcpy(_x, _b, oldSize * sizeof(number_t)); 61 | free_aligned(_b); 62 | _b = allocate_aligned(_maxXSize); 63 | std::swap(_b, _x); 64 | } else { 65 | _b = allocate_aligned(_maxXSize); 66 | #ifndef NDEBUG 67 | memset(_b, 0, _maxXSize * sizeof(number_t)); 68 | #endif 69 | } 70 | } 71 | } 72 | 73 | void Solver::setOptimizer(SparseOptimizer* optimizer) 74 | { 75 | _optimizer = optimizer; 76 | } 77 | 78 | void Solver::setLevenberg(bool levenberg) 79 | { 80 | _isLevenberg = levenberg; 81 | } 82 | 83 | void Solver::setAdditionalVectorSpace(size_t s) 84 | { 85 | _additionalVectorSpace = s; 86 | } 87 | 88 | } // end namespace 89 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/sparse_block_matrix_test.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "sparse_block_matrix.h" 28 | #include 29 | 30 | using namespace std; 31 | using namespace g2o; 32 | using namespace Eigen; 33 | 34 | typedef SparseBlockMatrix< MatrixX > 35 | SparseBlockMatrixX; 36 | 37 | std::ostream& operator << (std::ostream& os, const SparseBlockMatrixX::SparseMatrixBlock& m) { 38 | for (int i=0; iblock(0,0, true); 55 | cerr << b->rows() << " " << b->cols() << endl; 56 | for (int i=0; irows(); ++i) 57 | for (int j=0; jcols(); ++j){ 58 | (*b)(i,j)=i*b->cols()+j; 59 | } 60 | 61 | 62 | cerr << "block access 2" << endl; 63 | b=M->block(0,2, true); 64 | cerr << b->rows() << " " << b->cols() << endl; 65 | for (int i=0; irows(); ++i) 66 | for (int j=0; jcols(); ++j){ 67 | (*b)(i,j)=i*b->cols()+j; 68 | } 69 | 70 | b=M->block(3,2, true); 71 | cerr << b->rows() << " " << b->cols() << endl; 72 | for (int i=0; irows(); ++i) 73 | for (int j=0; jcols(); ++j){ 74 | (*b)(i,j)=i*b->cols()+j; 75 | } 76 | 77 | cerr << *M << endl; 78 | 79 | cerr << "SUM" << endl; 80 | 81 | SparseBlockMatrixX* Ms=0; 82 | M->add(Ms); 83 | M->add(Ms); 84 | cerr << *Ms; 85 | 86 | SparseBlockMatrixX* Mt=0; 87 | M->transpose(Mt); 88 | cerr << *Mt << endl; 89 | 90 | SparseBlockMatrixX* Mp=0; 91 | M->multiply(Mp, Mt); 92 | cerr << *Mp << endl; 93 | 94 | int iperm[]={3,2,1,0}; 95 | SparseBlockMatrixX* PMp=0; 96 | 97 | Mp->symmPermutation(PMp,iperm, false); 98 | cerr << *PMp << endl; 99 | 100 | PMp->clear(true); 101 | Mp->block(3,0)->fill(0.); 102 | Mp->symmPermutation(PMp,iperm, true); 103 | cerr << *PMp << endl; 104 | 105 | 106 | 107 | } 108 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/core/sparse_optimizer_terminate_action.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef SPARSE_OPTIMIZER_TERMINATE_ACTION_H 28 | #define SPARSE_OPTIMIZER_TERMINATE_ACTION_H 29 | 30 | #include "g2o_core_api.h" 31 | #include "hyper_graph_action.h" 32 | 33 | namespace g2o { 34 | 35 | class SparseOptimizer; 36 | 37 | /** 38 | * \brief stop iterating based on the gain which is (oldChi - currentChi) / currentChi. 39 | * 40 | * stop iterating based on the gain which is (oldChi - currentChi) / currentChi. 41 | * If the gain is larger than zero and below the threshold, then the optimizer is stopped. 42 | * Typically usage of this action includes adding it as a postIteration action, by calling 43 | * addPostIterationAction on a sparse optimizer. 44 | */ 45 | class G2O_CORE_API SparseOptimizerTerminateAction : public HyperGraphAction 46 | { 47 | public: 48 | SparseOptimizerTerminateAction(); 49 | virtual HyperGraphAction* operator()(const HyperGraph* graph, Parameters* parameters = 0); 50 | 51 | number_t gainThreshold() const { return _gainThreshold;} 52 | void setGainThreshold(number_t gainThreshold); 53 | 54 | int maxIterations() const { return _maxIterations;} 55 | void setMaxIterations(int maxit); 56 | 57 | protected: 58 | number_t _gainThreshold; 59 | number_t _lastChi; 60 | bool _auxTerminateFlag; 61 | int _maxIterations; 62 | 63 | void setOptimizerStopFlag(const SparseOptimizer* optimizer, bool stop); 64 | }; 65 | 66 | } // end namespace 67 | 68 | #endif 69 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/solvers/eigen/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(solver_eigen ${G2O_LIB_TYPE} 2 | solver_eigen.cpp 3 | linear_solver_eigen.h 4 | ) 5 | set_target_properties(solver_eigen PROPERTIES OUTPUT_NAME ${LIB_PREFIX}solver_eigen) 6 | target_link_libraries(solver_eigen core) -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/stuff/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(stuff ${G2O_LIB_TYPE} 2 | filesys_tools.h string_tools.h 3 | color_macros.h macros.h timeutil.cpp 4 | command_args.cpp misc.h sparse_helper.cpp timeutil.h 5 | command_args.h os_specific.c sparse_helper.h 6 | filesys_tools.cpp os_specific.h string_tools.cpp 7 | property.cpp property.h 8 | sampler.cpp sampler.h unscented.h 9 | tictoc.cpp tictoc.h 10 | g2o_stuff_api.h 11 | ) 12 | 13 | set_target_properties(stuff PROPERTIES OUTPUT_NAME ${LIB_PREFIX}stuff) 14 | target_link_libraries(stuff PUBLIC ${G2O_EIGEN3_EIGEN_TARGET}) 15 | 16 | if (APPLE) 17 | set_target_properties(stuff PROPERTIES INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}") 18 | endif() 19 | 20 | install(TARGETS stuff 21 | EXPORT ${G2O_TARGETS_EXPORT_NAME} 22 | RUNTIME DESTINATION ${RUNTIME_DESTINATION} 23 | LIBRARY DESTINATION ${LIBRARY_DESTINATION} 24 | ARCHIVE DESTINATION ${ARCHIVE_DESTINATION} 25 | INCLUDES DESTINATION ${INCLUDES_DESTINATION} 26 | ) 27 | 28 | # build our OpenGL helper library 29 | if(OPENGL_FOUND AND G2O_HAVE_OPENGL) 30 | add_library(opengl_helper ${G2O_LIB_TYPE} 31 | opengl_primitives.cpp opengl_primitives.h 32 | ) 33 | if (APPLE) 34 | set_target_properties(opengl_helper PROPERTIES INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}") 35 | endif() 36 | install(TARGETS opengl_helper 37 | EXPORT ${G2O_TARGETS_EXPORT_NAME} 38 | RUNTIME DESTINATION ${RUNTIME_DESTINATION} 39 | LIBRARY DESTINATION ${LIBRARY_DESTINATION} 40 | ARCHIVE DESTINATION ${ARCHIVE_DESTINATION} 41 | INCLUDES DESTINATION ${INCLUDES_DESTINATION} 42 | ) 43 | 44 | target_link_libraries(opengl_helper PUBLIC ${G2O_OPENGL_TARGET} ${G2O_EIGEN3_EIGEN_TARGET}) 45 | set_target_properties(opengl_helper PROPERTIES OUTPUT_NAME "${LIB_PREFIX}opengl_helper") 46 | endif() 47 | 48 | if(${CMAKE_SYSTEM_NAME} MATCHES "Linux" AND NOT ANDROID) 49 | target_link_libraries(stuff PUBLIC rt) 50 | endif() 51 | 52 | file(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 53 | 54 | install(FILES ${headers} DESTINATION ${INCLUDES_INSTALL_DIR}/stuff) 55 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/stuff/color_macros.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_COLOR_MACROS_H 28 | #define G2O_COLOR_MACROS_H 29 | 30 | // font attributes 31 | #define FT_BOLD "\033[1m" 32 | #define FT_UNDERLINE "\033[4m" 33 | 34 | //background color 35 | #define BG_BLACK "\033[40m" 36 | #define BG_RED "\033[41m" 37 | #define BG_GREEN "\033[42m" 38 | #define BG_YELLOW "\033[43m" 39 | #define BG_LIGHTBLUE "\033[44m" 40 | #define BG_MAGENTA "\033[45m" 41 | #define BG_BLUE "\033[46m" 42 | #define BG_WHITE "\033[47m" 43 | 44 | // font color 45 | #define CL_BLACK(s) "\033[30m" << s << "\033[0m" 46 | #define CL_RED(s) "\033[31m" << s << "\033[0m" 47 | #define CL_GREEN(s) "\033[32m" << s << "\033[0m" 48 | #define CL_YELLOW(s) "\033[33m" << s << "\033[0m" 49 | #define CL_LIGHTBLUE(s) "\033[34m" << s << "\033[0m" 50 | #define CL_MAGENTA(s) "\033[35m" << s << "\033[0m" 51 | #define CL_BLUE(s) "\033[36m" << s << "\033[0m" 52 | #define CL_WHITE(s) "\033[37m" << s << "\033[0m" 53 | 54 | #define FG_BLACK "\033[30m" 55 | #define FG_RED "\033[31m" 56 | #define FG_GREEN "\033[32m" 57 | #define FG_YELLOW "\033[33m" 58 | #define FG_LIGHTBLUE "\033[34m" 59 | #define FG_MAGENTA "\033[35m" 60 | #define FG_BLUE "\033[36m" 61 | #define FG_WHITE "\033[37m" 62 | 63 | #define FG_NORM "\033[0m" 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/stuff/filesys_tools.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 | /*************************************************************************** 28 | * filesysTools.h 29 | * 30 | * Fr 02 Mär 2007 23:14:21 CET 31 | * Copyright 2007 Rainer Kümmerle 32 | * Email rk@raikue.net 33 | ****************************************************************************/ 34 | 35 | #ifndef G2O_FILESYS_TOOLS_H 36 | #define G2O_FILESYS_TOOLS_H 37 | 38 | #include "g2o_stuff_api.h" 39 | 40 | /** @addtogroup utils **/ 41 | // @{ 42 | 43 | /** \file filesysTools.h 44 | * \brief utility functions for handling files, directory on Linux/Unix 45 | */ 46 | 47 | #include 48 | #include 49 | 50 | namespace g2o { 51 | 52 | /** 53 | * get filename extension (the part after the last .), e.g. 54 | * the extension of file.txt is txt 55 | */ 56 | G2O_STUFF_API std::string getFileExtension(const std::string& filename); 57 | 58 | /** 59 | * get the filename without the extension. 60 | * file.txt -> file 61 | */ 62 | G2O_STUFF_API std::string getPureFilename(const std::string& filename); 63 | 64 | /** 65 | * change the fileextension of a given filename. 66 | * Only if filename contains an extension, otherwise filename is returned. 67 | */ 68 | G2O_STUFF_API std::string changeFileExtension(const std::string& filename, const std::string& newExt, bool stripDot = false); 69 | 70 | /** 71 | * return the basename of the given filename 72 | * /etc/fstab -> fstab 73 | */ 74 | G2O_STUFF_API std::string getBasename(const std::string& filename); 75 | 76 | /** 77 | * return the directory of a given filename 78 | * /etc/fstab -> /etc 79 | */ 80 | G2O_STUFF_API std::string getDirname(const std::string& filename); 81 | 82 | /** 83 | * check if file exists (note a directory is also a file) 84 | */ 85 | G2O_STUFF_API bool fileExists(const char* filename); 86 | 87 | /** 88 | * return all files that match a given pattern, e.g., ~/blaa*.txt, /tmp/a* 89 | */ 90 | G2O_STUFF_API std::vector getFilesByPattern(const char* pattern); 91 | 92 | } // end namespace 93 | // @} 94 | #endif 95 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/stuff/g2o_stuff_api.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | /*************************************************************************** 28 | * Description: import/export macros for creating DLLS with Microsoft 29 | * compiler. Any exported function needs to be declared with the 30 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 31 | * for each DLL (arrrrrgh!!!) 32 | * 33 | * 17 Jan 2012 34 | * Email: pupilli@cs.bris.ac.uk 35 | ****************************************************************************/ 36 | #ifndef G2O_STUFF_API_H 37 | #define G2O_STUFF_API_H 38 | 39 | #include "g2o/config.h" 40 | 41 | #ifdef _MSC_VER 42 | // We are using a Microsoft compiler: 43 | 44 | #ifdef G2O_SHARED_LIBS 45 | #ifdef stuff_EXPORTS 46 | #define G2O_STUFF_API __declspec(dllexport) 47 | #else 48 | #define G2O_STUFF_API __declspec(dllimport) 49 | #endif 50 | #else 51 | #define G2O_STUFF_API 52 | #endif 53 | 54 | #else 55 | // Not Microsoft compiler so set empty definition: 56 | #define G2O_STUFF_API 57 | #endif 58 | 59 | #endif // G2O_STUFF_API_H 60 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/stuff/opengl_wrapper.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPENGL_WRAPPER_H 28 | #define G2O_OPENGL_WRAPPER_H 29 | 30 | #include "g2o/config.h" 31 | 32 | #ifdef WINDOWS 33 | #include 34 | #endif 35 | 36 | #ifdef G2O_OPENGL_FOUND 37 | # ifdef __APPLE__ 38 | # include 39 | # else 40 | # include 41 | # endif 42 | #endif 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/stuff/os_specific.c: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "os_specific.h" 28 | 29 | #ifdef WINDOWS 30 | 31 | int vasprintf(char** strp, const char* fmt, va_list ap) 32 | { 33 | int size = 100; 34 | char* p; 35 | char* np; 36 | 37 | if ((p = (char*)malloc(size * sizeof(char))) == NULL) 38 | return -1; 39 | 40 | while (1) { 41 | #ifdef _MSC_VER 42 | int n = vsnprintf_s(p, size, size - 1, fmt, ap); 43 | #else 44 | int n = vsnprintf(p, size, fmt, ap); 45 | #endif 46 | if (n > -1 && n < size) { 47 | *strp = p; 48 | return n; 49 | } 50 | if (n > -1) 51 | size = n+1; 52 | else 53 | size *= 2; 54 | if ((np = (char*)realloc (p, size * sizeof(char))) == NULL) { 55 | free(p); 56 | return -1; 57 | } else 58 | p = np; 59 | } 60 | } 61 | 62 | 63 | #endif 64 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/stuff/os_specific.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OS_SPECIFIC_HH_ 28 | #define G2O_OS_SPECIFIC_HH_ 29 | 30 | #ifdef WINDOWS 31 | #include 32 | #include 33 | #include 34 | #ifndef _WINDOWS 35 | #include 36 | #endif 37 | #define drand48() ((number_t) rand()/(number_t)RAND_MAX) 38 | 39 | #ifdef __cplusplus 40 | extern "C" { 41 | #endif 42 | 43 | int vasprintf(char** strp, const char* fmt, va_list ap); 44 | 45 | #ifdef __cplusplus 46 | } 47 | #endif 48 | 49 | #endif 50 | 51 | #ifdef UNIX 52 | #include 53 | // nothing to do on real operating systems 54 | #endif 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/stuff/sampler.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 "sampler.h" 28 | 29 | namespace g2o { 30 | 31 | static std::normal_distribution _univariateSampler(0., 1.); 32 | static std::uniform_real_distribution _uniformReal; 33 | static std::mt19937 _gen_real; 34 | 35 | number_t sampleUniform(number_t min, number_t max, std::mt19937* generator){ 36 | if (generator) 37 | return _uniformReal(*generator)*(max-min)+min; 38 | return _uniformReal(_gen_real)*(max-min)+min; 39 | } 40 | 41 | number_t sampleGaussian(std::mt19937* generator){ 42 | if (generator) 43 | return _univariateSampler(*generator); 44 | return _univariateSampler(_gen_real); 45 | } 46 | 47 | } 48 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/stuff/sparse_helper.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_SPARSE_HELPER_H 28 | #define G2O_SPARSE_HELPER_H 29 | 30 | #include 31 | 32 | #include "g2o/config.h" 33 | #include "g2o_stuff_api.h" 34 | 35 | namespace g2o { 36 | 37 | struct TripletEntry { 38 | int r, c; 39 | number_t x; 40 | TripletEntry(int r_, int c_, number_t x_) : r(r_), c(c_), x(x_) {} 41 | }; 42 | struct TripletColSort { 43 | bool operator()(const TripletEntry& e1, const TripletEntry& e2) const { 44 | return e1.c < e2.c || (e1.c == e2.c && e1.r < e2.r); 45 | } 46 | }; 47 | 48 | /** 49 | * write an array to a file, debugging 50 | */ 51 | G2O_STUFF_API bool writeVector(const std::string& filename, const number_t* v, int n); 52 | 53 | /** 54 | * write a CCS matrix given by pointer to column, row, and values 55 | */ 56 | G2O_STUFF_API bool writeCCSMatrix(const std::string& filename, int rows, int cols, const int* p, const int* i, 57 | const double* v, bool upperTriangleSymmetric = true); 58 | 59 | /** 60 | * write a triplet matrix given by pointers 61 | * @param filename filename to write to 62 | * @param nz number of elements 63 | * @param rows number of rows of the matrix 64 | * @param cols number of colmuns of the matrix 65 | * @param Ai pointer to the row index (nz elements) 66 | * @param Aj pointer to the column index (nz elements) 67 | * @param Ax pointer to the vlaues index (nz elements) 68 | */ 69 | G2O_STUFF_API bool writeTripletMatrix(const std::string& filename, int nz, int rows, int cols, 70 | const int* Ai, const int* Aj, const double* Ax, 71 | bool upperTriangleSymmetric = true); 72 | 73 | } // namespace g2o 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/stuff/tictoc.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_TICTOC_H 28 | #define G2O_TICTOC_H 29 | 30 | #include "g2o_stuff_api.h" 31 | 32 | #include 33 | 34 | namespace g2o { 35 | 36 | /** 37 | * \brief Profile the timing of certain parts of your algorithm. 38 | * 39 | * Profile the timing of certain parts of your algorithm. 40 | * A typical use-case is as follows: 41 | * 42 | * tictoc("doSomething"); 43 | * // place the code here. 44 | * tictoc("doSomething"); 45 | * 46 | * This will calculate statistics for the operations within 47 | * the two calls to tictoc() 48 | * 49 | * If the environment variable G2O_ENABLE_TICTOC is defined, the timing will 50 | * be performed. 51 | */ 52 | G2O_STUFF_API number_t tictoc(const char* algorithmPart); 53 | 54 | /** 55 | * \brief Simplify calls to tictoc() for a whole scope 56 | * 57 | * See also the macro G2O_SCOPED_TICTOC below. 58 | */ 59 | class G2O_STUFF_API ScopedTictoc 60 | { 61 | public: 62 | ScopedTictoc(const char* algorithmPart); 63 | ~ScopedTictoc(); 64 | protected: 65 | std::string _algorithmPart; 66 | }; 67 | 68 | } // end namespace 69 | 70 | #ifndef G2O_SCOPED_TICTOC 71 | #define G2O_SCOPED_TICTOC(s) \ 72 | g2o::ScopedTictoc scopedTictoc (s) 73 | #endif 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/stuff/timeutil.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "timeutil.h" 28 | #include 29 | 30 | namespace g2o { 31 | 32 | ScopeTime::ScopeTime(const char* title) : _title(title), _startTime(get_monotonic_time()) {} 33 | 34 | ScopeTime::~ScopeTime() { 35 | std::cerr << _title<<" took "<<1000*(get_monotonic_time()-_startTime)<<"ms.\n"; 36 | } 37 | 38 | number_t get_monotonic_time() 39 | { 40 | return seconds{ std::chrono::steady_clock::now().time_since_epoch() }.count(); 41 | } 42 | 43 | } // end namespace 44 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/types/slam3d/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(types_slam3d ${G2O_LIB_TYPE} 2 | dquat2mat.cpp dquat2mat.h 3 | isometry3d_mappings.h 4 | isometry3d_mappings.cpp 5 | isometry3d_gradients.cpp 6 | isometry3d_gradients.h 7 | vertex_se3.cpp 8 | vertex_se3.h 9 | edge_se3.cpp 10 | edge_se3.h 11 | se3quat.h 12 | se3_ops.h se3_ops.hpp 13 | types_slam3d.cpp 14 | types_slam3d.h 15 | ) 16 | set_target_properties(types_slam3d PROPERTIES OUTPUT_NAME ${LIB_PREFIX}types_slam3d) 17 | target_link_libraries(types_slam3d core) -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/types/slam3d/dquat2mat.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef _DQUAT2MAT_H_ 28 | #define _DQUAT2MAT_H_ 29 | #include 30 | #include "g2o_types_slam3d_api.h" 31 | 32 | namespace g2o { 33 | namespace internal { 34 | 35 | void G2O_TYPES_SLAM3D_API compute_dq_dR ( Eigen::Matrix& dq_dR , const number_t& r11 , const number_t& r21 , const number_t& r31 , const number_t& r12 , const number_t& r22 , const number_t& r32 , const number_t& r13 , const number_t& r23 , const number_t& r33 ); 36 | 37 | void G2O_TYPES_SLAM3D_API compute_dR_dq ( Eigen::Matrix& dR_dq , const number_t& qx , const number_t& qy , const number_t& qz , const number_t& qw ) ; 38 | } 39 | } 40 | #endif 41 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/types/slam3d/g2o_types_slam3d_api.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | /*************************************************************************** 28 | * Description: import/export macros for creating DLLS with Microsoft 29 | * compiler. Any exported function needs to be declared with the 30 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 31 | * for each DLL (arrrrrgh!!!) 32 | * 33 | * 17 Jan 2012 34 | * Email: pupilli@cs.bris.ac.uk 35 | ****************************************************************************/ 36 | #ifndef G2O_TYPES_SLAM3D_API_H 37 | #define G2O_TYPES_SLAM3D_API_H 38 | 39 | #include "g2o/config.h" 40 | 41 | #ifdef _MSC_VER 42 | // We are using a Microsoft compiler: 43 | #ifdef G2O_SHARED_LIBS 44 | #ifdef types_slam3d_EXPORTS 45 | #define G2O_TYPES_SLAM3D_API __declspec(dllexport) 46 | #else 47 | #define G2O_TYPES_SLAM3D_API __declspec(dllimport) 48 | #endif 49 | #else 50 | #define G2O_TYPES_SLAM3D_API 51 | #endif 52 | 53 | #else 54 | // Not Microsoft compiler so set empty definition: 55 | #define G2O_TYPES_SLAM3D_API 56 | #endif 57 | 58 | #endif // G2O_TYPES_SLAM3D_API_H 59 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/types/slam3d/isometry3d_gradients.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "isometry3d_gradients.h" 28 | #include 29 | namespace g2o { 30 | using namespace std; 31 | using namespace Eigen; 32 | 33 | namespace internal { 34 | #include "dquat2mat.cpp" 35 | } // end namespace internal 36 | 37 | } // end namespace 38 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/types/slam3d/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_SE3_OPS_H 28 | #define G2O_SE3_OPS_H 29 | 30 | #include 31 | #include 32 | 33 | #include "g2o_types_slam3d_api.h" 34 | 35 | namespace g2o { 36 | 37 | inline G2O_TYPES_SLAM3D_API Matrix3 skew(const Vector3&v); 38 | inline G2O_TYPES_SLAM3D_API Vector3 deltaR(const Matrix3& R); 39 | inline G2O_TYPES_SLAM3D_API Vector2 project(const Vector3&); 40 | inline G2O_TYPES_SLAM3D_API Vector3 project(const Vector4&); 41 | inline G2O_TYPES_SLAM3D_API Vector3 unproject(const Vector2&); 42 | inline G2O_TYPES_SLAM3D_API Vector4 unproject(const Vector3&); 43 | 44 | #include "se3_ops.hpp" 45 | 46 | } 47 | 48 | #endif //MATH_STUFF 49 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/types/slam3d/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 | Matrix3 skew(const Vector3&v) 28 | { 29 | Matrix3 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 | Vector3 deltaR(const Matrix3& R) 41 | { 42 | Vector3 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 | Vector2 project(const Vector3& v) 50 | { 51 | Vector2 res; 52 | res(0) = v(0)/v(2); 53 | res(1) = v(1)/v(2); 54 | return res; 55 | } 56 | 57 | Vector3 project(const Vector4& v) 58 | { 59 | Vector3 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 | Vector3 unproject(const Vector2& v) 67 | { 68 | Vector3 res; 69 | res(0) = v(0); 70 | res(1) = v(1); 71 | res(2) = 1; 72 | return res; 73 | } 74 | 75 | Vector4 unproject(const Vector3& v) 76 | { 77 | Vector4 res; 78 | res(0) = v(0); 79 | res(1) = v(1); 80 | res(2) = v(2); 81 | res(3) = 1; 82 | return res; 83 | } 84 | 85 | 86 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/types/slam3d/types_slam3d.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "types_slam3d.h" 28 | #include "g2o/core/factory.h" 29 | #include "g2o/stuff/macros.h" 30 | 31 | #include 32 | 33 | namespace g2o { 34 | 35 | G2O_REGISTER_TYPE_GROUP(slam3d); 36 | 37 | G2O_REGISTER_TYPE(VERTEX_SE3:QUAT, VertexSE3); 38 | G2O_REGISTER_TYPE(EDGE_SE3:QUAT, EdgeSE3); 39 | 40 | /*********** ACTIONS ************/ 41 | G2O_REGISTER_ACTION(VertexSE3WriteGnuplotAction); 42 | G2O_REGISTER_ACTION(EdgeSE3WriteGnuplotAction); 43 | 44 | } // end namespace 45 | -------------------------------------------------------------------------------- /thirdparty/g2o/g2o/types/slam3d/types_slam3d.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_TYPES_SLAM3D_ 28 | #define G2O_TYPES_SLAM3D_ 29 | 30 | #include "g2o/config.h" 31 | #include "g2o/core/base_vertex.h" 32 | #include "g2o/core/base_binary_edge.h" 33 | #include "g2o/core/hyper_graph_action.h" 34 | 35 | #define THREE_D_TYPES_ANALYTIC_JACOBIAN 36 | 37 | #include "vertex_se3.h" 38 | #include "edge_se3.h" 39 | 40 | #endif 41 | --------------------------------------------------------------------------------