├── .gitmodules ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake ├── FindEigen3.cmake ├── FindLibZip.cmake └── FindSuiteParse.cmake ├── configs ├── 4seasons.yaml ├── 4seasons_calib │ ├── camchain.yaml │ ├── cropped_calib304.txt │ └── undistorted_calib304.txt ├── ablations │ ├── 4seasonsNoInitialReadvancing.yaml │ ├── 4seasonsNoPGBA.yaml │ └── 4seasonsNoReinitAndMargReplacement.yaml ├── euroc.yaml ├── pcalib_linear_8bit.txt ├── realsense │ └── vignette_t265.png ├── t265_noise_tumvi.yaml ├── tumvi.yaml └── tumvi_calib │ ├── camchain.yaml │ ├── camera02.txt │ └── pcalib.txt ├── doc └── RealsenseLiveVersion.md ├── src ├── GTSAMIntegration │ ├── AugmentedScatter.cpp │ ├── AugmentedScatter.hpp │ ├── BAGTSAMIntegration.cpp │ ├── BAGTSAMIntegration.h │ ├── DelayedMarginalization.cpp │ ├── DelayedMarginalization.h │ ├── ExtUtils.h │ ├── FEJNoiseModelFactor.h │ ├── FEJValues.cpp │ ├── FEJValues.h │ ├── GTSAMUtils.cpp │ ├── GTSAMUtils.h │ ├── Marginalization.cpp │ ├── Marginalization.h │ ├── PoseTransformation.cpp │ ├── PoseTransformation.h │ ├── PoseTransformationFactor.cpp │ ├── PoseTransformationFactor.h │ ├── PoseTransformationIMU.cpp │ ├── PoseTransformationIMU.h │ ├── Sim3GTSAM.cpp │ └── Sim3GTSAM.h ├── IMU │ ├── BAIMULogic.cpp │ ├── BAIMULogic.h │ ├── CoarseIMULogic.cpp │ ├── CoarseIMULogic.h │ ├── IMUIntegration.cpp │ ├── IMUIntegration.hpp │ ├── IMUSettings.cpp │ ├── IMUSettings.h │ ├── IMUTypes.cpp │ ├── IMUTypes.h │ ├── IMUUtils.cpp │ └── IMUUtils.h ├── IMUInitialization │ ├── CoarseIMUInitOptimizer.cpp │ ├── CoarseIMUInitOptimizer.h │ ├── GravityInitializer.cpp │ ├── GravityInitializer.h │ ├── IMUInitSettings.cpp │ ├── IMUInitSettings.h │ ├── IMUInitStateChanger.h │ ├── IMUInitializer.cpp │ ├── IMUInitializer.h │ ├── IMUInitializerLogic.cpp │ ├── IMUInitializerLogic.h │ ├── IMUInitializerStates.cpp │ ├── IMUInitializerStates.h │ ├── IMUInitializerTransitions.cpp │ ├── IMUInitializerTransitions.h │ ├── PoseGraphBundleAdjustment.cpp │ └── PoseGraphBundleAdjustment.h ├── dso │ ├── 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 │ ├── IOWrapper │ │ ├── ImageDisplay.h │ │ ├── ImageDisplay_dummy.cpp │ │ ├── ImageRW.h │ │ ├── ImageRW_dummy.cpp │ │ ├── OpenCV │ │ │ ├── ImageDisplay_OpenCV.cpp │ │ │ └── ImageRW_OpenCV.cpp │ │ ├── Output3DWrapper.h │ │ ├── OutputWrapper │ │ │ └── SampleOutputWrapper.h │ │ └── Pangolin │ │ │ ├── FollowCamMode.cpp │ │ │ ├── FollowCamMode.h │ │ │ ├── KeyFrameDisplay.cpp │ │ │ ├── KeyFrameDisplay.h │ │ │ ├── PangolinDSOViewer.cpp │ │ │ └── PangolinDSOViewer.h │ ├── OptimizationBackend │ │ ├── AccumulatedSCHessian.cpp │ │ ├── AccumulatedSCHessian.h │ │ ├── AccumulatedTopHessian.cpp │ │ ├── AccumulatedTopHessian.h │ │ ├── EnergyFunctional.cpp │ │ ├── EnergyFunctional.h │ │ ├── EnergyFunctionalStructs.cpp │ │ ├── EnergyFunctionalStructs.h │ │ ├── MatrixAccumulators.h │ │ └── RawResidualJacobian.h │ ├── README.md │ └── util │ │ ├── DatasetReader.h │ │ ├── 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 ├── live │ ├── DatasetSaver.cpp │ ├── DatasetSaver.h │ ├── FrameContainer.cpp │ ├── FrameContainer.h │ ├── FrameSkippingStrategy.cpp │ ├── FrameSkippingStrategy.h │ ├── IMUInterpolator.cpp │ ├── IMUInterpolator.h │ ├── RealsenseT265.cpp │ └── RealsenseT265.h ├── main_dmvio_dataset.cpp ├── main_dmvio_t265.cpp └── util │ ├── GTData.hpp │ ├── MainSettings.cpp │ ├── MainSettings.h │ ├── SettingsUtil.cpp │ ├── SettingsUtil.h │ ├── TimeMeasurement.cpp │ └── TimeMeasurement.h ├── test ├── CMakeLists.txt ├── test_IMUInterpolator.cpp └── test_PoseTransformationFactor.cpp └── 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 └── libzip-1.1.1.tar.gz /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "thirdparty/sse2neon"] 2 | path = thirdparty/sse2neon 3 | url = https://github.com/jratcliff63367/sse2neon.git 4 | [submodule "test/googletest"] 5 | path = test/googletest 6 | url = https://github.com/google/googletest.git 7 | -------------------------------------------------------------------------------- /cmake/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 | -------------------------------------------------------------------------------- /cmake/FindLibZip.cmake: -------------------------------------------------------------------------------- 1 | # Finds libzip. 2 | # 3 | # This module defines: 4 | # LIBZIP_INCLUDE_DIR_ZIP 5 | # LIBZIP_INCLUDE_DIR_ZIPCONF 6 | # LIBZIP_LIBRARY 7 | # 8 | 9 | find_package(PkgConfig) 10 | pkg_check_modules(PC_LIBZIP QUIET libzip) 11 | 12 | find_path(LIBZIP_INCLUDE_DIR_ZIP 13 | NAMES zip.h 14 | HINTS ${PC_LIBZIP_INCLUDE_DIRS}) 15 | 16 | find_path(LIBZIP_INCLUDE_DIR_ZIPCONF 17 | NAMES zipconf.h 18 | HINTS ${PC_LIBZIP_INCLUDE_DIRS}) 19 | 20 | find_library(LIBZIP_LIBRARY 21 | NAMES zip) 22 | 23 | include(FindPackageHandleStandardArgs) 24 | FIND_PACKAGE_HANDLE_STANDARD_ARGS( 25 | LIBZIP DEFAULT_MSG 26 | LIBZIP_LIBRARY LIBZIP_INCLUDE_DIR_ZIP LIBZIP_INCLUDE_DIR_ZIPCONF) 27 | 28 | set(LIBZIP_VERSION 0) 29 | 30 | if (LIBZIP_INCLUDE_DIR_ZIPCONF) 31 | FILE(READ "${LIBZIP_INCLUDE_DIR_ZIPCONF}/zipconf.h" _LIBZIP_VERSION_CONTENTS) 32 | if (_LIBZIP_VERSION_CONTENTS) 33 | STRING(REGEX REPLACE ".*#define LIBZIP_VERSION \"([0-9.]+)\".*" "\\1" LIBZIP_VERSION "${_LIBZIP_VERSION_CONTENTS}") 34 | endif () 35 | endif () 36 | 37 | set(LIBZIP_VERSION ${LIBZIP_VERSION} CACHE STRING "Version number of libzip") 38 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /configs/4seasons.yaml: -------------------------------------------------------------------------------- 1 | accelerometer_noise_density: 0.10200528 2 | gyroscope_noise_density: 4.120916e-02 3 | accelerometer_random_walk: 9.8082e-04 4 | gyroscope_random_walk: 3.8785e-04 5 | integration_sigma: 0.316227 6 | setting_weightZeroPriorDSOInitY: 5e09 7 | setting_weightZeroPriorDSOInitX: 5e09 8 | setting_forceNoKFTranslationThresh: 0.01 -------------------------------------------------------------------------------- /configs/4seasons_calib/camchain.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | cam_overlaps: [1] 3 | T_cam_imu: 4 | - [-0.9998852242642406, -0.013522961078544133, 0.006831385051241187, 0.17541216744862287] 5 | - [-0.006890161859766396, 0.004304637029338462, -0.9999669973402087, 0.0036894333751345677] 6 | - [0.01349310815180704, -0.9998992947410829, -0.004397318352110671, -0.05810612695941222] 7 | - [0.0, 0.0, 0.0, 1.0] 8 | intrinsics: [527.9990706330082, 527.963495807245, 399.18451401412665, 172.8193108347693] 9 | resolution: [800, 400] 10 | distortion_coeffs: [-0.03559759964255725, -0.005093721310999416, 0.019716282737702494, -0.01583280039499382] 11 | camera_model: pinhole 12 | timeshift_cam_imu: -4.926753519571215e-06 13 | distortion_model: equidistant 14 | cam1: 15 | cam_overlaps: [0] 16 | T_cam_imu: 17 | - [-0.9999948729203221, -0.00041104034603049364, -0.0031757170690647394, -0.12432919293024627] 18 | - [0.003173467277625304, 0.005362979695895791, -0.9999805835886111, 0.0023471917960152505] 19 | - [0.0004280636712633416, -0.9999855346426845, -0.005361647776383633, -0.05662052461099226] 20 | - [0.0, 0.0, 0.0, 1.0] 21 | intrinsics: [529.2496538273606, 529.4013679656194, 412.4733148308946, 172.1405434152354] 22 | resolution: [800, 400] 23 | distortion_coeffs: [-0.03696481607810142, 0.0102400942942071, -0.01632902510720569, 0.009597717025035516] 24 | camera_model: pinhole 25 | timeshift_cam_imu: -4.5217727146455115e-06 26 | distortion_model: equidistant 27 | T_cam1_cam0: 28 | - [0.999867211010726, 0.009591886382536711, -0.013174067756669, -0.3004961618953472] 29 | - [-0.009603884302752923, 0.9999535222750985, -0.0008477593549240888, 0.0002870696089010898] 30 | - [0.0131653238445598, 0.0009741690043478774, 0.9999128588246174, 0.00337890826837825] 31 | - [0.0, 0.0, 0.0, 1.0] 32 | -------------------------------------------------------------------------------- /configs/4seasons_calib/cropped_calib304.txt: -------------------------------------------------------------------------------- 1 | Pinhole 501.4757919305817 501.4757919305817 421.7953735163109 167.65799492501083 0.0 0.0 0.0 0.0 2 | 800 304 3 | none 4 | 800 304 5 | -------------------------------------------------------------------------------- /configs/4seasons_calib/undistorted_calib304.txt: -------------------------------------------------------------------------------- 1 | Pinhole 0.626844739913227125 1.25368947982645425 0.527869216895388625 0.420394987312527075 0.0 0.0 0.0 0.0 2 | 800 400 3 | 0.626844739913227125 1.64959142082428190789 0.527869216895388625 0.5531512990954303618 0 4 | 800 304 5 | -------------------------------------------------------------------------------- /configs/ablations/4seasonsNoInitialReadvancing.yaml: -------------------------------------------------------------------------------- 1 | accelerometer_noise_density: 0.10200528 2 | gyroscope_noise_density: 4.120916e-02 3 | accelerometer_random_walk: 9.8082e-04 4 | gyroscope_random_walk: 3.8785e-04 5 | integration_sigma: 0.316227 6 | setting_weightZeroPriorDSOInitY: 5e09 7 | setting_weightZeroPriorDSOInitX: 5e09 8 | setting_forceNoKFTranslationThresh: 0.01 9 | init_transitionModel: 4 10 | init_pgba_reinitScaleUncertaintyThresh: 1e6 11 | init_scalePriorAfterInit: 1.0 -------------------------------------------------------------------------------- /configs/ablations/4seasonsNoPGBA.yaml: -------------------------------------------------------------------------------- 1 | accelerometer_noise_density: 0.10200528 2 | gyroscope_noise_density: 4.120916e-02 3 | accelerometer_random_walk: 9.8082e-04 4 | gyroscope_random_walk: 3.8785e-04 5 | integration_sigma: 0.316227 6 | setting_weightZeroPriorDSOInitY: 5e09 7 | setting_weightZeroPriorDSOInitX: 5e09 8 | setting_forceNoKFTranslationThresh: 0.01 9 | init_transitionModel: 5 10 | init_scalePriorAfterInit: 1.0 -------------------------------------------------------------------------------- /configs/ablations/4seasonsNoReinitAndMargReplacement.yaml: -------------------------------------------------------------------------------- 1 | accelerometer_noise_density: 0.10200528 2 | gyroscope_noise_density: 4.120916e-02 3 | accelerometer_random_walk: 9.8082e-04 4 | gyroscope_random_walk: 3.8785e-04 5 | integration_sigma: 0.316227 6 | setting_weightZeroPriorDSOInitY: 5e09 7 | setting_weightZeroPriorDSOInitX: 5e09 8 | setting_forceNoKFTranslationThresh: 0.01 9 | init_transitionModel: 1 10 | init_pgba_reinitScaleUncertaintyThresh: 1e6 -------------------------------------------------------------------------------- /configs/euroc.yaml: -------------------------------------------------------------------------------- 1 | accelerometer_noise_density: 0.2 2 | gyroscope_noise_density: 0.016968 3 | accelerometer_random_walk: 0.003 4 | gyroscope_random_walk: 0.000019393 5 | integration_sigma: 0.316227 -------------------------------------------------------------------------------- /configs/pcalib_linear_8bit.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 -------------------------------------------------------------------------------- /configs/realsense/vignette_t265.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukasvst/dm-vio/e306328098e08facbec1bfd45d22e19c77a3ccad/configs/realsense/vignette_t265.png -------------------------------------------------------------------------------- /configs/t265_noise_tumvi.yaml: -------------------------------------------------------------------------------- 1 | accelerometer_noise_density: 0.224 2 | gyroscope_noise_density: 0.01280 3 | accelerometer_random_walk: 0.0430 4 | gyroscope_random_walk: 0.00110 5 | integration_sigma: 0.316227 6 | maxSkipFramesVisualOnlyMode: 1 # This many frames can be skipped at a time while in visual-only mode. 7 | maxSkipFramesVisualInertial: 2 # This many frames can be skipped at a time while in visual-inertial mode. 8 | skipFramesVisualOnlyDelay: 30 # After visual initializer finished, wait this amount of frames before switching to the visualOnly threshold. This is useful because during the first frames the system might be less stable than later. 9 | minQueueSizeForSkipping: 2 # Don't skip frames if the image queue is smaller than this. 10 | maxTimeBetweenInitFrames: 1.0 # Reset the visual initializer if this time has passed between the first and last frame. 11 | init_pgba_skipFirstKFs: 1 12 | init_requestFullResetNormalizedErrorThreshold: 0.1 # If the error after the CoarseIMUInit is larger than this we assume the visual system failed and reset the full system. 13 | normalizeCamSize: 0.2 # Sets the size of the displayed camera (normalized with the scale obtained from IMU data). 14 | # With the following settings, VIO is initialized a bit earlier (default values in comment behind it). 15 | # To adjust these settings you can read the CoarseIMUInit scale and variance printed out by the system and see with which variance it typically outputs a reasonable scale. 16 | init_coarseScaleUncertaintyThresh: 5 # 1.0 17 | init_pgba_scaleUncertaintyThresh: 5 # 1.0 18 | init_pgba_reinitScaleUncertaintyThresh: 1 # 0.5 19 | # If you are sure that no clouds will cover a large part of the image you can include points with any depth by enabling the following line. 20 | # setting_minIdepth: 0.0 21 | -------------------------------------------------------------------------------- /configs/tumvi.yaml: -------------------------------------------------------------------------------- 1 | accelerometer_noise_density: 0.224 2 | gyroscope_noise_density: 0.01280 3 | accelerometer_random_walk: 0.0430 4 | gyroscope_random_walk: 0.00110 5 | integration_sigma: 0.316227 -------------------------------------------------------------------------------- /configs/tumvi_calib/camchain.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | T_cam_imu: 3 | - [-0.9995250378696743, 0.029615343885863205, -0.008522328211654736, 0.04727988224914392] 4 | - [0.0075019185074052044, -0.03439736061393144, -0.9993800792498829, -0.047443232143367084] 5 | - [-0.02989013031643309, -0.998969345370175, 0.03415885127385616, -0.0681999605066297] 6 | - [0.0, 0.0, 0.0, 1.0] 7 | cam_overlaps: [1] 8 | camera_model: pinhole 9 | distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 10 | 0.00020293673591811182] 11 | distortion_model: equidistant 12 | intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504] 13 | resolution: [512, 512] 14 | rostopic: /cam0/image_raw 15 | cam1: 16 | T_cam_imu: 17 | - [-0.9995110484978581, 0.030299116376600627, -0.0077218830287333565, -0.053697434688869734] 18 | - [0.008104079263822521, 0.012511643720192351, -0.9998888851620987, -0.046131737923635924] 19 | - [-0.030199136245891378, -0.9994625667418545, -0.012751072573940885, -0.07149261284195751] 20 | - [0.0, 0.0, 0.0, 1.0] 21 | T_cn_cnm1: 22 | - [0.9999994457734953, -0.0008233639921576076, -0.0006561436136444361, -0.10106110275180535] 23 | - [0.0007916877528168117, 0.9988994619156757, -0.04689603624058988, -0.0019764575873431013] 24 | - [0.0006940340102242531, 0.04689549078870055, 0.9988995601463054, -0.0011756424802046581] 25 | - [0.0, 0.0, 0.0, 1.0] 26 | cam_overlaps: [0] 27 | camera_model: pinhole 28 | distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606, 29 | 0.0003299517423931039] 30 | distortion_model: equidistant 31 | intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983] 32 | resolution: [512, 512] 33 | rostopic: /cam1/image_raw 34 | -------------------------------------------------------------------------------- /configs/tumvi_calib/camera02.txt: -------------------------------------------------------------------------------- 1 | EquiDistant 190.978477 190.973307 254.931706 256.897443 0.00348238940225 0.000715034845216 -0.00205323614187 0.000202936735918 2 | 512 512 3 | 0.2 0.2 0.499 0.499 0 4 | 512 512 -------------------------------------------------------------------------------- /src/GTSAMIntegration/AugmentedScatter.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #ifndef AugmentedScatter_hpp 24 | #define AugmentedScatter_hpp 25 | 26 | #include 27 | 28 | #include 29 | 30 | #include "util/NumType.h" 31 | #include "OptimizationBackend/EnergyFunctionalStructs.h" 32 | 33 | #include 34 | 35 | namespace dmvio 36 | { 37 | class AugmentedScatter : public gtsam::Scatter 38 | { 39 | public: 40 | // Scatter that can additionally handle keys that don't exist in the factor graph. For those keys the dimension must be specified in keyDimMap 41 | AugmentedScatter(const gtsam::GaussianFactorGraph& gfg,boost::optional ordering, const std::map& keyDimMap); 42 | 43 | iterator findNew(gtsam::Key key); 44 | 45 | std::pair computeHessian(const gtsam::GaussianFactorGraph& gfg); 46 | gtsam::Matrix computeAugmentedHessian(const gtsam::GaussianFactorGraph& gfg); 47 | }; 48 | } 49 | 50 | 51 | #endif /* AugmentedScatter_hpp */ 52 | -------------------------------------------------------------------------------- /src/GTSAMIntegration/ExtUtils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #ifndef DMVIO_EXTUTILS_H 24 | #define DMVIO_EXTUTILS_H 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #ifdef STACKTRACE 31 | #include 32 | #endif 33 | 34 | namespace dmvio 35 | { 36 | 37 | template void assertAlmostEq(const T& first, const T& second, double epsilon = 0.00001) 38 | { 39 | T diff = std::abs(first - second); 40 | if(diff > epsilon) 41 | { 42 | std::cout << "Error: Not Eq: " << diff << " first: " << first << " second" << second << std::endl; 43 | #ifdef STACKTRACE 44 | std::cout << boost::stacktrace::stacktrace(); 45 | #endif 46 | assert(0); 47 | } 48 | 49 | } 50 | 51 | template void assertEqEigen(const T& first, const T& second, double epsilon = 0.00001) 52 | { 53 | T diff = first - second; 54 | if(!diff.isZero(epsilon)) 55 | { 56 | std::cout << "Error: Not Eq:\n" << diff << "\nfirst:\n" << first << "\nsecond\n" << second << std::endl; 57 | std::cout << "Max diff: " << diff.cwiseAbs().maxCoeff() << std::endl; 58 | #ifdef STACKTRACE 59 | std::cout << boost::stacktrace::stacktrace(); 60 | #endif 61 | assert(0); 62 | } 63 | } 64 | 65 | 66 | template class MeanAccumulator 67 | { 68 | public: 69 | void add(const T& t) 70 | { 71 | sum += t; 72 | num++; 73 | } 74 | 75 | T getMean() 76 | { 77 | if(num == 0) 78 | { 79 | return 0; 80 | } 81 | return sum / num; 82 | } 83 | 84 | private: 85 | T sum = 0.0; 86 | int num = 0; 87 | }; 88 | typedef MeanAccumulator MeanAccumulatorD; 89 | 90 | } 91 | 92 | #endif //DMVIO_EXTUTILS_H 93 | -------------------------------------------------------------------------------- /src/GTSAMIntegration/FEJValues.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #include 24 | #include "FEJValues.h" 25 | 26 | void dmvio::setFEJMapForGraph(gtsam::NonlinearFactorGraph& graph, const std::shared_ptr& fejValues) 27 | { 28 | for(auto&& factor : graph) 29 | { 30 | auto* casted = dynamic_cast(factor.get()); 31 | if(casted) 32 | { 33 | casted->setFEJValues(fejValues); 34 | } 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /src/GTSAMIntegration/FEJValues.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #ifndef DMVIO_FEJVALUES_H 24 | #define DMVIO_FEJVALUES_H 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include "dso/util/settings.h" 31 | #include "Marginalization.h" 32 | #include "GTSAMUtils.h" 33 | 34 | namespace dmvio 35 | { 36 | 37 | // Handles First-Estimates Jacobians (FEJ) for GTSAM. 38 | class FEJValues 39 | { 40 | public: 41 | gtsam::Values fejValues; 42 | 43 | // Called when keys are become connected to marginalization factors. Their values will be inserted into fejValues. 44 | template void insertConnectedKeys(const T& connectedKeys, const gtsam::Values& currentValues) 45 | { 46 | // always insert current poses and affine brightness (no matter if connected). 47 | for(auto&& val : currentValues) 48 | { 49 | auto chr = gtsam::Symbol(val.key).chr(); 50 | if(chr == 'p' || chr == 'a') 51 | { 52 | eraseAndInsert(fejValues, val.key, val.value); 53 | } 54 | } 55 | for(auto&& key : connectedKeys) 56 | { 57 | if(!fejValues.exists(key)) 58 | { 59 | fejValues.insert(key, currentValues.at(key)); 60 | } 61 | } 62 | } 63 | 64 | // Remove keys from FEJMap, as they have been marginalized / removed. 65 | template void keysRemoved(const T& keysRemoved) 66 | { 67 | for(auto&& key : keysRemoved) 68 | { 69 | fejValues.erase(key); 70 | } 71 | } 72 | 73 | // Returns values containing neededKeys, using fejValues where available, otherwise current values. 74 | template gtsam::Values buildValues(const T& neededKeys, const gtsam::Values& currentValues) 75 | { 76 | gtsam::Values ret; 77 | for(auto&& key : neededKeys) 78 | { 79 | bool use = fejValues.exists(key); 80 | if(use) 81 | { 82 | ret.insert(key, fejValues.at(key)); 83 | }else 84 | { 85 | ret.insert(key, currentValues.at(key)); 86 | } 87 | } 88 | return ret; 89 | } 90 | 91 | }; 92 | 93 | // Interface for factors which can handle FEJ. 94 | // Implemented by PoseTransformationFactor and FEJNoiseModelFactor. 95 | class FactorHandlingFEJ 96 | { 97 | public: 98 | virtual void setFEJValues(std::shared_ptr fej) = 0; 99 | }; 100 | 101 | // calls setFEJValues for all factors in graph which implement FactorHandlingFEJ. 102 | void setFEJMapForGraph(gtsam::NonlinearFactorGraph& graph, const std::shared_ptr& fejValues); 103 | 104 | } 105 | 106 | #endif //DMVIO_FEJVALUES_H 107 | -------------------------------------------------------------------------------- /src/GTSAMIntegration/GTSAMUtils.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #include "GTSAMUtils.h" 24 | 25 | using namespace gtsam; 26 | 27 | gtsam::Matrix dmvio::augmentedHessianFromPair(const std::pair& pair) 28 | { 29 | int n = pair.first.rows(); 30 | 31 | gtsam::Matrix returning = gtsam::Matrix::Zero(n + 1, n + 1); 32 | returning.block(0, 0, n, n) = pair.first; 33 | returning.block(n, 0, 1, n) = pair.second.transpose(); 34 | returning.block(0, n, n, 1) = pair.second; 35 | return returning; 36 | } 37 | 38 | std::pair dmvio::pairFromAugmentedHessian(const gtsam::Matrix& matrix) 39 | { 40 | int n = matrix.rows() - 1; 41 | return std::pair(matrix.block(0, 0, n, n), matrix.block(0, n, n, 1)); 42 | } 43 | 44 | void dmvio::removeKeysFromGraph(gtsam::NonlinearFactorGraph& graph, const std::set& keysToRemove, 45 | int stopAfterNoRemoval) 46 | { 47 | if(keysToRemove.empty()) 48 | { 49 | return; 50 | } 51 | int noRemoval = 0; 52 | // We use the fact that we inserted the factors in order. 53 | for(auto it = graph.begin(); it != graph.end(); ++it) 54 | { 55 | auto&& keys = (*it)->keys(); 56 | bool keyContained = false; 57 | for(auto&& key : keys) 58 | { 59 | if(keysToRemove.find(key) != keysToRemove.end()) 60 | { 61 | keyContained = true; 62 | break; 63 | } 64 | } 65 | if(keyContained) 66 | { 67 | it = graph.erase(it); 68 | it--; 69 | noRemoval = 0; 70 | }else 71 | { 72 | noRemoval++; 73 | if(stopAfterNoRemoval > 0 && noRemoval >= stopAfterNoRemoval) break; 74 | } 75 | } 76 | } 77 | 78 | 79 | -------------------------------------------------------------------------------- /src/GTSAMIntegration/GTSAMUtils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #ifndef DMVIO_GTSAMUTILS_H 24 | #define DMVIO_GTSAMUTILS_H 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace dmvio 32 | { 33 | 34 | std::pair pairFromAugmentedHessian(const gtsam::Matrix& matrix); 35 | 36 | gtsam::Matrix augmentedHessianFromPair(const std::pair& pair); 37 | 38 | void removeKeysFromGraph(gtsam::NonlinearFactorGraph& graph, const std::set& keysToRemove, 39 | int stopAfterNoRemoval = -1); 40 | 41 | template void eraseAndInsert(gtsam::Values& values, gtsam::Key key, const T& value) 42 | { 43 | if(values.exists(key)) 44 | { 45 | values.update(key, value); 46 | }else 47 | { 48 | values.insert(key, value); 49 | } 50 | } 51 | 52 | template void eraseAndInsert(gtsam::Values::shared_ptr values, gtsam::Key key, const T& value) 53 | { 54 | eraseAndInsert(*values, key, value); 55 | } 56 | 57 | template gtsam::Key getMinKeyWithChr(const C& keys, unsigned char chr) 58 | { 59 | auto minIt = std::min_element(keys.begin(), keys.end(), [chr](const gtsam::Key& key1, const gtsam::Key& key2) 60 | { 61 | gtsam::Symbol sym1(key1); 62 | gtsam::Symbol sym2(key2); 63 | return (sym1.chr() == chr && sym1.index() < sym2.index()) || sym2.chr() != chr; 64 | }); 65 | return *minIt; 66 | } 67 | 68 | template gtsam::Key getMaxKeyWithChr(const C& keys, unsigned char chr) 69 | { 70 | // we still use min_element but reverse the comparison 71 | auto minIt = std::min_element(keys.begin(), keys.end(), [chr](const gtsam::Key& key1, const gtsam::Key& key2) 72 | { 73 | gtsam::Symbol sym1(key1); 74 | gtsam::Symbol sym2(key2); 75 | return (sym1.chr() == chr && sym1.index() > sym2.index()) || sym2.chr() != chr; 76 | }); 77 | return *minIt; 78 | } 79 | 80 | } 81 | 82 | #endif //DMVIO_GTSAMUTILS_H 83 | -------------------------------------------------------------------------------- /src/GTSAMIntegration/Marginalization.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * The code in this file is in part based on code written by Vladyslav Usenko for the paper "Direct Visual-Inertial Odometry with Stereo Cameras". 4 | * 5 | * Copyright (c) 2022 Lukas von Stumberg , Vladyslav Usenko 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 | * DM-VIO 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 | * DM-VIO 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 DM-VIO. If not, see . 22 | */ 23 | 24 | #ifndef DMVIO_MARGINALIZATION_H 25 | #define DMVIO_MARGINALIZATION_H 26 | 27 | #include 28 | #include 29 | 30 | namespace dmvio 31 | { 32 | // Returns a factor graph where the specified keys are marginalized. 33 | // connectedKeyCallback (can also be nullptr) will be called as soon as the connected variables are computed (and before the actual marginalization is performed). 34 | gtsam::NonlinearFactorGraph::shared_ptr 35 | marginalizeOut(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, 36 | const gtsam::FastVector& keysToMarginalize, 37 | std::function&)> connectedKeyCallback); 38 | 39 | // Like the above method, but can also delete the marginalized variables from the passed values. 40 | gtsam::NonlinearFactorGraph::shared_ptr marginalizeOut(const gtsam::NonlinearFactorGraph& graph, gtsam::Values& values, 41 | const gtsam::FastVector& keysToMarginalize, 42 | std::function&)> connectedKeyCallback, 44 | bool deleteFromValues); 45 | 46 | // Fills newGraph with factors which are not connected and marginalizedOutGraph with all factors which will be marginalized out, 47 | // Also fills setOfKeysToMarginalize, and connectedKeys. 48 | void extractKeysToMarginalize(const gtsam::NonlinearFactorGraph& graph, gtsam::NonlinearFactorGraph& newGraph, 49 | gtsam::NonlinearFactorGraph& marginalizedOutGraph, 50 | gtsam::FastSet& setOfKeysToMarginalize, 51 | gtsam::FastSet& connectedKeys); 52 | 53 | // Compute the Schur complement with the given dimension of marginalized factors and other factors. 54 | gtsam::Matrix computeSchurComplement(const gtsam::Matrix& augmentedHessian, int mSize, int aSize); 55 | 56 | } 57 | 58 | #endif //DMVIO_MARGINALIZATION_H 59 | -------------------------------------------------------------------------------- /src/GTSAMIntegration/Sim3GTSAM.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | #include "Sim3GTSAM.h" 27 | 28 | using namespace gtsam; 29 | 30 | ScaleGTSAM::ScaleGTSAM(double scale) 31 | : scale(scale) 32 | { 33 | } 34 | 35 | Sophus::Sim3d ScaleGTSAM::sim() const 36 | { 37 | Sophus::Sim3d ret; 38 | ret.setScale(scale); 39 | return ret; 40 | } 41 | 42 | void ScaleGTSAM::print(const std::string& str) const 43 | { 44 | std::cout << str << " Scale: " << scale << std::endl; 45 | } 46 | 47 | size_t ScaleGTSAM::dim() const 48 | { 49 | return 1; 50 | } 51 | 52 | bool ScaleGTSAM::equals(const ScaleGTSAM& other, double tol) const 53 | { 54 | return fabs(scale - other.scale) < tol; 55 | } 56 | 57 | ScaleGTSAM ScaleGTSAM::identity() 58 | { 59 | return ScaleGTSAM(0); 60 | } 61 | 62 | ScaleGTSAM ScaleGTSAM::operator*(const ScaleGTSAM& T) const 63 | { 64 | return ScaleGTSAM((sim() * T.sim()).scale()); 65 | } 66 | 67 | ScaleGTSAM ScaleGTSAM::inverse() const 68 | { 69 | return ScaleGTSAM(sim().inverse().scale()); 70 | } 71 | 72 | gtsam::Vector1 ScaleGTSAM::Logmap(const ScaleGTSAM& s, gtsam::OptionalJacobian<1, 1> Hm) 73 | { 74 | Sophus::Sim3d::Tangent tangent = Sophus::Sim3d::log(s.sim()); 75 | gtsam::Vector1 ret; 76 | ret(0) = tangent(6); 77 | return ret; 78 | } 79 | 80 | ScaleGTSAM ScaleGTSAM::Expmap(const Vector1& v, gtsam::OptionalJacobian<1, 1> Hm) 81 | { 82 | double scaleInc = v(0); 83 | // For larger values the exp would get too large. 84 | if(std::abs(scaleInc) > 10) 85 | { 86 | scaleInc *= 10 / std::abs(scaleInc); 87 | } 88 | gtsam::Vector7 myInc = gtsam::Vector7::Zero(); 89 | myInc(6) = scaleInc; 90 | Sophus::Sim3d simAfter = Sophus::Sim3d::exp(myInc); 91 | return ScaleGTSAM(simAfter.scale()); 92 | } 93 | 94 | gtsam::Matrix1 ScaleGTSAM::AdjointMap() const 95 | { 96 | // This is probably just one always, which could be used to make it faster. 97 | gtsam::Matrix77 adj = sim().Adj(); 98 | return adj.block(6, 6, 1, 1); 99 | } 100 | -------------------------------------------------------------------------------- /src/GTSAMIntegration/Sim3GTSAM.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #ifndef DMVIO_SIM3GTSAM_H 24 | #define DMVIO_SIM3GTSAM_H 25 | 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | // In contrast to Sim3GTSAM this contains only the scale. 32 | // Could probably be made faster by not basing it on Sophus. 33 | class ScaleGTSAM : public gtsam::LieGroup 34 | { 35 | public: 36 | ScaleGTSAM(double scale); 37 | 38 | Sophus::Sim3d sim() const; 39 | double scale = 1.0; 40 | 41 | // For the LieGroup. 42 | static ScaleGTSAM identity(); 43 | 44 | /// Composition 45 | ScaleGTSAM operator*(const ScaleGTSAM& T) const; 46 | 47 | /// Return the inverse 48 | ScaleGTSAM inverse() const; 49 | 50 | static gtsam::Vector1 Logmap(const ScaleGTSAM& s, 51 | gtsam::OptionalJacobian<1, 1> Hm = boost::none); 52 | 53 | static ScaleGTSAM Expmap(const gtsam::Vector1& v, 54 | gtsam::OptionalJacobian<1, 1> Hm = boost::none); 55 | 56 | /// Chart at the origin 57 | struct ChartAtOrigin 58 | { 59 | static ScaleGTSAM Retract(const gtsam::Vector1& v, ChartJacobian H = boost::none) 60 | { 61 | return ScaleGTSAM::Expmap(v, H); 62 | } 63 | 64 | static gtsam::Vector1 Local(const ScaleGTSAM& other, ChartJacobian H = boost::none) 65 | { 66 | return ScaleGTSAM::Logmap(other, H); 67 | } 68 | }; 69 | using LieGroup::inverse; 70 | 71 | gtsam::Matrix1 AdjointMap() const; 72 | 73 | 74 | void print(const std::string& str) const; 75 | size_t dim() const; 76 | bool equals(const ScaleGTSAM& other, double tol) const; 77 | }; 78 | 79 | namespace gtsam 80 | { 81 | 82 | template<> 83 | struct traits : public internal::LieGroup 84 | { 85 | }; 86 | 87 | template<> 88 | struct traits : public internal::LieGroup 89 | { 90 | }; 91 | 92 | } 93 | 94 | #endif //DMVIO_SIM3GTSAM_H 95 | -------------------------------------------------------------------------------- /src/IMU/IMUTypes.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #include "IMUTypes.h" 24 | 25 | using namespace dmvio; 26 | 27 | IMUMeasurement::IMUMeasurement(const Eigen::Vector3d& accData, const Eigen::Vector3d& gyrData, double integrationTime) 28 | : accData(accData), gyrData(gyrData), integrationTime(integrationTime) 29 | {} 30 | 31 | const Eigen::Vector3d& IMUMeasurement::getAccData() const 32 | { 33 | return accData; 34 | } 35 | 36 | const Eigen::Vector3d& IMUMeasurement::getGyrData() const 37 | { 38 | return gyrData; 39 | } 40 | 41 | double IMUMeasurement::getIntegrationTime() const 42 | { 43 | return integrationTime; 44 | } 45 | -------------------------------------------------------------------------------- /src/IMU/IMUTypes.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #ifndef DMVIO_IMUTYPES_H 24 | #define DMVIO_IMUTYPES_H 25 | 26 | #include 27 | #include 28 | 29 | namespace dmvio 30 | { 31 | class IMUMeasurement 32 | { 33 | public: 34 | IMUMeasurement(const Eigen::Vector3d& accData, const Eigen::Vector3d& gyrData, double integrationTime); 35 | 36 | const Eigen::Vector3d& getAccData() const; 37 | 38 | const Eigen::Vector3d& getGyrData() const; 39 | 40 | double getIntegrationTime() const; 41 | 42 | private: 43 | Eigen::Vector3d accData{}; 44 | Eigen::Vector3d gyrData{}; 45 | double integrationTime; // time between this and previous IMU measurement. 46 | }; 47 | 48 | typedef std::vector IMUData; 49 | } 50 | 51 | #endif //DMVIO_IMUTYPES_H 52 | -------------------------------------------------------------------------------- /src/IMU/IMUUtils.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #include 24 | #include 25 | #include "IMUUtils.h" 26 | #include "IMUTypes.h" 27 | #include "IMUSettings.h" 28 | 29 | using namespace dmvio; 30 | using namespace gtsam; 31 | 32 | void dmvio::integrateIMUData(const IMUData& imuData, gtsam::PreintegratedImuMeasurements& preintegrated) 33 | { 34 | for(const auto& measurement : imuData) 35 | { 36 | if(measurement.getIntegrationTime() == 0.0) continue; 37 | preintegrated.integrateMeasurement(gtsam::Vector(measurement.getAccData()), 38 | gtsam::Vector(measurement.getGyrData()), 39 | measurement.getIntegrationTime()); 40 | } 41 | } 42 | 43 | gtsam::noiseModel::Diagonal::shared_ptr dmvio::computeBiasNoiseModel(const IMUCalibration& imuCalibration, 44 | const gtsam::PreintegratedImuMeasurements& imuMeasurements) 45 | { 46 | double sigma_b_a = imuCalibration.sigma_between_b_a * sqrt(imuMeasurements.deltaTij()); 47 | double sigma_b_g = imuCalibration.sigma_between_b_g * sqrt(imuMeasurements.deltaTij()); 48 | return gtsam::noiseModel::Diagonal::Sigmas( 49 | (gtsam::Vector(6) << sigma_b_a, sigma_b_a, sigma_b_a, sigma_b_g, sigma_b_g, sigma_b_g).finished()); 50 | } 51 | 52 | void IMUTransformPriorSettings::registerArgs(dmvio::SettingsUtil& set, std::string prefix) 53 | { 54 | set.registerArg(prefix + "priorExtrinsicsRot", priorExtrinsicsRot); 55 | set.registerArg(prefix + "priorExtrinsicsTrans", priorExtrinsicsTrans); 56 | set.registerArg(prefix + "priorGravityDirection", priorGravityDirection); 57 | set.registerArg(prefix + "priorGravityDirectionZ", priorGravityDirectionZ); 58 | } 59 | 60 | std::vector dmvio::getPriorsAndAddValuesForTransform( 61 | const TransformDSOToIMU& transform, const IMUTransformPriorSettings& settings, gtsam::Values& values) 62 | { 63 | std::vector ret; 64 | int symInd = transform.getSymbolInd(); 65 | if(transform.optimizeScale()) 66 | { 67 | gtsam::Key scaleKey = gtsam::Symbol('s', symInd); 68 | ScaleGTSAM sim(transform.getScale()); 69 | values.insert(scaleKey, sim); 70 | } 71 | 72 | if(transform.optimizeGravity()) 73 | { 74 | gtsam::Key gravityKey = Symbol('g', symInd); 75 | gtsam::Rot3 initialRot(transform.getR_dsoW_metricW().matrix()); 76 | gtsam::Rot3 zeroRot; // Initialize with current transform but set prior to zero transform. 77 | values.insert(gravityKey, initialRot); 78 | 79 | Eigen::Vector3d gravityModel; 80 | double rotationalSigma = settings.priorGravityDirection; 81 | gravityModel.setConstant(rotationalSigma); 82 | gravityModel(2) = settings.priorGravityDirectionZ; 83 | gtsam::PriorFactor::shared_ptr rotationPrior( 84 | new gtsam::PriorFactor(gravityKey, zeroRot, 85 | gtsam::noiseModel::Diagonal::Sigmas(gravityModel))); 86 | ret.push_back(rotationPrior); 87 | } 88 | 89 | if(transform.optimizeExtrinsics()) 90 | { 91 | gtsam::Key extrinsicsKey = Symbol('i', symInd); 92 | gtsam::Pose3 initialExtr(transform.getT_cam_imu().matrix()); 93 | values.insert(extrinsicsKey, initialExtr); 94 | 95 | gtsam::Vector6 extrinsicsModel; 96 | extrinsicsModel.segment(0, 3).setConstant(settings.priorExtrinsicsRot); 97 | extrinsicsModel.segment(3, 3).setConstant(settings.priorExtrinsicsTrans); 98 | gtsam::PriorFactor::shared_ptr extrinsicsPrior(new gtsam::PriorFactor( 99 | extrinsicsKey, initialExtr, gtsam::noiseModel::Diagonal::Sigmas(extrinsicsModel) 100 | )); 101 | ret.push_back(extrinsicsPrior); 102 | } 103 | return ret; 104 | } 105 | -------------------------------------------------------------------------------- /src/IMU/IMUUtils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #ifndef DMVIO_IMUUTILS_H 24 | #define DMVIO_IMUUTILS_H 25 | 26 | #include "IMUTypes.h" 27 | #include 28 | #include "GTSAMIntegration/PoseTransformationIMU.h" 29 | #include "util/SettingsUtil.h" 30 | 31 | namespace dmvio 32 | { 33 | 34 | class IMUCalibration; 35 | class TransformDSOToIMU; 36 | 37 | void integrateIMUData(const IMUData& imuData, gtsam::PreintegratedImuMeasurements& preintegrated); 38 | 39 | gtsam::noiseModel::Diagonal::shared_ptr 40 | computeBiasNoiseModel(const IMUCalibration& imuCalibration, const gtsam::PreintegratedImuMeasurements& imuMeasurements); 41 | 42 | // Settings regarding prior on the symbols optimized by TransformDSOToIMU. 43 | class IMUTransformPriorSettings 44 | { 45 | public: 46 | double priorExtrinsicsRot = 0.01; // rotational and translational prior for extrinsics. 47 | double priorExtrinsicsTrans = 0.1; 48 | double priorGravityDirection = 0.4; // Prior on gravity direction (first xy and then z). 49 | double priorGravityDirectionZ = 0.0001; 50 | 51 | void registerArgs(dmvio::SettingsUtil& set, std::string prefix = ""); 52 | }; 53 | 54 | // Add values for the variables optimized by TransformDSOToIMU and return a list of prior factors, 55 | // according to the IMUTransformPriorSettings. 56 | std::vector getPriorsAndAddValuesForTransform( 57 | const TransformDSOToIMU& transform, const IMUTransformPriorSettings& settings, gtsam::Values& values); 58 | 59 | } 60 | #endif //DMVIO_IMUUTILS_H 61 | -------------------------------------------------------------------------------- /src/IMUInitialization/CoarseIMUInitOptimizer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #ifndef DMVIO_COARSEIMUINITOPTIMIZER_H 24 | #define DMVIO_COARSEIMUINITOPTIMIZER_H 25 | 26 | 27 | #include "sophus/se3.hpp" 28 | #include "GTSAMIntegration/PoseTransformationIMU.h" 29 | #include "IMU/IMUTypes.h" 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include "IMU/IMUSettings.h" 35 | #include 36 | 37 | namespace dso 38 | { 39 | class FrameShell; 40 | } 41 | 42 | namespace dmvio 43 | { 44 | // This is the Coarse IMU Initializer from the paper 45 | // It only optimizes IMU variables (scale, gravity direction, bias, velocities). 46 | // To be exact: velocities, bias and all symbols optimized by the passed PoseTransformation are optimized. 47 | // Owned (and methods called) by IMUInitializer, doesn't know DSO. 48 | class CoarseIMUInitOptimizer 49 | { 50 | public: 51 | // transformDSOToIMU is updated during optimization. 52 | // Note that a reference to the settings and calibration is kept! 53 | // Note: the owner of this class is responsible for adding initial values and priors for the variables optimized 54 | // by transformDSOToIMU (e.g s0, g0). 55 | explicit CoarseIMUInitOptimizer(std::shared_ptr transformDSOToIMU, 56 | const IMUCalibration& imuCalibration, 57 | const CoarseIMUInitOptimizerSettings& settingsPassed); 58 | 59 | // Add frame to the optimizer. 60 | void addPose(int frameId, const Sophus::SE3d& camToWorld, const gtsam::PreintegratedImuMeasurements* imuData); 61 | void addPose(const dso::FrameShell& shell, const gtsam::PreintegratedImuMeasurements* imuData); 62 | 63 | struct OptimizationResult 64 | { 65 | OptimizationResult(int numIterations, double error, double normalizedError, bool good); 66 | int numIterations; 67 | double error; 68 | double normalizedError; 69 | bool good; 70 | }; 71 | 72 | OptimizationResult optimize(); 73 | std::shared_ptr getUpdatedTransform(); 74 | gtsam::imuBias::ConstantBias getBias(); 75 | gtsam::Key getBiasKey(); 76 | 77 | gtsam::Marginals getMarginals(); 78 | 79 | void takeOverOptimizedValues(); 80 | 81 | gtsam::NonlinearFactorGraph graph; 82 | gtsam::Values values; 83 | gtsam::Values optimizedValues; 84 | int numFrames = 0; 85 | int imuFactorsRemovedUntil = -1; 86 | private: 87 | void handleFirstFrame(int frameId); 88 | 89 | const CoarseIMUInitOptimizerSettings& settings; 90 | const IMUCalibration& imuCalibration; 91 | 92 | std::shared_ptr transformDSOToIMU; 93 | 94 | int prevFrameId = -1; 95 | gtsam::Pose3 prevFramePose; // Needed for fixPoses. 96 | 97 | gtsam::LevenbergMarquardtParams params = gtsam::LevenbergMarquardtParams::CeresDefaults(); 98 | 99 | // Only used if fixPoses == false; 100 | gtsam::SharedNoiseModel posePriorModel; 101 | 102 | // For implementing maxNumPoses: 103 | std::deque poseIds; // pose ids currently in the graph. 104 | 105 | // used to get updated poses from DSO before optimizing. 106 | std::map activeShells; 107 | 108 | }; 109 | 110 | 111 | } 112 | 113 | #endif //DMVIO_COARSEIMUINITOPTIMIZER_H 114 | -------------------------------------------------------------------------------- /src/IMUInitialization/GravityInitializer.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #include "GravityInitializer.h" 24 | #include 25 | 26 | using dmvio::GravityInitializer; 27 | using dmvio::IMUIntegration; 28 | 29 | GravityInitializer::GravityInitializer(int numMeasurementsToUse, const IMUCalibration& imuCalibration) 30 | : maxNumMeasurements(numMeasurementsToUse) 31 | { 32 | gravity = imuCalibration.gravity; 33 | } 34 | 35 | Sophus::SE3d 36 | GravityInitializer::addMeasure(const IMUData& imuData, const Sophus::SE3d& currToFirst) 37 | { 38 | int numMeasure = 0; 39 | Eigen::Vector3d measure(0.0, 0.0, 0.0); 40 | for(int i = 0; i < imuData.size(); ++i) 41 | { 42 | Eigen::Vector3d curr = imuData[i].getAccData(); 43 | measure += curr; 44 | numMeasure++; 45 | } 46 | measure /= (double) numMeasure; 47 | 48 | measures.push_back(measure); 49 | 50 | if(measures.size() > maxNumMeasurements) 51 | { 52 | measures.pop_front(); 53 | } 54 | 55 | Eigen::Vector3d filteredM(0.0, 0.0, 0.0); 56 | for(auto&& m : measures) 57 | { 58 | filteredM += m; 59 | } 60 | filteredM /= (double) measures.size(); 61 | 62 | measure = filteredM; 63 | 64 | Eigen::Quaterniond quat; 65 | quat.setFromTwoVectors(measure, -gravity); 66 | 67 | Sophus::SE3d imuToWorld(quat, Eigen::Vector3d::Zero()); 68 | 69 | return imuToWorld; 70 | } 71 | 72 | double dmvio::getGravityError(const Sophus::SE3d& imuToWorld, const Sophus::SE3d& imuToWorldGT) 73 | { 74 | Eigen::Vector3d g = (gtsam::Vector(3) 75 | << 0, 0, -9.8082).finished(); // Only the direction actually matters so it's ok if this is not the actually used gravity. 76 | // g is in world coordinates, so check what g is in drone coordinates. 77 | Eigen::Vector3d gDrone = imuToWorld.inverse().rotationMatrix() * g; 78 | Eigen::Vector3d gDroneGT = imuToWorldGT.inverse().rotationMatrix() * g; 79 | 80 | // Compute angle between the two vectors. 81 | double angle = std::acos(gDrone.dot(gDroneGT) / (gDrone.norm() * gDroneGT.norm())); 82 | double degrees = (angle * 180.0) / boost::math::constants::pi(); 83 | 84 | return degrees; 85 | } 86 | -------------------------------------------------------------------------------- /src/IMUInitialization/GravityInitializer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #ifndef DMVIO_GRAVITYINITIALIZER_H 24 | #define DMVIO_GRAVITYINITIALIZER_H 25 | 26 | #include "sophus/se3.hpp" 27 | #include "IMU/IMUIntegration.hpp" 28 | 29 | namespace dmvio 30 | { 31 | // Averages accelerometer measurements to provide a simple initialization for gravity direction. 32 | class GravityInitializer 33 | { 34 | public: 35 | GravityInitializer(int numMeasurementsToUse, const IMUCalibration& imuCalibration); 36 | 37 | // returns an approximate imuToWorld transform (only rotation). 38 | Sophus::SE3d addMeasure(const IMUData& imuData, const Sophus::SE3d& currToFirst); 39 | 40 | private: 41 | int maxNumMeasurements; // Num of last gravity measurements to average. 42 | std::deque measures; 43 | Eigen::Vector3d gravity; 44 | }; 45 | 46 | double getGravityError(const Sophus::SE3d& imuToWorld, const Sophus::SE3d& imuToWorldGT); 47 | 48 | } 49 | 50 | #endif //DMVIO_GRAVITYINITIALIZER_H 51 | -------------------------------------------------------------------------------- /src/IMUInitialization/IMUInitSettings.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #include "util/SettingsUtil.h" 24 | #include "IMUInitSettings.h" 25 | 26 | using namespace dmvio; 27 | 28 | 29 | void IMUInitSettings::registerArgs(dmvio::SettingsUtil& set, std::string prefix) 30 | { 31 | set.registerArg(prefix + "transitionModel", transitionModel); 32 | set.registerArg(prefix + "onlyKFs", onlyKFs); 33 | set.registerArg(prefix + "coarseScaleUncertaintyThresh", coarseScaleUncertaintyThresh); 34 | set.registerArg(prefix + "initDSOParams", initDSOParams); 35 | set.registerArg(prefix + "scalePriorAfterInit", scalePriorAfterInit); 36 | set.registerArg(prefix + "disableVIOUntilFirstInit", disableVIOUntilFirstInit); 37 | set.registerArg(prefix + "multithreadedInitDespiteNonRT", multithreadedInitDespiteNonRT); 38 | 39 | transformPriors.registerArgs(set, prefix); 40 | coarseInitSettings.registerArgs(set, prefix); 41 | pgbaSettings.registerArgs(set, prefix + "pgba_"); 42 | 43 | thresholdSettings.threshScale = 1.02; 44 | thresholdSettings.registerArgs(set, prefix); 45 | secondThresholdSettings.threshScale = 10000000.0; 46 | secondThresholdSettings.registerArgs(set, prefix + "second"); 47 | } 48 | 49 | void CoarseIMUInitOptimizerSettings::registerArgs(dmvio::SettingsUtil& set, std::string prefix) 50 | { 51 | set.registerArg(prefix + "maxNumPoses", maxNumPoses); 52 | set.registerArg(prefix + "fixPoses", fixPoses); 53 | set.registerArg(prefix + "multipleBiases", multipleBiases); 54 | 55 | set.registerArg(prefix + "priorRotSigma", priorRotSigma); 56 | set.registerArg(prefix + "priorTransSigma", priorTransSigma); 57 | 58 | set.registerArg(prefix + "lambdaLowerBound", lambdaLowerBound); 59 | 60 | set.registerArg(prefix + "conversionType", conversionType); 61 | 62 | set.registerArg(prefix + "updatePoses", updatePoses); 63 | 64 | set.registerArg(prefix + "requestFullResetErrorThreshold", requestFullResetErrorThreshold); 65 | set.registerArg(prefix + "requestFullResetNormalizedErrorThreshold", requestFullResetNormalizedErrorThreshold); 66 | } 67 | 68 | void PGBASettings::registerArgs(dmvio::SettingsUtil& set, std::string prefix) 69 | { 70 | set.registerArg(prefix + "delay", delay); 71 | set.registerArg(prefix + "scaleUncertaintyThresh", scaleUncertaintyThresh); 72 | set.registerArg(prefix + "reinitScaleUncertaintyThresh", reinitScaleUncertaintyThresh); 73 | set.registerArg(prefix + "skipFirstKFs", skipFirstKFs); 74 | set.registerArg(prefix + "conversionType", conversionType); 75 | 76 | set.registerArg(prefix + "prepareGraphAddFactors", prepareGraphAddFactors); 77 | set.registerArg(prefix + "prepareGraphAddDelValues", prepareGraphAddDelValues); 78 | 79 | transformPriors.registerArgs(set, prefix); 80 | } 81 | 82 | void IMUThresholdSettings::registerArgs(dmvio::SettingsUtil& set, std::string prefix) 83 | { 84 | set.registerArg(prefix + "threshScale", threshScale); 85 | set.registerArg(prefix + "threshGravdir", threshGravdir); 86 | } 87 | -------------------------------------------------------------------------------- /src/IMUInitialization/IMUInitStateChanger.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #ifndef DMVIO_IMUINITSTATECHANGER_H 24 | #define DMVIO_IMUINITSTATECHANGER_H 25 | 26 | #include 27 | #include 28 | 29 | namespace dmvio 30 | { 31 | 32 | class IMUInitializerState; 33 | 34 | // Provides methods to change the state. 35 | class IMUInitStateChanger 36 | { 37 | public: 38 | // Can directly be called to change the state. 39 | virtual void lockAndSetState(std::unique_ptr&& newState) = 0; 40 | 41 | // Aquire lock for calling setState. 42 | virtual std::unique_lock acquireSetStateLock() = 0; 43 | // Before calling setState a lock must be acquired with the previous method! 44 | virtual void setState(std::unique_ptr&& newState) = 0; 45 | }; 46 | 47 | } 48 | 49 | #endif //DMVIO_IMUINITSTATECHANGER_H 50 | -------------------------------------------------------------------------------- /src/IMUInitialization/IMUInitializerLogic.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #ifndef DMVIO_IMUINITIALIZERLOGIC_H 24 | #define DMVIO_IMUINITIALIZERLOGIC_H 25 | 26 | 27 | #include 28 | #include 29 | #include 30 | #include "CoarseIMUInitOptimizer.h" 31 | #include "PoseGraphBundleAdjustment.h" 32 | #include "IMUInitStateChanger.h" 33 | 34 | namespace dmvio 35 | { 36 | 37 | class IMUInitializerState; 38 | 39 | class IMUInitVariances 40 | { 41 | public: 42 | IMUInitVariances() = default; 43 | IMUInitVariances(const gtsam::Marginals& marginals, gtsam::Key scaleKey, gtsam::Key biasKey); 44 | 45 | bool indetermined = true; 46 | double scaleVariance; 47 | gtsam::Matrix biasCovariance; 48 | }; 49 | 50 | class StateTransitionModel; 51 | 52 | // Helper class which encapsulates common logic and data for the states of the IMUInitializer. 53 | class IMUInitializerLogic 54 | { 55 | public: 56 | typedef std::function InitCallback; 57 | 58 | IMUInitializerLogic(std::string resultsPrefix, 59 | boost::shared_ptr preintegrationParams, 60 | const dmvio::IMUCalibration& imuCalibration, 61 | dmvio::IMUInitSettings& settings, 62 | DelayedMarginalizationGraphs* delayedMarginalization, 63 | bool linearizeOperation, InitCallback callOnInit, 64 | IMUInitStateChanger& stateChanger); 65 | 66 | // This is required to change the state from different threads. 67 | IMUInitStateChanger& stateChanger; 68 | 69 | DelayedMarginalizationGraphs* delayedMarginalizationGraphs; // Used to replace the main graph on init. 70 | 71 | // if true the factory methods will create the CoarseIMUInitState (or the PGBAState respectively) in realtime mode. 72 | bool realtimeCoarseIMUInit; 73 | bool realtimePGBA; 74 | 75 | InitCallback callOnInit; 76 | 77 | std::shared_ptr transformDSOToIMU; 78 | std::shared_ptr transformDSOToIMUAfterPGBA; 79 | std::shared_ptr optScale; 80 | std::shared_ptr optGravity; 81 | std::shared_ptr optT_cam_imu; 82 | 83 | const IMUCalibration& imuCalibration; 84 | IMUInitSettings& settings; 85 | 86 | // This is the bias used for the preintegration in the main system. 87 | gtsam::imuBias::ConstantBias latestBias; 88 | 89 | // For CoarseIMUInit: 90 | std::unique_ptr coarseIMUOptimizer; 91 | gtsam::PreintegratedImuMeasurements imuMeasurements; 92 | void addPose(const dso::FrameShell& shell, bool willBecomeKeyframe, const IMUData* imuData); 93 | IMUInitVariances performCoarseIMUInit(double timestamp); 94 | 95 | // For PGBA. 96 | std::unique_ptr pgba; 97 | 98 | }; 99 | 100 | 101 | } 102 | 103 | 104 | #endif //DMVIO_IMUINITIALIZERLOGIC_H 105 | -------------------------------------------------------------------------------- /src/dso/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 | 25 | #pragma once 26 | 27 | 28 | #include "util/NumType.h" 29 | 30 | #include "FullSystem/HessianBlocks.h" 31 | namespace dso 32 | { 33 | 34 | 35 | struct ImmaturePointTemporaryResidual 36 | { 37 | public: 38 | ResState state_state; 39 | double state_energy; 40 | ResState state_NewState; 41 | double state_NewEnergy; 42 | FrameHessian* target; 43 | }; 44 | 45 | 46 | enum ImmaturePointStatus { 47 | IPS_GOOD=0, // traced well and good 48 | IPS_OOB, // OOB: end tracking & marginalize! 49 | IPS_OUTLIER, // energy too high: if happens again: outlier! 50 | IPS_SKIPPED, // traced well and good (but not actually traced). 51 | IPS_BADCONDITION, // not traced because of bad condition. 52 | IPS_UNINITIALIZED}; // not even traced once. 53 | 54 | 55 | class ImmaturePoint 56 | { 57 | public: 58 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 59 | // static values 60 | float color[MAX_RES_PER_POINT]; 61 | float weights[MAX_RES_PER_POINT]; 62 | 63 | 64 | 65 | 66 | 67 | Mat22f gradH; 68 | Vec2f gradH_ev; 69 | Mat22f gradH_eig; 70 | float energyTH; 71 | float u,v; 72 | FrameHessian* host; 73 | int idxInImmaturePoints; 74 | 75 | float quality; 76 | 77 | float my_type; 78 | 79 | float idepth_min; 80 | float idepth_max; 81 | ImmaturePoint(int u_, int v_, FrameHessian* host_, float type, CalibHessian* HCalib); 82 | ~ImmaturePoint(); 83 | 84 | ImmaturePointStatus traceOn(FrameHessian* frame, const Mat33f &hostToFrame_KRKi, const Vec3f &hostToFrame_Kt, const Vec2f &hostToFrame_affine, CalibHessian* HCalib, bool debugPrint=false); 85 | 86 | ImmaturePointStatus lastTraceStatus; 87 | Vec2f lastTraceUV; 88 | float lastTracePixelInterval; 89 | 90 | float idepth_GT; 91 | 92 | double linearizeResidual( 93 | CalibHessian * HCalib, const float outlierTHSlack, 94 | ImmaturePointTemporaryResidual* tmpRes, 95 | float &Hdd, float &bd, 96 | float idepth); 97 | float getdPixdd( 98 | CalibHessian * HCalib, 99 | ImmaturePointTemporaryResidual* tmpRes, 100 | float idepth); 101 | 102 | float calcResidual( 103 | CalibHessian * HCalib, const float outlierTHSlack, 104 | ImmaturePointTemporaryResidual* tmpRes, 105 | float idepth); 106 | 107 | private: 108 | }; 109 | 110 | } 111 | 112 | -------------------------------------------------------------------------------- /src/dso/FullSystem/PixelSelector2.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO, written by Jakob Engel. 3 | * It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). 4 | * 5 | * Copyright 2022 Lukas von Stumberg 6 | * Copyright 2016 Technical University of Munich and Intel. 7 | * Developed by Jakob Engel , 8 | * for more information see . 9 | * If you use this code, please cite the respective publications as 10 | * listed on the above website. 11 | * 12 | * DSO is free software: you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation, either version 3 of the License, or 15 | * (at your option) any later version. 16 | * 17 | * DSO is distributed in the hope that it will be useful, 18 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 19 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 20 | * GNU General Public License for more details. 21 | * 22 | * You should have received a copy of the GNU General Public License 23 | * along with DSO. If not, see . 24 | */ 25 | 26 | 27 | #pragma once 28 | 29 | #include "util/NumType.h" 30 | 31 | namespace dso 32 | { 33 | 34 | enum PixelSelectorStatus {PIXSEL_VOID=0, PIXSEL_1, PIXSEL_2, PIXSEL_3}; 35 | 36 | 37 | class FrameHessian; 38 | 39 | class PixelSelector 40 | { 41 | public: 42 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 43 | int makeMaps( 44 | const FrameHessian* const fh, 45 | float* map_out, float density, int recursionsLeft=1, bool plot=false, float thFactor=1); 46 | 47 | PixelSelector(int w, int h); 48 | ~PixelSelector(); 49 | int currentPotential; 50 | 51 | 52 | bool allowFast; 53 | void makeHists(const FrameHessian* const fh); 54 | private: 55 | 56 | Eigen::Vector3i select(const FrameHessian* const fh, 57 | float* map_out, int pot, float thFactor=1); 58 | 59 | 60 | unsigned char* randomPattern; 61 | 62 | 63 | int* gradHist; 64 | float* ths; 65 | float* thsSmoothed; 66 | int thsStep; 67 | const FrameHessian* gradHistFrame; 68 | 69 | // block width, and block height. 70 | int bW, bH; 71 | // number of blocks in x and y dimension. 72 | int nbW, nbH; 73 | }; 74 | 75 | 76 | 77 | 78 | } 79 | 80 | -------------------------------------------------------------------------------- /src/dso/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 | 25 | #pragma once 26 | 27 | #include "util/NumType.h" 28 | #include "FullSystem/FullSystem.h" 29 | #include "FullSystem/HessianBlocks.h" 30 | #include "util/settings.h" 31 | 32 | namespace dso 33 | { 34 | 35 | 36 | EIGEN_STRONG_INLINE float derive_idepth( 37 | const Vec3f &t, const float &u, const float &v, 38 | const int &dx, const int &dy, const float &dxInterp, 39 | const float &dyInterp, const float &drescale) 40 | { 41 | return (dxInterp*drescale * (t[0]-t[2]*u) 42 | + dyInterp*drescale * (t[1]-t[2]*v))*SCALE_IDEPTH; 43 | } 44 | 45 | 46 | 47 | EIGEN_STRONG_INLINE bool projectPoint( 48 | const float &u_pt,const float &v_pt, 49 | const float &idepth, 50 | const Mat33f &KRKi, const Vec3f &Kt, 51 | float &Ku, float &Kv) 52 | { 53 | Vec3f ptp = KRKi * Vec3f(u_pt,v_pt, 1) + Kt*idepth; 54 | Ku = ptp[0] / ptp[2]; 55 | Kv = ptp[1] / ptp[2]; 56 | return Ku>1.1f && Kv>1.1f && Kucxl())*HCalib->fxli(), 72 | (v_pt+dy-HCalib->cyl())*HCalib->fyli(), 73 | 1); 74 | 75 | Vec3f ptp = R * KliP + t*idepth; 76 | drescale = 1.0f/ptp[2]; 77 | new_idepth = idepth*drescale; 78 | 79 | if(!(drescale>0)) return false; 80 | 81 | u = ptp[0] * drescale; 82 | v = ptp[1] * drescale; 83 | Ku = u*HCalib->fxl() + HCalib->cxl(); 84 | Kv = v*HCalib->fyl() + HCalib->cyl(); 85 | 86 | return Ku>1.1f && Kv>1.1f && Ku, 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 | 25 | #pragma once 26 | 27 | 28 | #include "util/globalCalib.h" 29 | #include "vector" 30 | 31 | #include "util/NumType.h" 32 | #include 33 | #include 34 | #include "util/globalFuncs.h" 35 | #include "OptimizationBackend/RawResidualJacobian.h" 36 | 37 | namespace dso 38 | { 39 | class PointHessian; 40 | class FrameHessian; 41 | class CalibHessian; 42 | 43 | class EFResidual; 44 | 45 | 46 | enum ResLocation {ACTIVE=0, LINEARIZED, MARGINALIZED, NONE}; 47 | enum ResState {IN=0, OOB, OUTLIER}; 48 | 49 | struct FullJacRowT 50 | { 51 | Eigen::Vector2f projectedTo[MAX_RES_PER_POINT]; 52 | }; 53 | 54 | class PointFrameResidual 55 | { 56 | public: 57 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 58 | 59 | EFResidual* efResidual; 60 | 61 | static int instanceCounter; 62 | 63 | 64 | ResState state_state; 65 | double state_energy; 66 | ResState state_NewState; 67 | double state_NewEnergy; 68 | double state_NewEnergyWithOutlier; 69 | 70 | 71 | void setState(ResState s) {state_state = s;} 72 | 73 | 74 | PointHessian* point; 75 | FrameHessian* host; 76 | FrameHessian* target; 77 | RawResidualJacobian* J; 78 | 79 | 80 | bool isNew; 81 | 82 | 83 | Eigen::Vector2f projectedTo[MAX_RES_PER_POINT]; 84 | Vec3f centerProjectedTo; 85 | 86 | ~PointFrameResidual(); 87 | PointFrameResidual(); 88 | PointFrameResidual(PointHessian* point_, FrameHessian* host_, FrameHessian* target_); 89 | double linearize(CalibHessian* HCalib); 90 | 91 | 92 | void resetOOB() 93 | { 94 | state_NewEnergy = state_energy = 0; 95 | state_NewState = ResState::OUTLIER; 96 | 97 | setState(ResState::IN); 98 | }; 99 | void applyRes( bool copyJacobians); 100 | 101 | void debugPlot(); 102 | 103 | void printRows(std::vector &v, VecX &r, int nFrames, int nPoints, int M, int res); 104 | }; 105 | } 106 | 107 | -------------------------------------------------------------------------------- /src/dso/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 | 25 | 26 | #pragma once 27 | #include 28 | #include "util/NumType.h" 29 | #include "util/MinimalImage.h" 30 | 31 | 32 | namespace dso 33 | { 34 | 35 | namespace IOWrap 36 | { 37 | 38 | void displayImage(const char* windowName, const MinimalImageB* img, bool autoSize = false); 39 | void displayImage(const char* windowName, const MinimalImageB3* img, bool autoSize = false); 40 | void displayImage(const char* windowName, const MinimalImageF* img, bool autoSize = false); 41 | void displayImage(const char* windowName, const MinimalImageF3* img, bool autoSize = false); 42 | void displayImage(const char* windowName, const MinimalImageB16* img, bool autoSize = false); 43 | 44 | 45 | void displayImageStitch(const char* windowName, const std::vector images, int cc=0, int rc=0); 46 | void displayImageStitch(const char* windowName, const std::vector images, int cc=0, int rc=0); 47 | void displayImageStitch(const char* windowName, const std::vector images, int cc=0, int rc=0); 48 | void displayImageStitch(const char* windowName, const std::vector images, int cc=0, int rc=0); 49 | 50 | int waitKey(int milliseconds); 51 | void closeAllWindows(); 52 | 53 | } 54 | 55 | } 56 | -------------------------------------------------------------------------------- /src/dso/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 | 25 | 26 | #include "IOWrapper/ImageDisplay.h" 27 | 28 | namespace dso 29 | { 30 | 31 | 32 | namespace IOWrap 33 | { 34 | void displayImage(const char* windowName, const MinimalImageB* img, bool autoSize) {}; 35 | void displayImage(const char* windowName, const MinimalImageB3* img, bool autoSize) {}; 36 | void displayImage(const char* windowName, const MinimalImageF* img, bool autoSize) {}; 37 | void displayImage(const char* windowName, const MinimalImageF3* img, bool autoSize) {}; 38 | void displayImage(const char* windowName, const MinimalImageB16* img, bool autoSize) {}; 39 | 40 | 41 | void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) {}; 42 | void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) {}; 43 | void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) {}; 44 | void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) {}; 45 | 46 | int waitKey(int milliseconds) {return 0;}; 47 | void closeAllWindows() {}; 48 | } 49 | 50 | } 51 | -------------------------------------------------------------------------------- /src/dso/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 | 25 | 26 | #pragma once 27 | #include "util/NumType.h" 28 | #include "util/MinimalImage.h" 29 | 30 | namespace dso 31 | { 32 | namespace IOWrap 33 | { 34 | 35 | MinimalImageB* readImageBW_8U(std::string filename); 36 | MinimalImageB3* readImageRGB_8U(std::string filename); 37 | MinimalImage* readImageBW_16U(std::string filename); 38 | 39 | 40 | MinimalImageB* readStreamBW_8U(char* data, int numBytes); 41 | 42 | void writeImage(std::string filename, MinimalImageB* img); 43 | void writeImage(std::string filename, MinimalImageB3* img); 44 | void writeImage(std::string filename, MinimalImageF* img); 45 | void writeImage(std::string filename, MinimalImageF3* img); 46 | 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /src/dso/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 | 25 | 26 | #include "IOWrapper/ImageRW.h" 27 | 28 | namespace dso 29 | { 30 | 31 | 32 | namespace IOWrap 33 | { 34 | 35 | MinimalImageB* readImageBW_8U(std::string filename) {printf("not implemented. bye!\n"); return 0;}; 36 | MinimalImageB3* readImageRGB_8U(std::string filename) {printf("not implemented. bye!\n"); return 0;}; 37 | MinimalImage* readImageBW_16U(std::string filename) {printf("not implemented. bye!\n"); return 0;}; 38 | MinimalImageB* readStreamBW_8U(char* data, int numBytes) {printf("not implemented. bye!\n"); return 0;}; 39 | void writeImage(std::string filename, MinimalImageB* img) {}; 40 | void writeImage(std::string filename, MinimalImageB3* img) {}; 41 | void writeImage(std::string filename, MinimalImageF* img) {}; 42 | void writeImage(std::string filename, MinimalImageF3* img) {}; 43 | 44 | } 45 | 46 | } 47 | -------------------------------------------------------------------------------- /src/dso/IOWrapper/OpenCV/ImageRW_OpenCV.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO, written by Jakob Engel. 3 | * It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). 4 | * 5 | * Copyright 2022 Lukas von Stumberg 6 | * Copyright 2016 Technical University of Munich and Intel. 7 | * Developed by Jakob Engel , 8 | * for more information see . 9 | * If you use this code, please cite the respective publications as 10 | * listed on the above website. 11 | * 12 | * DSO is free software: you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation, either version 3 of the License, or 15 | * (at your option) any later version. 16 | * 17 | * DSO is distributed in the hope that it will be useful, 18 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 19 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 20 | * GNU General Public License for more details. 21 | * 22 | * You should have received a copy of the GNU General Public License 23 | * along with DSO. If not, see . 24 | */ 25 | 26 | 27 | 28 | #include "IOWrapper/ImageRW.h" 29 | #include 30 | #include 31 | 32 | 33 | namespace dso 34 | { 35 | 36 | namespace IOWrap 37 | { 38 | MinimalImageB* readImageBW_8U(std::string filename) 39 | { 40 | cv::Mat m = cv::imread(filename, cv::IMREAD_GRAYSCALE); 41 | if(m.rows*m.cols==0) 42 | { 43 | printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str()); 44 | return 0; 45 | } 46 | if(m.type() == CV_8UC3) 47 | { 48 | // can happen for webp 49 | cv::cvtColor(m, m, cv::COLOR_BGR2GRAY); 50 | } 51 | if(m.type() != CV_8U) 52 | { 53 | printf("cv::imread did something strange! this may segfault. %i \n", m.type()); 54 | return 0; 55 | } 56 | MinimalImageB* img = new MinimalImageB(m.cols, m.rows); 57 | memcpy(img->data, m.data, m.rows*m.cols); 58 | return img; 59 | } 60 | 61 | MinimalImageB3* readImageRGB_8U(std::string filename) 62 | { 63 | cv::Mat m = cv::imread(filename, cv::IMREAD_COLOR); 64 | if(m.rows*m.cols==0) 65 | { 66 | printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str()); 67 | return 0; 68 | } 69 | if(m.type() != CV_8UC3) 70 | { 71 | printf("cv::imread did something strange! this may segfault. \n"); 72 | return 0; 73 | } 74 | MinimalImageB3* img = new MinimalImageB3(m.cols, m.rows); 75 | memcpy(img->data, m.data, 3*m.rows*m.cols); 76 | return img; 77 | } 78 | 79 | MinimalImage* readImageBW_16U(std::string filename) 80 | { 81 | cv::Mat m = cv::imread(filename, cv::IMREAD_UNCHANGED); 82 | if(m.rows*m.cols==0) 83 | { 84 | printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str()); 85 | return 0; 86 | } 87 | if(m.type() != CV_16U) 88 | { 89 | printf("readImageBW_16U called on image that is not a 16bit grayscale image. this may segfault. \n"); 90 | return 0; 91 | } 92 | MinimalImage* img = new MinimalImage(m.cols, m.rows); 93 | memcpy(img->data, m.data, 2*m.rows*m.cols); 94 | return img; 95 | } 96 | 97 | MinimalImageB* readStreamBW_8U(char* data, int numBytes) 98 | { 99 | cv::Mat m = cv::imdecode(cv::Mat(numBytes,1,CV_8U, data), cv::IMREAD_GRAYSCALE); 100 | if(m.rows*m.cols==0) 101 | { 102 | printf("cv::imdecode could not read stream (%d bytes)! this may segfault. \n", numBytes); 103 | return 0; 104 | } 105 | if(m.type() != CV_8U) 106 | { 107 | printf("cv::imdecode did something strange! this may segfault. \n"); 108 | return 0; 109 | } 110 | MinimalImageB* img = new MinimalImageB(m.cols, m.rows); 111 | memcpy(img->data, m.data, m.rows*m.cols); 112 | return img; 113 | } 114 | 115 | 116 | 117 | void writeImage(std::string filename, MinimalImageB* img) 118 | { 119 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_8U, img->data)); 120 | } 121 | void writeImage(std::string filename, MinimalImageB3* img) 122 | { 123 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_8UC3, img->data)); 124 | } 125 | void writeImage(std::string filename, MinimalImageF* img) 126 | { 127 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_32F, img->data)); 128 | } 129 | void writeImage(std::string filename, MinimalImageF3* img) 130 | { 131 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_32FC3, img->data)); 132 | } 133 | 134 | } 135 | 136 | } 137 | -------------------------------------------------------------------------------- /src/dso/IOWrapper/Pangolin/FollowCamMode.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2023 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #include "FollowCamMode.h" 24 | 25 | namespace dso 26 | { 27 | namespace IOWrap 28 | { 29 | 30 | pangolin::OpenGlRenderState* 31 | FollowCamMode::updateVisualizationCam(const Sophus::SE3d& camToWorldIn, pangolin::OpenGlRenderState& renderStateIn) 32 | { 33 | bool transOnly = followCamTransSetting->Get(); 34 | bool followCam = followCamSetting->Get(); // curr setting value. 35 | followCam = followCam || transOnly; 36 | active = active || followCam; 37 | 38 | if(!active) 39 | { 40 | return &renderStateIn; 41 | } 42 | 43 | output.SetProjectionMatrix(renderStateIn.GetProjectionMatrix()); 44 | 45 | auto&& modelViewIn = renderStateIn.GetModelViewMatrix(); 46 | Sophus::SE3d T_pcam_world(modelViewIn); // world to Pangolin-Cam. 47 | 48 | Sophus::SE3d output_cam_w; 49 | 50 | Sophus::SE3d camToWorld = getAverageCamToWorld(camToWorldIn); 51 | 52 | if(transOnly && !transActive && followActive) 53 | { 54 | // We want transOnly, but before, we were using normal followCam. --> first disable that, then proceed as 55 | // normal. 56 | disableOffsetForFollowCam(camToWorld); 57 | followActive = false; 58 | } 59 | 60 | if(followCam && !transOnly && transActive) 61 | { 62 | // We want followCam, but before we were using transOnly --> first disable that. 63 | Sophus::SE3d noRot{Eigen::Quaterniond::Identity(), camToWorld.translation()}; 64 | disableOffsetForFollowCam(noRot); 65 | followActive = false; 66 | }else if(transOnly || transActive) 67 | { 68 | // We are using translation only or have just disabled it --> set rotation to identity. 69 | camToWorld.setQuaternion(Eigen::Quaterniond::Identity()); 70 | } 71 | 72 | if(followCam) 73 | { 74 | if(!followActive) 75 | { 76 | // Activate follow cam --> set offset! 77 | setOffsetForFollowCam(camToWorld); 78 | } 79 | output_cam_w = T_pcam_world * currOffset * camToWorld.inverse(); 80 | }else 81 | { 82 | if(followActive) 83 | { 84 | // Disable follow cam --> set offset! 85 | disableOffsetForFollowCam(camToWorld); 86 | } 87 | output_cam_w = T_pcam_world * currOffset; 88 | } 89 | 90 | output.SetModelViewMatrix(output_cam_w.matrix()); 91 | 92 | followActive = followCam; 93 | transActive = transOnly; 94 | 95 | return &output; 96 | } 97 | 98 | void FollowCamMode::setOffsetForFollowCam(Sophus::SE3d& camToWorld) 99 | { 100 | currOffset = currOffset * camToWorld; 101 | } 102 | 103 | void FollowCamMode::disableOffsetForFollowCam(Sophus::SE3d& camToWorld) 104 | { 105 | currOffset = currOffset * camToWorld.inverse(); 106 | } 107 | 108 | void FollowCamMode::createPangolinSettings() 109 | { 110 | followCamSetting = std::make_unique>("ui.followCam", false, true); 111 | followCamTransSetting = std::make_unique>("ui.followCamTrans", false, true); 112 | smoothnessSetting = std::make_unique>("ui.smoothness", 50, 1, 100, false); 113 | } 114 | 115 | Sophus::SE3d FollowCamMode::getAverageCamToWorld(const Sophus::SE3d& camToWorld) 116 | { 117 | int num = smoothnessSetting->Get(); 118 | 119 | rotations.emplace_back(camToWorld.unit_quaternion()); 120 | translations.emplace_back(camToWorld.translation()); 121 | 122 | while(rotations.size() > num) 123 | { 124 | rotations.pop_front(); 125 | } 126 | while(translations.size() > num) 127 | { 128 | translations.pop_front(); 129 | } 130 | 131 | Eigen::Quaterniond avgQuat = computeAverageQuat(rotations.begin(), rotations.end()); 132 | Eigen::Vector3d avgTrans; 133 | avgTrans.setZero(); 134 | for(auto&& trans : translations) 135 | { 136 | avgTrans += trans; 137 | } 138 | avgTrans /= translations.size(); 139 | 140 | return Sophus::SE3d(avgQuat, avgTrans); 141 | } 142 | 143 | } 144 | } -------------------------------------------------------------------------------- /src/dso/IOWrapper/Pangolin/FollowCamMode.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2023 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #ifndef DMVIO_FOLLOWCAMMODE_H 24 | #define DMVIO_FOLLOWCAMMODE_H 25 | 26 | #include 27 | #include 28 | 29 | namespace dso 30 | { 31 | namespace IOWrap 32 | { 33 | 34 | class FollowCamMode 35 | { 36 | public: 37 | FollowCamMode() = default; 38 | 39 | pangolin::OpenGlRenderState* updateVisualizationCam(const Sophus::SE3d& camToWorld, 40 | pangolin::OpenGlRenderState& renderStateIn); 41 | 42 | void createPangolinSettings(); 43 | 44 | private: 45 | Sophus::SE3d getAverageCamToWorld(const Sophus::SE3d& camToWorld); 46 | 47 | void setOffsetForFollowCam(Sophus::SE3d& camToWorld); 48 | void disableOffsetForFollowCam(Sophus::SE3d& camToWorld); 49 | 50 | std::unique_ptr> followCamSetting, followCamTransSetting; 51 | std::unique_ptr> smoothnessSetting; 52 | 53 | bool followActive = false; 54 | bool transActive = false; 55 | Sophus::SE3d currOffset; 56 | 57 | pangolin::OpenGlRenderState output; 58 | 59 | // camToWorld translations / rotations for calculating the smoothed follow trajectory. 60 | std::deque rotations; 61 | std::deque translations; 62 | bool active = false; 63 | }; 64 | 65 | // Compute average of multiple Eigen::Quaterniond. Assumes that the quaternions are normalized! 66 | template Eigen::Quaterniond computeAverageQuat(T begin, T end) 67 | { 68 | // compute sum of q * qT 69 | Eigen::Matrix4d matrix = Eigen::Matrix4d::Zero(); 70 | for(auto it = begin; it != end; ++it) 71 | { 72 | matrix += it->coeffs() * it->coeffs().transpose(); 73 | } 74 | // solution is the unit Eigen vector corresponding to the largest Eigen value. 75 | auto eigenVecs = Eigen::SelfAdjointEigenSolver(matrix).eigenvectors(); 76 | return Eigen::Quaterniond(eigenVecs.col(eigenVecs.cols() - 1)); 77 | } 78 | 79 | } 80 | } 81 | 82 | #endif //DMVIO_FOLLOWCAMMODE_H 83 | -------------------------------------------------------------------------------- /src/dso/IOWrapper/Pangolin/KeyFrameDisplay.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO, written by Jakob Engel. 3 | * It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). 4 | * 5 | * Copyright 2022 Lukas von Stumberg 6 | * Copyright 2016 Technical University of Munich and Intel. 7 | * Developed by Jakob Engel , 8 | * for more information see . 9 | * If you use this code, please cite the respective publications as 10 | * listed on the above website. 11 | * 12 | * DSO is free software: you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation, either version 3 of the License, or 15 | * (at your option) any later version. 16 | * 17 | * DSO is distributed in the hope that it will be useful, 18 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 19 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 20 | * GNU General Public License for more details. 21 | * 22 | * You should have received a copy of the GNU General Public License 23 | * along with DSO. If not, see . 24 | */ 25 | 26 | 27 | #pragma once 28 | 29 | #undef Success 30 | #include 31 | #include "util/NumType.h" 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | namespace dso 38 | { 39 | class CalibHessian; 40 | class FrameHessian; 41 | class FrameShell; 42 | 43 | namespace IOWrap 44 | { 45 | 46 | template 47 | struct InputPointSparse 48 | { 49 | float u; 50 | float v; 51 | float idpeth; 52 | float idepth_hessian; 53 | float relObsBaseline; 54 | int numGoodRes; 55 | unsigned char color[ppp]; 56 | unsigned char status; 57 | }; 58 | 59 | struct MyVertex 60 | { 61 | float point[3]; 62 | unsigned char color[4]; 63 | }; 64 | 65 | // stores a pointcloud associated to a Keyframe. 66 | class KeyFrameDisplay 67 | { 68 | 69 | public: 70 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 71 | KeyFrameDisplay(); 72 | ~KeyFrameDisplay(); 73 | 74 | // copies points from KF over to internal buffer, 75 | // keeping some additional information so we can render it differently. 76 | void setFromKF(FrameHessian* fh, CalibHessian* HCalib); 77 | 78 | // copies points from KF over to internal buffer, 79 | // keeping some additional information so we can render it differently. 80 | void setFromF(FrameShell* fs, CalibHessian* HCalib); 81 | 82 | void setFromPose(const Sophus::SE3& pose, CalibHessian* HCalib); 83 | 84 | // copies & filters internal data to GL buffer for rendering. if nothing to do: does nothing. 85 | bool refreshPC(bool canRefresh, float scaledTH, float absTH, int mode, float minBS, int sparsity); 86 | 87 | // renders cam & pointcloud. 88 | void drawCam(float lineWidth = 1, float* color = 0, float sizeFactor=1); 89 | void drawPC(float pointSize); 90 | 91 | int id; 92 | bool active; 93 | SE3 camToWorld; 94 | 95 | inline bool operator < (const KeyFrameDisplay& other) const 96 | { 97 | return (id < other.id); 98 | } 99 | 100 | 101 | private: 102 | float fx,fy,cx,cy; 103 | float fxi,fyi,cxi,cyi; 104 | int width, height; 105 | 106 | float my_scaledTH, my_absTH, my_scale; 107 | int my_sparsifyFactor; 108 | int my_displayMode; 109 | float my_minRelBS; 110 | bool needRefresh; 111 | 112 | 113 | int numSparsePoints; 114 | int numSparseBufferSize; 115 | InputPointSparse* originalInputSparse; 116 | 117 | 118 | bool bufferValid; 119 | int numGLBufferPoints; 120 | int numGLBufferGoodPoints; 121 | pangolin::GlBuffer vertexBuffer; 122 | pangolin::GlBuffer colorBuffer; 123 | }; 124 | 125 | } 126 | } 127 | 128 | -------------------------------------------------------------------------------- /src/dso/OptimizationBackend/AccumulatedSCHessian.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 | 25 | #pragma once 26 | 27 | 28 | #include "util/NumType.h" 29 | #include "util/IndexThreadReduce.h" 30 | #include "OptimizationBackend/MatrixAccumulators.h" 31 | #include "vector" 32 | #include 33 | 34 | namespace dso 35 | { 36 | 37 | class EFPoint; 38 | class EnergyFunctional; 39 | 40 | 41 | class AccumulatedSCHessianSSE 42 | { 43 | public: 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 45 | inline AccumulatedSCHessianSSE() 46 | { 47 | for(int i=0;i[n*n]; 73 | accEB[tid] = new AccumulatorX<8>[n*n]; 74 | accD[tid] = new AccumulatorXX<8,8>[n*n*n]; 75 | } 76 | accbc[tid].initialize(); 77 | accHcc[tid].initialize(); 78 | 79 | for(int i=0;i* red, MatXX &H, VecX &b, EnergyFunctional const * const EF, bool MT) 94 | { 95 | // sum up, splitting by bock in square. 96 | if(MT) 97 | { 98 | MatXX Hs[NUM_THREADS]; 99 | VecX bs[NUM_THREADS]; 100 | for(int i=0;ireduce(boost::bind(&AccumulatedSCHessianSSE::stitchDoubleInternal, 108 | this,Hs, bs, EF, _1, _2, _3, _4), 0, nframes[0]*nframes[0], 0); 109 | 110 | // sum up results 111 | H = Hs[0]; 112 | b = bs[0]; 113 | 114 | for(int i=1;i(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose(); 132 | } 133 | } 134 | 135 | 136 | AccumulatorXX<8,CPARS>* accE[NUM_THREADS]; 137 | AccumulatorX<8>* accEB[NUM_THREADS]; 138 | AccumulatorXX<8,8>* accD[NUM_THREADS]; 139 | AccumulatorXX accHcc[NUM_THREADS]; 140 | AccumulatorX accbc[NUM_THREADS]; 141 | int nframes[NUM_THREADS]; 142 | 143 | 144 | void addPointsInternal( 145 | std::vector* points, bool shiftPriorToZero, 146 | int min=0, int max=1, Vec10* stats=0, int tid=0) 147 | { 148 | for(int i=min;i, 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 | 25 | #pragma once 26 | 27 | 28 | #include "util/NumType.h" 29 | #include "OptimizationBackend/MatrixAccumulators.h" 30 | #include "vector" 31 | #include 32 | #include "util/IndexThreadReduce.h" 33 | 34 | 35 | namespace dso 36 | { 37 | 38 | class EFPoint; 39 | class EnergyFunctional; 40 | 41 | 42 | 43 | class AccumulatedTopHessianSSE 44 | { 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 47 | inline AccumulatedTopHessianSSE() 48 | { 49 | for(int tid=0;tid < NUM_THREADS; tid++) 50 | { 51 | nres[tid]=0; 52 | acc[tid]=0; 53 | nframes[tid]=0; 54 | } 55 | 56 | }; 57 | inline ~AccumulatedTopHessianSSE() 58 | { 59 | for(int tid=0;tid < NUM_THREADS; tid++) 60 | { 61 | if(acc[tid] != 0) delete[] acc[tid]; 62 | } 63 | }; 64 | 65 | inline void setZero(int nFrames, int min=0, int max=1, Vec10* stats=0, int tid=0) 66 | { 67 | 68 | if(nFrames != nframes[tid]) 69 | { 70 | if(acc[tid] != 0) delete[] acc[tid]; 71 | #if USE_XI_MODEL 72 | acc[tid] = new Accumulator14[nFrames*nFrames]; 73 | #else 74 | acc[tid] = new AccumulatorApprox[nFrames*nFrames]; 75 | #endif 76 | } 77 | 78 | for(int i=0;i void addPoint(EFPoint* p, EnergyFunctional const * const ef, int tid=0); 88 | 89 | 90 | 91 | void stitchDoubleMT(IndexThreadReduce* red, MatXX &H, VecX &b, EnergyFunctional const * const EF, bool usePrior, bool MT) 92 | { 93 | // sum up, splitting by bock in square. 94 | if(MT) 95 | { 96 | MatXX Hs[NUM_THREADS]; 97 | VecX bs[NUM_THREADS]; 98 | for(int i=0;ireduce(boost::bind(&AccumulatedTopHessianSSE::stitchDoubleInternal, 106 | this,Hs, bs, EF, usePrior, _1, _2, _3, _4), 0, nframes[0]*nframes[0], 0); 107 | 108 | // sum up results 109 | H = Hs[0]; 110 | b = bs[0]; 111 | 112 | for(int i=1;i(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose(); 131 | 132 | for(int t=h+1;t(hIdx, tIdx).noalias() += H.block<8,8>(tIdx, hIdx).transpose(); 136 | H.block<8,8>(tIdx, hIdx).noalias() = H.block<8,8>(hIdx, tIdx).transpose(); 137 | } 138 | } 139 | } 140 | 141 | 142 | 143 | 144 | int nframes[NUM_THREADS]; 145 | 146 | EIGEN_ALIGN16 AccumulatorApprox* acc[NUM_THREADS]; 147 | 148 | 149 | int nres[NUM_THREADS]; 150 | 151 | 152 | template void addPointsInternal( 153 | std::vector* points, EnergyFunctional const * const ef, 154 | int min=0, int max=1, Vec10* stats=0, int tid=0) 155 | { 156 | for(int i=min;i((*points)[i],ef,tid); 157 | } 158 | 159 | 160 | 161 | private: 162 | 163 | void stitchDoubleInternal( 164 | MatXX* H, VecX* b, EnergyFunctional const * const EF, bool usePrior, 165 | int min, int max, Vec10* stats, int tid); 166 | }; 167 | } 168 | 169 | -------------------------------------------------------------------------------- /src/dso/OptimizationBackend/EnergyFunctional.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO, written by Jakob Engel. 3 | * It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). 4 | * 5 | * Copyright 2022 Lukas von Stumberg 6 | * Copyright 2016 Technical University of Munich and Intel. 7 | * Developed by Jakob Engel , 8 | * for more information see . 9 | * If you use this code, please cite the respective publications as 10 | * listed on the above website. 11 | * 12 | * DSO is free software: you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation, either version 3 of the License, or 15 | * (at your option) any later version. 16 | * 17 | * DSO is distributed in the hope that it will be useful, 18 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 19 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 20 | * GNU General Public License for more details. 21 | * 22 | * You should have received a copy of the GNU General Public License 23 | * along with DSO. If not, see . 24 | */ 25 | 26 | 27 | #pragma once 28 | 29 | 30 | #include "util/NumType.h" 31 | #include "util/IndexThreadReduce.h" 32 | #include "vector" 33 | #include 34 | #include "map" 35 | 36 | 37 | namespace dmvio 38 | { 39 | class BAGTSAMIntegration; 40 | } 41 | 42 | namespace dso 43 | { 44 | 45 | class PointFrameResidual; 46 | class CalibHessian; 47 | class FrameHessian; 48 | class PointHessian; 49 | 50 | 51 | class EFResidual; 52 | class EFPoint; 53 | class EFFrame; 54 | class EnergyFunctional; 55 | class AccumulatedTopHessian; 56 | class AccumulatedTopHessianSSE; 57 | class AccumulatedSCHessian; 58 | class AccumulatedSCHessianSSE; 59 | 60 | 61 | extern bool EFAdjointsValid; 62 | extern bool EFIndicesValid; 63 | extern bool EFDeltaValid; 64 | 65 | 66 | 67 | class EnergyFunctional { 68 | public: 69 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 70 | friend class EFFrame; 71 | friend class EFPoint; 72 | friend class EFResidual; 73 | friend class AccumulatedTopHessian; 74 | friend class AccumulatedTopHessianSSE; 75 | friend class AccumulatedSCHessian; 76 | friend class AccumulatedSCHessianSSE; 77 | 78 | EnergyFunctional(dmvio::BAGTSAMIntegration >samIntegration); 79 | ~EnergyFunctional(); 80 | 81 | 82 | EFResidual* insertResidual(PointFrameResidual* r); 83 | EFFrame* insertFrame(FrameHessian* fh, CalibHessian* Hcalib); 84 | EFPoint* insertPoint(PointHessian* ph); 85 | 86 | void dropResidual(EFResidual* r); 87 | void marginalizeFrame(EFFrame* fh); 88 | void removePoint(EFPoint* ph); 89 | 90 | 91 | 92 | void marginalizePointsF(); 93 | void dropPointsF(); 94 | void solveSystemF(int iteration, double lambda, CalibHessian* HCalib); 95 | double calcMEnergyF(bool useNewValues); 96 | double calcLEnergyF_MT(); 97 | 98 | 99 | void makeIDX(); 100 | 101 | void setDeltaF(CalibHessian* HCalib); 102 | 103 | void setAdjointsF(CalibHessian* Hcalib); 104 | 105 | std::vector frames; 106 | int nPoints, nFrames, nResiduals; 107 | 108 | // HMForGTSAM, bMForGTSAM only contain marginalized points until the next time a keyframe is marginalized. 109 | // With each keyframe marginalization the information in them is transferred to the GTSAMIntegration. 110 | MatXX HM, HMForGTSAM; 111 | VecX bM, bMForGTSAM; 112 | 113 | int resInA, resInL, resInM; 114 | MatXX lastHS; 115 | VecX lastbS; 116 | VecX lastX; 117 | std::vector lastNullspaces_forLogging; 118 | std::vector lastNullspaces_pose; 119 | std::vector lastNullspaces_scale; 120 | std::vector lastNullspaces_affA; 121 | std::vector lastNullspaces_affB; 122 | 123 | IndexThreadReduce* red; 124 | 125 | 126 | std::map, 129 | Eigen::aligned_allocator> 130 | > connectivityMap; 131 | 132 | private: 133 | 134 | VecX getStitchedDeltaF() const; 135 | 136 | void resubstituteF_MT(VecX x, CalibHessian* HCalib, bool MT); 137 | void resubstituteFPt(const VecCf &xc, Mat18f* xAd, int min, int max, Vec10* stats, int tid); 138 | 139 | void accumulateAF_MT(MatXX &H, VecX &b, bool MT); 140 | void accumulateLF_MT(MatXX &H, VecX &b, bool MT); 141 | void accumulateSCF_MT(MatXX &H, VecX &b, bool MT); 142 | 143 | void calcLEnergyPt(int min, int max, Vec10* stats, int tid); 144 | 145 | void orthogonalize(VecX* b, MatXX* H); 146 | Mat18f* adHTdeltaF; 147 | 148 | Mat88* adHost; 149 | Mat88* adTarget; 150 | 151 | Mat88f* adHostF; 152 | Mat88f* adTargetF; 153 | 154 | 155 | VecC cPrior; 156 | VecCf cDeltaF; 157 | VecCf cPriorF; 158 | 159 | AccumulatedTopHessianSSE* accSSE_top_L; 160 | AccumulatedTopHessianSSE* accSSE_top_A; 161 | 162 | 163 | AccumulatedSCHessianSSE* accSSE_bot; 164 | 165 | std::vector allPoints; 166 | std::vector allPointsToMarg; 167 | 168 | float currentLambda; 169 | 170 | dmvio::BAGTSAMIntegration >samIntegration; 171 | }; 172 | } 173 | 174 | -------------------------------------------------------------------------------- /src/dso/OptimizationBackend/EnergyFunctionalStructs.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 | 25 | #include "OptimizationBackend/EnergyFunctionalStructs.h" 26 | #include "OptimizationBackend/EnergyFunctional.h" 27 | #include "FullSystem/FullSystem.h" 28 | #include "FullSystem/HessianBlocks.h" 29 | #include "FullSystem/Residuals.h" 30 | 31 | #if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) 32 | #include "SSE2NEON.h" 33 | #endif 34 | 35 | namespace dso 36 | { 37 | 38 | 39 | void EFResidual::takeDataF() 40 | { 41 | std::swap(J, data->J); 42 | 43 | Vec2f JI_JI_Jd = J->JIdx2 * J->Jpdd; 44 | 45 | for(int i=0;i<6;i++) 46 | JpJdF[i] = J->Jpdxi[0][i]*JI_JI_Jd[0] + J->Jpdxi[1][i] * JI_JI_Jd[1]; 47 | 48 | JpJdF.segment<2>(6) = J->JabJIdx*J->Jpdd; 49 | } 50 | 51 | 52 | void EFFrame::takeData() 53 | { 54 | prior = data->getPrior().head<8>(); 55 | delta = data->get_state_minus_stateZero().head<8>(); 56 | delta_prior = (data->get_state() - data->getPriorZero()).head<8>(); 57 | 58 | 59 | 60 | // Vec10 state_zero = data->get_state_zero(); 61 | // state_zero.segment<3>(0) = SCALE_XI_TRANS * state_zero.segment<3>(0); 62 | // state_zero.segment<3>(3) = SCALE_XI_ROT * state_zero.segment<3>(3); 63 | // state_zero[6] = SCALE_A * state_zero[6]; 64 | // state_zero[7] = SCALE_B * state_zero[7]; 65 | // state_zero[8] = SCALE_A * state_zero[8]; 66 | // state_zero[9] = SCALE_B * state_zero[9]; 67 | // 68 | // std::cout << "state_zero: " << state_zero.transpose() << "\n"; 69 | 70 | 71 | assert(data->frameID != -1); 72 | 73 | frameID = data->frameID; 74 | } 75 | 76 | 77 | 78 | 79 | void EFPoint::takeData() 80 | { 81 | priorF = data->hasDepthPrior ? setting_idepthFixPrior*SCALE_IDEPTH*SCALE_IDEPTH : 0; 82 | if(setting_solverMode & SOLVER_REMOVE_POSEPRIOR) priorF=0; 83 | 84 | deltaF = data->idepth-data->idepth_zero; 85 | } 86 | 87 | 88 | void EFResidual::fixLinearizationF(EnergyFunctional* ef) 89 | { 90 | Vec8f dp = ef->adHTdeltaF[hostIDX+ef->nFrames*targetIDX]; 91 | 92 | // compute Jp*delta 93 | __m128 Jp_delta_x = _mm_set1_ps(J->Jpdxi[0].dot(dp.head<6>()) 94 | +J->Jpdc[0].dot(ef->cDeltaF) 95 | +J->Jpdd[0]*point->deltaF); 96 | __m128 Jp_delta_y = _mm_set1_ps(J->Jpdxi[1].dot(dp.head<6>()) 97 | +J->Jpdc[1].dot(ef->cDeltaF) 98 | +J->Jpdd[1]*point->deltaF); 99 | __m128 delta_a = _mm_set1_ps((float)(dp[6])); 100 | __m128 delta_b = _mm_set1_ps((float)(dp[7])); 101 | 102 | for(int i=0;iresF)+i); 106 | rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JIdx))+i),Jp_delta_x)); 107 | rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JIdx+1))+i),Jp_delta_y)); 108 | rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JabF))+i),delta_a)); 109 | rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JabF+1))+i),delta_b)); 110 | _mm_store_ps(((float*)&res_toZeroF)+i, rtz); 111 | } 112 | 113 | isLinearized = true; 114 | } 115 | 116 | } 117 | -------------------------------------------------------------------------------- /src/dso/OptimizationBackend/EnergyFunctionalStructs.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 | 25 | #pragma once 26 | 27 | 28 | #include "util/NumType.h" 29 | #include "vector" 30 | #include 31 | #include "OptimizationBackend/RawResidualJacobian.h" 32 | 33 | namespace dso 34 | { 35 | 36 | class PointFrameResidual; 37 | class CalibHessian; 38 | class FrameHessian; 39 | class PointHessian; 40 | 41 | class EFResidual; 42 | class EFPoint; 43 | class EFFrame; 44 | class EnergyFunctional; 45 | 46 | 47 | 48 | 49 | 50 | 51 | class EFResidual 52 | { 53 | public: 54 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 55 | 56 | inline EFResidual(PointFrameResidual* org, EFPoint* point_, EFFrame* host_, EFFrame* target_) : 57 | data(org), point(point_), host(host_), target(target_) 58 | { 59 | isLinearized=false; 60 | isActiveAndIsGoodNEW=false; 61 | J = new RawResidualJacobian(); 62 | assert(((long)this)%16==0); 63 | assert(((long)J)%16==0); 64 | } 65 | inline ~EFResidual() 66 | { 67 | delete J; 68 | } 69 | 70 | 71 | void takeDataF(); 72 | 73 | 74 | void fixLinearizationF(EnergyFunctional* ef); 75 | 76 | 77 | // structural pointers 78 | PointFrameResidual* data; 79 | int hostIDX, targetIDX; 80 | EFPoint* point; 81 | EFFrame* host; 82 | EFFrame* target; 83 | int idxInAll; 84 | 85 | RawResidualJacobian* J; 86 | 87 | VecNRf res_toZeroF; 88 | Vec8f JpJdF; 89 | 90 | 91 | // status. 92 | bool isLinearized; 93 | 94 | // if residual is not OOB & not OUTLIER & should be used during accumulations 95 | bool isActiveAndIsGoodNEW; 96 | inline const bool &isActive() const {return isActiveAndIsGoodNEW;} 97 | }; 98 | 99 | 100 | enum EFPointStatus {PS_GOOD=0, PS_MARGINALIZE, PS_DROP}; 101 | 102 | class EFPoint 103 | { 104 | public: 105 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 106 | EFPoint(PointHessian* d, EFFrame* host_) : data(d),host(host_) 107 | { 108 | takeData(); 109 | stateFlag=EFPointStatus::PS_GOOD; 110 | } 111 | void takeData(); 112 | 113 | PointHessian* data; 114 | 115 | 116 | 117 | float priorF; 118 | float deltaF; 119 | 120 | 121 | // constant info (never changes in-between). 122 | int idxInPoints; 123 | EFFrame* host; 124 | 125 | // contains all residuals. 126 | std::vector residualsAll; 127 | 128 | float bdSumF; 129 | float HdiF; 130 | float Hdd_accLF; 131 | VecCf Hcd_accLF; 132 | float bd_accLF; 133 | float Hdd_accAF; 134 | VecCf Hcd_accAF; 135 | float bd_accAF; 136 | 137 | 138 | EFPointStatus stateFlag; 139 | }; 140 | 141 | 142 | 143 | class EFFrame 144 | { 145 | public: 146 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 147 | EFFrame(FrameHessian* d) : data(d) 148 | { 149 | takeData(); 150 | } 151 | void takeData(); 152 | 153 | 154 | Vec8 prior; // prior hessian (diagonal) 155 | Vec8 delta_prior; // = state-state_prior (E_prior = (delta_prior)' * diag(prior) * (delta_prior) 156 | Vec8 delta; // state - state_zero. 157 | 158 | 159 | 160 | std::vector points; 161 | FrameHessian* data; 162 | int idx; // idx in frames. 163 | 164 | int frameID; 165 | }; 166 | 167 | } 168 | 169 | -------------------------------------------------------------------------------- /src/dso/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 | 25 | #pragma once 26 | 27 | 28 | #include "util/NumType.h" 29 | 30 | namespace dso 31 | { 32 | struct RawResidualJacobian 33 | { 34 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 35 | // ================== new structure: save independently =============. 36 | VecNRf resF; 37 | 38 | // the two rows of d[x,y]/d[xi]. 39 | Vec6f Jpdxi[2]; // 2x6 40 | 41 | // the two rows of d[x,y]/d[C]. 42 | VecCf Jpdc[2]; // 2x4 43 | 44 | // the two rows of d[x,y]/d[idepth]. 45 | Vec2f Jpdd; // 2x1 46 | 47 | // the two columns of d[r]/d[x,y]. 48 | VecNRf JIdx[2]; // 9x2 49 | 50 | // = the two columns of d[r] / d[ab] 51 | VecNRf JabF[2]; // 9x2 52 | 53 | 54 | // = JIdx^T * JIdx (inner product). Only as a shorthand. 55 | Mat22f JIdx2; // 2x2 56 | // = Jab^T * JIdx (inner product). Only as a shorthand. 57 | Mat22f JabJIdx; // 2x2 58 | // = Jab^T * Jab (inner product). Only as a shorthand. 59 | Mat22f Jab2; // 2x2 60 | 61 | }; 62 | } 63 | 64 | -------------------------------------------------------------------------------- /src/dso/util/FrameShell.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO, written by Jakob Engel. 3 | * It has been modified by Lukas von Stumberg for the inclusion in DM-VIO (http://vision.in.tum.de/dm-vio). 4 | * 5 | * Copyright 2022 Lukas von Stumberg 6 | * Copyright 2016 Technical University of Munich and Intel. 7 | * Developed by Jakob Engel , 8 | * for more information see . 9 | * If you use this code, please cite the respective publications as 10 | * listed on the above website. 11 | * 12 | * DSO is free software: you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation, either version 3 of the License, or 15 | * (at your option) any later version. 16 | * 17 | * DSO is distributed in the hope that it will be useful, 18 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 19 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 20 | * GNU General Public License for more details. 21 | * 22 | * You should have received a copy of the GNU General Public License 23 | * along with DSO. If not, see . 24 | */ 25 | 26 | 27 | #pragma once 28 | 29 | #include "util/NumType.h" 30 | #include "algorithm" 31 | #include 32 | 33 | namespace dso 34 | { 35 | 36 | 37 | class FrameShell 38 | { 39 | public: 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 41 | int id; // INTERNAL ID, starting at zero. 42 | int incoming_id; // ID passed into DSO 43 | double timestamp; // timestamp passed into DSO. 44 | 45 | // set once after tracking 46 | SE3 camToTrackingRef; 47 | FrameShell* trackingRef; 48 | 49 | // constantly adapted. 50 | SE3 camToWorld; // Write: TRACKING, while frame is still fresh; MAPPING: only when locked [shellPoseMutex]. 51 | AffLight aff_g2l; 52 | bool poseValid; 53 | bool trackingWasGood; 54 | 55 | int keyframeId = -1; // Id of the KF or -1 for non-KFs. 56 | 57 | // statisitcs 58 | int statistics_outlierResOnThis; 59 | int statistics_goodResOnThis; 60 | int marginalizedAt; 61 | double movedByOpt; 62 | 63 | static boost::mutex shellPoseMutex; 64 | 65 | inline FrameShell() 66 | { 67 | id=0; 68 | poseValid=true; 69 | trackingWasGood = true; 70 | camToWorld = SE3(); 71 | timestamp=0; 72 | marginalizedAt=-1; 73 | movedByOpt=0; 74 | statistics_outlierResOnThis=statistics_goodResOnThis=0; 75 | trackingRef=0; 76 | camToTrackingRef = SE3(); 77 | } 78 | }; 79 | 80 | 81 | } 82 | 83 | -------------------------------------------------------------------------------- /src/dso/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 | 25 | #pragma once 26 | #include 27 | #include 28 | 29 | 30 | namespace dso 31 | { 32 | 33 | 34 | class ImageAndExposure 35 | { 36 | public: 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 38 | float* image; // irradiance. between 0 and 256 39 | int w,h; // width and height; 40 | double timestamp; 41 | float exposure_time; // exposure time in ms. 42 | inline ImageAndExposure(int w_, int h_, double timestamp_=0) : w(w_), h(h_), timestamp(timestamp_) 43 | { 44 | image = new float[w*h]; 45 | exposure_time=1; 46 | } 47 | inline ~ImageAndExposure() 48 | { 49 | delete[] image; 50 | } 51 | 52 | inline void copyMetaTo(ImageAndExposure &other) 53 | { 54 | other.exposure_time = exposure_time; 55 | } 56 | 57 | inline ImageAndExposure* getDeepCopy() 58 | { 59 | ImageAndExposure* img = new ImageAndExposure(w,h,timestamp); 60 | img->exposure_time = exposure_time; 61 | memcpy(img->image, image, w*h*sizeof(float)); 62 | return img; 63 | } 64 | }; 65 | 66 | 67 | } 68 | -------------------------------------------------------------------------------- /src/dso/util/MinimalImage.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 | 25 | #pragma once 26 | 27 | #include "util/NumType.h" 28 | #include "algorithm" 29 | 30 | namespace dso 31 | { 32 | 33 | template 34 | class MinimalImage 35 | { 36 | public: 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 38 | int w; 39 | int h; 40 | T* data; 41 | 42 | /* 43 | * creates minimal image with own memory 44 | */ 45 | inline MinimalImage(int w_, int h_) : w(w_), h(h_) 46 | { 47 | data = new T[w*h]; 48 | ownData=true; 49 | } 50 | 51 | /* 52 | * creates minimal image wrapping around existing memory 53 | */ 54 | inline MinimalImage(int w_, int h_, T* data_) : w(w_), h(h_) 55 | { 56 | data = data_; 57 | ownData=false; 58 | } 59 | 60 | inline ~MinimalImage() 61 | { 62 | if(ownData) delete [] data; 63 | } 64 | 65 | inline MinimalImage* getClone() 66 | { 67 | MinimalImage* clone = new MinimalImage(w,h); 68 | memcpy(clone->data, data, sizeof(T)*w*h); 69 | return clone; 70 | } 71 | 72 | 73 | inline T& at(int x, int y) {return data[(int)x+((int)y)*w];} 74 | inline T& at(int i) {return data[i];} 75 | 76 | inline void setBlack() 77 | { 78 | memset(data, 0, sizeof(T)*w*h); 79 | } 80 | 81 | inline void setConst(T val) 82 | { 83 | for(int i=0;i Vec3b; 160 | typedef MinimalImage MinimalImageF; 161 | typedef MinimalImage MinimalImageF3; 162 | typedef MinimalImage MinimalImageB; 163 | typedef MinimalImage MinimalImageB3; 164 | typedef MinimalImage MinimalImageB16; 165 | 166 | } 167 | 168 | -------------------------------------------------------------------------------- /src/dso/util/Undistort.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 | 25 | #pragma once 26 | 27 | #include "util/ImageAndExposure.h" 28 | #include "util/MinimalImage.h" 29 | #include "util/NumType.h" 30 | #include "Eigen/Core" 31 | 32 | 33 | 34 | 35 | 36 | namespace dso 37 | { 38 | 39 | 40 | class PhotometricUndistorter 41 | { 42 | public: 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 44 | PhotometricUndistorter(std::string file, std::string noiseImage, std::string vignetteImage, int w_, int h_); 45 | ~PhotometricUndistorter(); 46 | 47 | // removes readout noise, and converts to irradiance. 48 | // affine normalizes values to 0 <= I < 256. 49 | // raw irradiance = a*I + b. 50 | // output will be written in [output]. 51 | template void processFrame(T* image_in, float exposure_time, float factor=1); 52 | void unMapFloatImage(float* image); 53 | 54 | ImageAndExposure* output; 55 | 56 | float* getG() {if(!valid) return 0; else return G;}; 57 | private: 58 | float G[256*256]; 59 | int GDepth; 60 | float* vignetteMap; 61 | float* vignetteMapInv; 62 | int w,h; 63 | bool valid; 64 | }; 65 | 66 | 67 | class Undistort 68 | { 69 | public: 70 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 71 | virtual ~Undistort(); 72 | 73 | virtual void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const = 0; 74 | 75 | 76 | inline const Mat33 getK() const {return K;}; 77 | inline const Eigen::Vector2i getSize() const {return Eigen::Vector2i(w,h);}; 78 | inline const VecX getOriginalParameter() const {return parsOrg;}; 79 | inline const Eigen::Vector2i getOriginalSize() {return Eigen::Vector2i(wOrg,hOrg);}; 80 | inline bool isValid() {return valid;}; 81 | 82 | template 83 | ImageAndExposure* undistort(const MinimalImage* image_raw, float exposure=0, double timestamp=0, float factor=1) const; 84 | static Undistort* getUndistorterForFile(std::string configFilename, std::string gammaFilename, std::string vignetteFilename); 85 | 86 | void loadPhotometricCalibration(std::string file, std::string noiseImage, std::string vignetteImage); 87 | 88 | PhotometricUndistorter* photometricUndist; 89 | 90 | protected: 91 | int w, h, wOrg, hOrg, wUp, hUp; 92 | int upsampleUndistFactor; 93 | Mat33 K; 94 | VecX parsOrg; 95 | bool valid; 96 | bool passthrough; 97 | 98 | float* remapX; 99 | float* remapY; 100 | 101 | void applyBlurNoise(float* img) const; 102 | 103 | void makeOptimalK_crop(); 104 | void makeOptimalK_full(); 105 | 106 | void readFromFile(const char* configFileName, int nPars, std::string prefix = ""); 107 | }; 108 | 109 | class UndistortFOV : public Undistort 110 | { 111 | public: 112 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 113 | 114 | UndistortFOV(const char* configFileName, bool noprefix); 115 | ~UndistortFOV(); 116 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; 117 | 118 | }; 119 | 120 | class UndistortRadTan : public Undistort 121 | { 122 | public: 123 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 124 | UndistortRadTan(const char* configFileName, bool noprefix); 125 | ~UndistortRadTan(); 126 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; 127 | 128 | }; 129 | 130 | class UndistortEquidistant : public Undistort 131 | { 132 | public: 133 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 134 | UndistortEquidistant(const char* configFileName, bool noprefix); 135 | ~UndistortEquidistant(); 136 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; 137 | 138 | }; 139 | 140 | class UndistortPinhole : public Undistort 141 | { 142 | public: 143 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 144 | UndistortPinhole(const char* configFileName, bool noprefix); 145 | ~UndistortPinhole(); 146 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; 147 | 148 | private: 149 | float inputCalibration[8]; 150 | }; 151 | 152 | class UndistortKB : public Undistort 153 | { 154 | public: 155 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 156 | UndistortKB(const char* configFileName, bool noprefix); 157 | ~UndistortKB(); 158 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; 159 | 160 | }; 161 | 162 | } 163 | 164 | -------------------------------------------------------------------------------- /src/dso/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 | 25 | 26 | #include "util/globalCalib.h" 27 | #include "stdio.h" 28 | #include 29 | 30 | namespace dso 31 | { 32 | int wG[PYR_LEVELS], hG[PYR_LEVELS]; 33 | float fxG[PYR_LEVELS], fyG[PYR_LEVELS], 34 | cxG[PYR_LEVELS], cyG[PYR_LEVELS]; 35 | 36 | float fxiG[PYR_LEVELS], fyiG[PYR_LEVELS], 37 | cxiG[PYR_LEVELS], cyiG[PYR_LEVELS]; 38 | 39 | Eigen::Matrix3f KG[PYR_LEVELS], KiG[PYR_LEVELS]; 40 | 41 | 42 | float wM3G; 43 | float hM3G; 44 | 45 | void setGlobalCalib(int w, int h,const Eigen::Matrix3f &K) 46 | { 47 | int wlvl=w; 48 | int hlvl=h; 49 | pyrLevelsUsed=1; 50 | while(wlvl%2==0 && hlvl%2==0 && wlvl*hlvl > 5000 && pyrLevelsUsed < PYR_LEVELS) 51 | { 52 | wlvl /=2; 53 | hlvl /=2; 54 | pyrLevelsUsed++; 55 | } 56 | printf("using pyramid levels 0 to %d. coarsest resolution: %d x %d!\n", 57 | pyrLevelsUsed-1, wlvl, hlvl); 58 | if(wlvl>100 && hlvl > 100) 59 | { 60 | printf("\n\n===============WARNING!===================\n " 61 | "using not enough pyramid levels.\n" 62 | "Consider scaling to a resolution that is a multiple of a power of 2.\n"); 63 | } 64 | if(pyrLevelsUsed < 3) 65 | { 66 | printf("\n\n===============WARNING!===================\n " 67 | "I need higher resolution.\n" 68 | "I will probably segfault.\n"); 69 | } 70 | 71 | wM3G = w-3; 72 | hM3G = h-3; 73 | 74 | wG[0] = w; 75 | hG[0] = h; 76 | KG[0] = K; 77 | fxG[0] = K(0,0); 78 | fyG[0] = K(1,1); 79 | cxG[0] = K(0,2); 80 | cyG[0] = K(1,2); 81 | KiG[0] = KG[0].inverse(); 82 | fxiG[0] = KiG[0](0,0); 83 | fyiG[0] = KiG[0](1,1); 84 | cxiG[0] = KiG[0](0,2); 85 | cyiG[0] = KiG[0](1,2); 86 | 87 | for (int level = 1; level < pyrLevelsUsed; ++ level) 88 | { 89 | wG[level] = w >> level; 90 | hG[level] = h >> level; 91 | 92 | fxG[level] = fxG[level-1] * 0.5; 93 | fyG[level] = fyG[level-1] * 0.5; 94 | cxG[level] = (cxG[0] + 0.5) / ((int)1<, 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 | 25 | 26 | #pragma once 27 | #include "util/settings.h" 28 | #include "util/NumType.h" 29 | 30 | namespace dso 31 | { 32 | extern int wG[PYR_LEVELS], hG[PYR_LEVELS]; 33 | extern float fxG[PYR_LEVELS], fyG[PYR_LEVELS], 34 | cxG[PYR_LEVELS], cyG[PYR_LEVELS]; 35 | 36 | extern float fxiG[PYR_LEVELS], fyiG[PYR_LEVELS], 37 | cxiG[PYR_LEVELS], cyiG[PYR_LEVELS]; 38 | 39 | extern Eigen::Matrix3f KG[PYR_LEVELS],KiG[PYR_LEVELS]; 40 | 41 | extern float wM3G; 42 | extern float hM3G; 43 | 44 | void setGlobalCalib(int w, int h, const Eigen::Matrix3f &K ); 45 | } 46 | -------------------------------------------------------------------------------- /src/live/DatasetSaver.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #include "DatasetSaver.h" 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | dmvio::DatasetSaver::DatasetSaver(std::string saveFolder) 31 | { 32 | // Throw exception if folder exists! 33 | if(boost::filesystem::exists(saveFolder)) 34 | { 35 | throw boost::filesystem::filesystem_error("Folder already exists.", boost::system::error_code()); 36 | } 37 | boost::filesystem::create_directory(saveFolder); 38 | boost::filesystem::path savePath(saveFolder); 39 | boost::filesystem::create_directory(savePath / "cam0"); 40 | 41 | imgSaveFolder = (savePath / "cam0").string(); 42 | 43 | timesFile.open((savePath / "times.txt").string()); 44 | imuFile.open((savePath / "imu_orig.txt").string()); 45 | 46 | imageSaveThread = std::thread{&DatasetSaver::saveImagesWorker, this}; 47 | 48 | } 49 | 50 | void dmvio::DatasetSaver::saveImagesWorker() 51 | { 52 | while(running) 53 | { 54 | std::tuple tuple; 55 | { 56 | std::unique_lock lock(mutex); 57 | while(imageQueue.size() == 0) 58 | { 59 | if(!running) return; 60 | frameArrivedCond.wait(lock); 61 | } 62 | 63 | tuple = std::move(imageQueue.front()); 64 | imageQueue.pop_front(); 65 | 66 | if(imageQueue.size() > 1) 67 | { 68 | std::cout << "Save image queue size: " << imageQueue.size() << std::endl; 69 | } 70 | } 71 | 72 | double timestamp = std::get<1>(tuple); 73 | long long id = static_cast(timestamp * 1e9); 74 | 75 | std::stringstream filename; 76 | filename << imgSaveFolder << "/" << id << ".jpg"; 77 | 78 | timesFile << id << " " << std::fixed << timestamp << " " << std::get<2>(tuple) << "\n"; 79 | 80 | std::vector compression_params = {cv::IMWRITE_JPEG_QUALITY, 99}; // jpg quality. 81 | cv::imwrite(filename.str(), std::get<0>(tuple), compression_params); 82 | } 83 | } 84 | 85 | void dmvio::DatasetSaver::addImage(cv::Mat mat, double timestamp, double exposure) 86 | { 87 | { 88 | std::unique_lock lock(mutex); 89 | imageQueue.emplace_back(mat, timestamp, exposure); 90 | } 91 | frameArrivedCond.notify_all(); 92 | } 93 | 94 | void dmvio::DatasetSaver::addIMUData(double timestamp, std::vector accData, std::vector gyrData) 95 | { 96 | imuFile << static_cast(timestamp * 1e9); 97 | for(int i = 0; i < 3; ++i) 98 | { 99 | imuFile << " " << gyrData[i]; 100 | } 101 | for(int i = 0; i < 3; ++i) 102 | { 103 | imuFile << " " << accData[i]; 104 | } 105 | imuFile << "\n"; 106 | } 107 | 108 | void dmvio::DatasetSaver::end() 109 | { 110 | running = false; 111 | frameArrivedCond.notify_all(); 112 | imageSaveThread.join(); 113 | } 114 | 115 | -------------------------------------------------------------------------------- /src/live/DatasetSaver.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #ifndef DMVIO_DATASETSAVER_H 24 | #define DMVIO_DATASETSAVER_H 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | namespace dmvio 35 | { 36 | 37 | // Helper for recording live data to file. 38 | class DatasetSaver 39 | { 40 | public: 41 | DatasetSaver(std::string saveFolder); 42 | 43 | // timestamp in seconds, exposure in milliseconds. 44 | void addImage(cv::Mat mat, double timestamp, double exposure); 45 | 46 | void addIMUData(double timestamp, std::vector accData, std::vector gyrData); 47 | 48 | void saveImagesWorker(); 49 | 50 | void end(); 51 | 52 | private: 53 | std::string imgSaveFolder; 54 | 55 | std::ofstream timesFile, imuFile; 56 | 57 | std::thread imageSaveThread; 58 | 59 | // protects image queue. 60 | std::mutex mutex; 61 | std::condition_variable frameArrivedCond; 62 | std::deque> imageQueue; 63 | 64 | bool running = true; 65 | 66 | }; 67 | 68 | 69 | } 70 | #endif //DMVIO_DATASETSAVER_H 71 | -------------------------------------------------------------------------------- /src/live/FrameContainer.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #include 24 | #include 25 | #include "FrameContainer.h" 26 | #include "IMUInterpolator.h" 27 | #include "IMU/IMUTypes.h" 28 | 29 | std::pair, dmvio::IMUData> 30 | dmvio::FrameContainer::getImageAndIMUData(int maxSkipFrames) 31 | { 32 | std::unique_lock lock(framesMutex); 33 | while(frames.size() == 0 && !stopSystem) // Wait for new image. 34 | { 35 | frameArrivedCond.wait(lock); 36 | } 37 | 38 | if(stopSystem) return std::make_pair(nullptr, dmvio::IMUData{}); 39 | 40 | IMUData imuData; 41 | 42 | // Skip frames if necessary. 43 | // Now frames.size() must be greater than 0. 44 | size_t useFrame = frames.size() - 1; 45 | int numFramesAfter = 0; 46 | if(frames.size() > 1) 47 | { 48 | int framesToSkip = frames.size() - 1; // also the index of the frame that will be used. 49 | if(maxSkipFrames >= 0 && maxSkipFrames < framesToSkip) 50 | { 51 | framesToSkip = maxSkipFrames; 52 | } 53 | useFrame = framesToSkip; 54 | numFramesAfter = frames.size() - useFrame - 1; 55 | if(!dso::setting_debugout_runquiet) 56 | { 57 | std::cout << "SKIPPING " << framesToSkip << " FRAMES!" << " frames remaining in queue: " 58 | << numFramesAfter << std::endl; 59 | } 60 | } 61 | 62 | auto returnImg = std::move(frames[useFrame].img); 63 | 64 | // Fill IMU data to return, also consider IMU data for skipped frames. 65 | for(int j = 0; j <= useFrame; ++j) 66 | { 67 | std::vector& data = frames[j].imuData; 68 | for(int i = 0; i < data.size(); ++i) 69 | { 70 | Eigen::Vector3d acc, gyr; 71 | for(int x = 0; x < 3; ++x) 72 | { 73 | acc(x) = data[i].accData[x]; 74 | gyr(x) = data[i].gyrData[x]; 75 | } 76 | 77 | double integrationTime = 0.0; 78 | 79 | integrationTime = data[i].timestamp - prevTimestamp; 80 | assert(integrationTime >= 0); 81 | 82 | imuData.emplace_back(acc, gyr, integrationTime); 83 | 84 | prevTimestamp = data[i].timestamp; 85 | } 86 | } 87 | if(prevTimestamp < 0.0) 88 | { 89 | prevTimestamp = frames[useFrame].imgTimestamp; 90 | } 91 | assert(std::abs(frames[useFrame].imgTimestamp - prevTimestamp) < 0.0001); 92 | frames.erase(frames.begin(), frames.begin() + useFrame + 1); 93 | assert(frames.size() == numFramesAfter); 94 | 95 | return std::make_pair(std::move(returnImg), imuData); 96 | } 97 | 98 | void dmvio::FrameContainer::addFrame(Frame frame) 99 | { 100 | { 101 | std::unique_lock lock(framesMutex); 102 | frames.push_back(std::move(frame)); 103 | } 104 | frameArrivedCond.notify_all(); 105 | } 106 | 107 | int dmvio::FrameContainer::getQueueSize() 108 | { 109 | std::unique_lock lock(framesMutex); 110 | return frames.size(); 111 | } 112 | 113 | void dmvio::FrameContainer::stop() 114 | { 115 | stopSystem = true; 116 | frameArrivedCond.notify_all(); 117 | } 118 | 119 | dmvio::Frame::Frame(std::unique_ptr&& img, double imgTimestamp) : img(std::move(img)), 120 | imgTimestamp(imgTimestamp) 121 | {} 122 | -------------------------------------------------------------------------------- /src/live/FrameContainer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #ifndef DMVIO_FRAMECONTAINER_H 24 | #define DMVIO_FRAMECONTAINER_H 25 | 26 | #include 27 | #include "util/ImageAndExposure.h" 28 | #include "IMU/IMUTypes.h" 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | namespace dmvio 35 | { 36 | // Data structure for IMU data during interpolation (see also IMUInterpolator.h). 37 | class IMUDataDuringInterpolation 38 | { 39 | public: 40 | IMUDataDuringInterpolation(double timestamp); 41 | 42 | bool operator<(const IMUDataDuringInterpolation& other) const; 43 | 44 | enum SaveStatus 45 | { 46 | DONT_SAVE, SHALL_SAVE, SAVED 47 | }; 48 | 49 | std::vector accData; // Accelerometer data at a specific timestamp (usually 3 elements for x,y,z). 50 | std::vector gyrData; // Gyroscope data at a specific timestamp (usually 3 elements for x,y,z). 51 | double timestamp; // Timestamp of this IMU data sample. 52 | bool gyrSet; // True if gyroscope data has been set. 53 | bool accSet; // True if accelerometer data has been set. 54 | SaveStatus saveStatus = DONT_SAVE; // only relevant for saving IMU data to file while running. 55 | }; 56 | 57 | // Image frame and corresponding IMU imu data for storage in the FrameContainer. 58 | class Frame 59 | { 60 | public: 61 | Frame() = default; 62 | Frame(std::unique_ptr&& img, double imgTimestamp); 63 | std::unique_ptr img; 64 | std::vector imuData; 65 | double imgTimestamp = 0.0; 66 | }; 67 | 68 | // Used to store (addFrame) and retrieve (getImageAndIMUData) images and corresponding IMU data asynchronously. 69 | // Also contains logic to skip frames if necessary. 70 | class FrameContainer 71 | { 72 | public: 73 | FrameContainer() = default; 74 | 75 | // Retrieve the newest image and corresponding IMU data. 76 | // Will wait until a new image arrives if no image is in the queue. 77 | // If there is more than one image in the queue it will skip maxSkipFrames (if maxSkipFrames is >= 0). 78 | // If maxSkipFrames == -1 it will always skip to the newest image. 79 | std::pair, IMUData> getImageAndIMUData(int maxSkipFrames = -1); 80 | 81 | // Returns the number of images in the queue. 82 | int getQueueSize(); 83 | 84 | // Adds a new image and corresponding IMU data to the queue. Can be called in a different thread than the calls 85 | // to getImageAndIMUData. 86 | void addFrame(Frame frame); 87 | 88 | // Can be used to stop a call to getImageAndIMUData and return an empty image. 89 | void stop(); 90 | 91 | private: 92 | std::mutex framesMutex; // Protects the frames array. 93 | std::condition_variable frameArrivedCond; 94 | 95 | std::deque frames; 96 | 97 | double prevTimestamp = -1.0; // timestamp of last measurement. 98 | 99 | bool stopSystem = false; 100 | }; 101 | } 102 | 103 | 104 | #endif //DMVIO_FRAMECONTAINER_H 105 | -------------------------------------------------------------------------------- /src/live/FrameSkippingStrategy.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #include "FrameSkippingStrategy.h" 24 | 25 | void dmvio::FrameSkippingSettings::registerArgs(dmvio::SettingsUtil& set) 26 | { 27 | set.registerArg("maxSkipFramesVisualInit", maxSkipFramesVisualInit); 28 | set.registerArg("maxSkipFramesVisualOnlyMode", maxSkipFramesVisualOnlyMode); 29 | set.registerArg("maxSkipFramesVisualInertial", maxSkipFramesVisualInertial); 30 | set.registerArg("maxSkipFramesFullReset", maxSkipFramesFullReset); 31 | set.registerArg("skipFramesVisualOnlyDelay", skipFramesVisualOnlyDelay); 32 | set.registerArg("minQueueSizeForSkipping", minQueueSizeForSkipping); 33 | } 34 | 35 | dmvio::FrameSkippingStrategy::FrameSkippingStrategy(dmvio::FrameSkippingSettings settings) 36 | : settings(std::move(settings)) 37 | {} 38 | 39 | int dmvio::FrameSkippingStrategy::getMaxSkipFrames(int queueSize) 40 | { 41 | std::unique_lock lock(mutex); 42 | if(resetLast) 43 | { 44 | // After full reset skip all frames. 45 | resetLast = false; 46 | return settings.maxSkipFramesFullReset; 47 | } 48 | if(queueSize < settings.minQueueSizeForSkipping) 49 | { 50 | return 0; 51 | } 52 | switch(lastStatus) 53 | { 54 | case VISUAL_INIT: 55 | return settings.maxSkipFramesVisualInit; 56 | case VISUAL_ONLY: 57 | if(visualOnlyDelay > 0) 58 | { 59 | visualOnlyDelay--; 60 | return settings.maxSkipFramesVisualInit; 61 | } 62 | return settings.maxSkipFramesVisualOnlyMode; 63 | case VISUAL_INERTIAL: 64 | return settings.maxSkipFramesVisualInertial; 65 | } 66 | } 67 | 68 | void dmvio::FrameSkippingStrategy::publishSystemStatus(dmvio::SystemStatus systemStatus) 69 | { 70 | std::unique_lock lock(mutex); 71 | if(lastStatus == VISUAL_INIT && systemStatus == VISUAL_ONLY) 72 | { 73 | visualOnlyDelay = settings.skipFramesVisualOnlyDelay; 74 | } 75 | lastStatus = systemStatus; 76 | } 77 | 78 | void dmvio::FrameSkippingStrategy::reset() 79 | { 80 | std::unique_lock lock(mutex); 81 | resetLast = true; 82 | } 83 | 84 | -------------------------------------------------------------------------------- /src/live/FrameSkippingStrategy.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #ifndef DMVIO_FRAMESKIPPINGSTRATEGY_H 24 | #define DMVIO_FRAMESKIPPINGSTRATEGY_H 25 | 26 | #include "dso/IOWrapper/Output3DWrapper.h" 27 | #include "util/SettingsUtil.h" 28 | #include 29 | 30 | namespace dmvio 31 | { 32 | 33 | // Settings for frame skipping. 34 | class FrameSkippingSettings 35 | { 36 | public: 37 | void registerArgs(dmvio::SettingsUtil& set); 38 | 39 | // Maximum frames to skip ... 40 | int maxSkipFramesVisualInit = 0; // ... during visual initializer phase. 41 | int maxSkipFramesVisualOnlyMode = 1; // ... during visual only mode. 42 | int maxSkipFramesVisualInertial = 2; // ... during visual-inertial mode. 43 | int maxSkipFramesFullReset = -1; // ... when a full reset happens. 44 | // -1 means that all frames until the newest one are skipped (resembling a value of infinity). 45 | 46 | // After visual initializer wait this amount if frames before switching to the visualOnly threshold. 47 | int skipFramesVisualOnlyDelay = 30; 48 | 49 | // Don't skip if the queue size is < this value. 50 | int minQueueSizeForSkipping = 2; 51 | }; 52 | 53 | // Contains the logic to decide how many frames to skip (depending on the current state of the system). 54 | class FrameSkippingStrategy : public dso::IOWrap::Output3DWrapper 55 | { 56 | public: 57 | FrameSkippingStrategy(FrameSkippingSettings settings); 58 | 59 | // Get current maxSkipFrames according to strategy. 60 | // queueSize is the number of frames currently in the image queue. 61 | int getMaxSkipFrames(int queueSize); 62 | 63 | // Overidden from Output3DWrapper 64 | void publishSystemStatus(dmvio::SystemStatus systemStatus) override; 65 | void reset() override; 66 | 67 | 68 | private: 69 | FrameSkippingSettings settings; 70 | 71 | std::mutex mutex; 72 | SystemStatus lastStatus; 73 | 74 | bool resetLast = false; 75 | 76 | int visualOnlyDelay = 0; 77 | }; 78 | 79 | 80 | } 81 | 82 | #endif //DMVIO_FRAMESKIPPINGSTRATEGY_H 83 | -------------------------------------------------------------------------------- /src/live/RealsenseT265.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #ifndef DMVIO_REALSENSET265_H 24 | #define DMVIO_REALSENSET265_H 25 | 26 | #include 27 | #include "IMU/IMUSettings.h" 28 | #include "live/IMUInterpolator.h" 29 | #include "FrameContainer.h" 30 | #include "util/Undistort.h" 31 | #include "DatasetSaver.h" 32 | 33 | namespace dmvio 34 | { 35 | // Class for interacting with the RealsenseT265 camera. 36 | class RealsenseT265 37 | { 38 | public: 39 | // Images and IMU data will be passed into frameContainer which can be used to get synchronized image and IMU data. 40 | // Factory calibration will be saved to cameraCalibSavePath. 41 | // If datasetSaver is set, the IMU data and images will also be saved to file. 42 | RealsenseT265(FrameContainer& frameContainer, std::string cameraCalibSavePath, DatasetSaver* datasetSaver); 43 | 44 | // Start receiving data. 45 | void start(); 46 | 47 | // Set the undistorter to use. Until this is set, no images are passed forward to the frameContainer. 48 | void setUndistorter(dso::Undistort* undistort); 49 | 50 | std::unique_ptr imuCalibration; 51 | private: 52 | void readCalibration(); 53 | std::string cameraCalibSavePath; 54 | 55 | rs2::context context; 56 | rs2::config config; 57 | rs2::pipeline pipe; 58 | 59 | rs2::pipeline_profile profile; 60 | 61 | // Use left camera. 62 | int useCam = 0; 63 | 64 | rs2_motion_device_intrinsic gyroIntrinsics, accelIntrinsics; 65 | 66 | std::atomic calibrationRead{false}; 67 | 68 | // IMU interpolator will take care of creating "fake measurements" to synchronize the sensors by interpolating IMU data. 69 | IMUInterpolator imuInt; 70 | dso::Undistort* undistorter = nullptr; 71 | double lastImgTimestamp = -1.0; 72 | 73 | DatasetSaver* saver; 74 | }; 75 | 76 | } 77 | 78 | #endif //DMVIO_REALSENSET265_H 79 | -------------------------------------------------------------------------------- /src/util/GTData.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #ifndef GTData_hpp 24 | #define GTData_hpp 25 | 26 | #include 27 | 28 | #include 29 | 30 | #include 31 | #include 32 | 33 | namespace dmvio 34 | { 35 | class GTData 36 | { 37 | public: 38 | 39 | inline GTData() 40 | {} 41 | 42 | inline GTData(Sophus::SE3 pose, Eigen::Vector3d velocity, Eigen::Vector3d biasRotation, 43 | Eigen::Vector3d biasTranslation) 44 | : pose(pose), velocity(velocity), biasRotation(biasRotation), biasTranslation(biasTranslation) 45 | {} 46 | 47 | 48 | Sophus::SE3 pose; 49 | Eigen::Vector3d velocity; // Note: velocities might be in the vicon frame instead of the world frame... 50 | Eigen::Vector3d biasRotation; 51 | Eigen::Vector3d biasTranslation; 52 | }; 53 | } 54 | 55 | #endif /* GTData_hpp */ 56 | -------------------------------------------------------------------------------- /src/util/MainSettings.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | // This file contains common utils used for the main files. 24 | 25 | #ifndef DMVIO_MAINSETTINGS_H 26 | #define DMVIO_MAINSETTINGS_H 27 | 28 | #include 29 | 30 | namespace dmvio 31 | { 32 | 33 | // Parses the main commandline arguments needed for DM-VIO and forwards all other arguments to the SettingsUtil. 34 | class MainSettings 35 | { 36 | public: 37 | // Parse all commandline arguments. Unknown arguments will be forwarded to settingsUtil. 38 | void parseArguments(int argc, char** argv, SettingsUtil& settingsUtil); 39 | 40 | // Parse a single argument. Unknown arguments will be forwarded to settingsUtil. 41 | void parseArgument(char* arg, dmvio::SettingsUtil& settingsUtil); 42 | 43 | // Register args for these settings and for global DSO settings. 44 | void registerArgs(dmvio::SettingsUtil& set); 45 | 46 | std::string vignette = ""; 47 | std::string gammaCalib = ""; 48 | std::string calib = ""; 49 | std::string imuCalibFile = ""; 50 | 51 | // only relevant for datasets. 52 | float playbackSpeed = 0; // 0 for linearize (play as fast as possible, while sequentializing tracking & mapping). otherwise, factor on timestamps. 53 | bool preload = false; 54 | 55 | // 0 means photometric calibration (exposure times, vignette and response calibration) is available, 1 means no 56 | // photometric calibration there. 57 | // Note that the vignette will only be used if set to 0. 58 | int mode = 0; 59 | 60 | void settingsDefault(int preset); 61 | }; 62 | 63 | } 64 | 65 | 66 | #endif //DMVIO_MAINSETTINGS_H 67 | -------------------------------------------------------------------------------- /src/util/SettingsUtil.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #include "SettingsUtil.h" 24 | 25 | dmvio::SettingsUtil::Parameter::Parameter(void* pointer, 26 | const std::function& commandLineHandler, 27 | const std::function& yamlHandler, 28 | const std::function& printHandler) : pointer( 29 | pointer), commandLineHandler(commandLineHandler), yamlHandler(yamlHandler), printHandler(printHandler) 30 | {} 31 | 32 | void dmvio::SettingsUtil::tryReadFromYaml(const YAML::Node& node) 33 | { 34 | // Loop through parameters and check if their name is in the node. 35 | for(auto& pair : parameters) 36 | { 37 | // We give preference to commandline over yaml. 38 | if(!pair.second.loadedFromCommandLine) 39 | { 40 | std::string name = pair.first; 41 | if(node[name]) 42 | { 43 | pair.second.yamlHandler(pair.second.pointer, node[name]); 44 | } 45 | } 46 | } 47 | } 48 | 49 | bool dmvio::SettingsUtil::tryReadFromCommandLine(const std::string& arg) 50 | { 51 | // Extract name as part before the = sign and lookup in map 52 | auto pos = arg.find('='); 53 | if(pos == std::string::npos) 54 | { 55 | return false; 56 | } 57 | std::string name = arg.substr(0, pos); 58 | 59 | auto it = parameters.find(name); 60 | if(it == parameters.end()) 61 | { 62 | return false; 63 | } 64 | 65 | it->second.commandLineHandler(it->second.pointer, arg.substr(pos + 1)); 66 | it->second.loadedFromCommandLine = true; 67 | 68 | return true; 69 | } 70 | 71 | void dmvio::SettingsUtil::printAllSettings(std::ostream& stream) 72 | { 73 | for(auto& pair : parameters) 74 | { 75 | stream << pair.first << ": "; 76 | pair.second.printHandler(pair.second.pointer, stream); 77 | stream << '\n'; 78 | } 79 | } 80 | 81 | void dmvio::SettingsUtil::createPangolinSettings() 82 | { 83 | for(auto&& param : parameters) 84 | { 85 | auto* set = param.second.pangolinSetting.get(); 86 | if(set) 87 | { 88 | set->createVar(); 89 | } 90 | } 91 | } 92 | 93 | void dmvio::SettingsUtil::updatePangolinSettings() 94 | { 95 | for(auto&& param : parameters) 96 | { 97 | auto* set = param.second.pangolinSetting.get(); 98 | if(set) 99 | { 100 | set->updateVar(); 101 | } 102 | } 103 | } 104 | -------------------------------------------------------------------------------- /src/util/TimeMeasurement.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #include "TimeMeasurement.h" 24 | #include 25 | 26 | using namespace dmvio; 27 | using namespace std::chrono; 28 | 29 | 30 | std::map dmvio::TimeMeasurement::logs = std::map(); 31 | bool dmvio::TimeMeasurement::saveFileOpen = false; 32 | 33 | dmvio::TimeMeasurement::TimeMeasurement(std::string name) 34 | : name(name) 35 | { 36 | begin = high_resolution_clock::now(); 37 | } 38 | 39 | dmvio::TimeMeasurement::~TimeMeasurement() 40 | { 41 | end(); 42 | } 43 | 44 | double dmvio::TimeMeasurement::end() 45 | { 46 | if(ended) 47 | { 48 | return -1; 49 | } 50 | 51 | auto end = high_resolution_clock::now(); 52 | double duration = duration_cast>(end - begin).count(); 53 | 54 | logs[name].addMeasurement(duration); 55 | 56 | ended = true; 57 | 58 | return duration; 59 | } 60 | 61 | void dmvio::TimeMeasurement::saveResults(std::string filename) 62 | { 63 | std::ofstream saveFile; 64 | saveFile.open(filename); 65 | 66 | for(const auto& pair : logs) 67 | { 68 | saveFile << pair.first << ' ' << pair.second << '\n'; 69 | } 70 | saveFile.close(); 71 | } 72 | 73 | void dmvio::TimeMeasurement::cancel() 74 | { 75 | ended = true; 76 | } 77 | 78 | void dmvio::MeasurementLog::addMeasurement(double time) 79 | { 80 | if(num == 0) 81 | { 82 | first = time; 83 | } 84 | sum += time; 85 | num++; 86 | 87 | double shifted = time - first; 88 | sumShifted += shifted; 89 | sumSquared += shifted * shifted; 90 | 91 | if(time > max) 92 | { 93 | max = time; 94 | } 95 | } 96 | 97 | void dmvio::MeasurementLog::writeLogLine(std::ostream& stream) const 98 | { 99 | double mean = getMean(); 100 | double variance = getVariance(); 101 | 102 | stream << mean << ' ' << variance << ' ' << max << ' ' << num; 103 | 104 | } 105 | 106 | double dmvio::MeasurementLog::getVariance() const 107 | { 108 | double variance = (sumSquared - (sumShifted * sumShifted) / num) / (num - 1); 109 | return variance; 110 | } 111 | 112 | double dmvio::MeasurementLog::getMean() const 113 | { 114 | double mean = sum / (double) num; 115 | return mean; 116 | } 117 | 118 | double dmvio::MeasurementLog::getMax() const 119 | { 120 | return max; 121 | } 122 | 123 | int dmvio::MeasurementLog::getNum() const 124 | { 125 | return num; 126 | } 127 | 128 | std::ostream& operator<<(std::ostream& os, const dmvio::MeasurementLog& obj) 129 | { 130 | obj.writeLogLine(os); 131 | return os; 132 | } 133 | -------------------------------------------------------------------------------- /src/util/TimeMeasurement.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DM-VIO. 3 | * 4 | * Copyright (c) 2022 Lukas von Stumberg . 5 | * for more information see . 6 | * If you use this code, please cite the respective publications as 7 | * listed on the above website. 8 | * 9 | * DM-VIO is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * DM-VIO is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with DM-VIO. If not, see . 21 | */ 22 | 23 | #ifndef DMVIO_TIMEMEASUREMENT_H 24 | #define DMVIO_TIMEMEASUREMENT_H 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | 33 | namespace dmvio 34 | { 35 | // Saves mean, maximum, and variance. 36 | class MeasurementLog 37 | { 38 | public: 39 | MeasurementLog() = default; 40 | 41 | void addMeasurement(double time); 42 | 43 | void writeLogLine(std::ostream& stream) const; 44 | 45 | int getNum() const; 46 | double getMax() const; 47 | double getMean() const; 48 | double getVariance() const; 49 | private: 50 | double sum{0}; 51 | double max{0}; 52 | int num{0}; 53 | 54 | double first{-1}; 55 | // For computing stddev we shift by the first sample. 56 | double sumShifted{0}; 57 | double sumSquared{0}; 58 | 59 | }; 60 | 61 | // Used to measure and log wall time for different code parts. 62 | // Note that this class is only partially thread-safe, meaning there should not be measurements with the same name 63 | // in different threads, otherwise there can be an endless loop / segfault in the first call! 64 | class TimeMeasurement final 65 | { 66 | public: 67 | TimeMeasurement(std::string name); 68 | TimeMeasurement(const TimeMeasurement&) = delete; 69 | ~TimeMeasurement(); 70 | 71 | // End the measurement interval. Optional, if not called the destructor will call it. 72 | double end(); 73 | 74 | // Cancel the time measurement. 75 | void cancel(); 76 | 77 | static void saveResults(std::string filename); 78 | 79 | private: 80 | static bool saveFileOpen; 81 | static std::ofstream saveFile; 82 | static std::map logs; 83 | 84 | std::string name; 85 | std::chrono::high_resolution_clock::time_point begin; 86 | bool ended{false}; 87 | }; 88 | } 89 | 90 | std::ostream& operator<<(std::ostream& os, const dmvio::MeasurementLog& obj); 91 | 92 | 93 | #endif //DMVIO_TIMEMEASUREMENT_H 94 | -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(Tests) 2 | 3 | if(EXISTS "${CMAKE_CURRENT_LIST_DIR}/googletest/CMakeLists.txt") 4 | add_subdirectory(googletest) 5 | include_directories(${gtest_SOURCE_DIR}/include ${gtest_SOURCE_DIR}) 6 | 7 | add_executable(Google_Tests_run test_PoseTransformationFactor.cpp test_IMUInterpolator.cpp) 8 | target_link_libraries(Google_Tests_run gtest gtest_main dmvio ${DMVIO_LINKED_LIBRARIES}) 9 | endif() -------------------------------------------------------------------------------- /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_se3.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 "se3.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 SE3Group SE3Type; 37 | typedef typename SE3Group::Point Point; 38 | typedef typename SE3Group::Tangent Tangent; 39 | 40 | vector se3_vec; 41 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)), 42 | Point(0,0,0))); 43 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.2, 0.5, -1.0)), 44 | Point(10,0,0))); 45 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.)), 46 | Point(0,100,5))); 47 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), 48 | Point(0,0,0))); 49 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), 50 | Point(0,-0.00000001,0.0000000001))); 51 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), 52 | Point(0.01,0,0))); 53 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(M_PI, 0, 0)), 54 | Point(4,-5,0))); 55 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)), 56 | Point(0,0,0)) 57 | *SE3Type(SO3Type::exp(Point(M_PI, 0, 0)), 58 | Point(0,0,0)) 59 | *SE3Type(SO3Type::exp(Point(-0.2, -0.5, -0.0)), 60 | Point(0,0,0))); 61 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.3, 0.5, 0.1)), 62 | Point(2,0,-7)) 63 | *SE3Type(SO3Type::exp(Point(M_PI, 0, 0)), 64 | Point(0,0,0)) 65 | *SE3Type(SO3Type::exp(Point(-0.3, -0.5, -0.1)), 66 | Point(0,6,0))); 67 | vector tangent_vec; 68 | Tangent tmp; 69 | tmp << 0,0,0,0,0,0; 70 | tangent_vec.push_back(tmp); 71 | tmp << 1,0,0,0,0,0; 72 | tangent_vec.push_back(tmp); 73 | tmp << 0,1,0,1,0,0; 74 | tangent_vec.push_back(tmp); 75 | tmp << 0,-5,10,0,0,0; 76 | tangent_vec.push_back(tmp); 77 | tmp << -1,1,0,0,0,1; 78 | tangent_vec.push_back(tmp); 79 | tmp << 20,-1,0,-1,1,0; 80 | tangent_vec.push_back(tmp); 81 | tmp << 30,5,-1,20,-1,0; 82 | tangent_vec.push_back(tmp); 83 | 84 | vector point_vec; 85 | point_vec.push_back(Point(1,2,4)); 86 | 87 | Tests tests; 88 | tests.setGroupElements(se3_vec); 89 | tests.setTangentVectors(tangent_vec); 90 | tests.setPoints(point_vec); 91 | 92 | tests.runAllTests(); 93 | cerr << "passed." << endl << endl; 94 | } 95 | 96 | int main() { 97 | cerr << "Test SE3" << endl << endl; 98 | 99 | cerr << "Double tests: " << endl; 100 | tests(); 101 | 102 | cerr << "Float tests: " << endl; 103 | tests(); 104 | return 0; 105 | } 106 | -------------------------------------------------------------------------------- /thirdparty/Sophus/sophus/test_sim3.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 | 28 | #include "sim3.hpp" 29 | #include "tests.hpp" 30 | 31 | using namespace Sophus; 32 | using namespace std; 33 | 34 | template 35 | void tests() { 36 | 37 | typedef Sim3Group Sim3Type; 38 | typedef RxSO3Group RxSO3Type; 39 | typedef typename Sim3Group::Point Point; 40 | typedef typename Sim3Group::Tangent Tangent; 41 | typedef Matrix Vector4Type; 42 | 43 | vector sim3_vec; 44 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.2, 0.5, 0.0,1.)), 45 | Point(0,0,0))); 46 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.2, 0.5, -1.0,1.1)), 47 | Point(10,0,0))); 48 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0., 0., 0.,1.1)), 49 | Point(0,10,5))); 50 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0., 0., 0.00001, 0.)), 51 | Point(0,0,0))); 52 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp( 53 | Vector4Type(0., 0., 0.00001, 0.0000001)), 54 | Point(1,-1.00000001,2.0000000001))); 55 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0., 0., 0.00001, 0)), 56 | Point(0.01,0,0))); 57 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(M_PI, 0, 0,0.9)), 58 | Point(4,-5,0))); 59 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.2, 0.5, 0.0,0)), 60 | Point(0,0,0)) 61 | *Sim3Type(RxSO3Type::exp(Vector4Type(M_PI, 0, 0,0)), 62 | Point(0,0,0)) 63 | *Sim3Type(RxSO3Type::exp(Vector4Type(-0.2, -0.5, -0.0,0)), 64 | Point(0,0,0))); 65 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.3, 0.5, 0.1,0)), 66 | Point(2,0,-7)) 67 | *Sim3Type(RxSO3Type::exp(Vector4Type(M_PI, 0, 0,0)), 68 | Point(0,0,0)) 69 | *Sim3Type(RxSO3Type::exp(Vector4Type(-0.3, -0.5, -0.1,0)), 70 | Point(0,6,0))); 71 | vector tangent_vec; 72 | Tangent tmp; 73 | tmp << 0,0,0,0,0,0,0; 74 | tangent_vec.push_back(tmp); 75 | tmp << 1,0,0,0,0,0,0; 76 | tangent_vec.push_back(tmp); 77 | tmp << 0,1,0,1,0,0,0.1; 78 | tangent_vec.push_back(tmp); 79 | tmp << 0,0,1,0,1,0,0.1; 80 | tangent_vec.push_back(tmp); 81 | tmp << -1,1,0,0,0,1,-0.1; 82 | tangent_vec.push_back(tmp); 83 | tmp << 20,-1,0,-1,1,0,-0.1; 84 | tangent_vec.push_back(tmp); 85 | tmp << 30,5,-1,20,-1,0,1.5; 86 | tangent_vec.push_back(tmp); 87 | 88 | 89 | vector point_vec; 90 | point_vec.push_back(Point(1,2,4)); 91 | 92 | Tests tests; 93 | tests.setGroupElements(sim3_vec); 94 | tests.setTangentVectors(tangent_vec); 95 | tests.setPoints(point_vec); 96 | 97 | tests.runAllTests(); 98 | } 99 | 100 | int main() { 101 | cerr << "Test Sim3" << endl << endl; 102 | 103 | cerr << "Double tests: " << endl; 104 | tests(); 105 | 106 | cerr << "Float tests: " << endl; 107 | tests(); 108 | return 0; 109 | } 110 | -------------------------------------------------------------------------------- /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/libzip-1.1.1.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukasvst/dm-vio/e306328098e08facbec1bfd45d22e19c77a3ccad/thirdparty/libzip-1.1.1.tar.gz --------------------------------------------------------------------------------