├── CMakeLists.txt ├── README.md ├── calib └── euroc │ ├── IMU_info.txt │ ├── T_C0C1.txt │ ├── cam0.txt │ └── cam1.txt ├── cmake ├── FindEigen3.cmake ├── FindGlog.cmake ├── FindLibZip.cmake └── FindSuiteParse.cmake ├── pic ├── 2019-04-09-V203.png ├── MH01.png ├── MH02_1.png ├── MH03_1.png ├── MH04_1.png ├── MH05_1.png ├── V102_1.png ├── euroc_V2_03.png └── euroc_v1_03.png ├── run.bash ├── run_euroc.bash ├── run_mono.bash ├── src ├── FullSystem │ ├── CoarseInitializer.cpp │ ├── CoarseInitializer.h │ ├── CoarseTracker.cpp │ ├── CoarseTracker.h │ ├── FullSystem.cpp │ ├── FullSystem.h │ ├── FullSystemDebugStuff.cpp │ ├── FullSystemMarginalize.cpp │ ├── FullSystemOptPoint.cpp │ ├── FullSystemOptimize.cpp │ ├── HessianBlocks.cpp │ ├── HessianBlocks.h │ ├── IMUPreintegrator.cpp │ ├── IMUPreintegrator.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 │ │ ├── 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 ├── main_dso_pangolin.cpp └── 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 └── 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 /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET(PROJECT_NAME DSO) 2 | 3 | PROJECT(${PROJECT_NAME}) 4 | CMAKE_MINIMUM_REQUIRED(VERSION 2.6) 5 | #set(CMAKE_VERBOSE_MAKEFILE ON) 6 | 7 | 8 | set(BUILD_TYPE Release) 9 | #set(BUILD_TYPE RelWithDebInfo) 10 | 11 | set(EXECUTABLE_OUTPUT_PATH bin) 12 | set(LIBRARY_OUTPUT_PATH lib) 13 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 14 | 15 | # required libraries 16 | find_package(SuiteParse REQUIRED) 17 | find_package(Eigen3 REQUIRED) 18 | find_package(Boost COMPONENTS system thread) 19 | 20 | # optional libraries 21 | find_package(LibZip QUIET) 22 | find_package(Pangolin 0.2 QUIET) 23 | find_package(OpenCV QUIET) 24 | 25 | find_package(Glog REQUIRED) 26 | include_directories(${Glog_INCLUDE_DIRS}) 27 | 28 | # flags 29 | add_definitions("-DENABLE_SSE") 30 | set(CMAKE_CXX_FLAGS 31 | "${SSE_FLAGS} -O3 -g -std=c++0x -march=native" 32 | # "${SSE_FLAGS} -O3 -g -std=c++0x -fno-omit-frame-pointer" 33 | ) 34 | 35 | if (MSVC) 36 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /EHsc") 37 | endif (MSVC) 38 | 39 | # Sources files 40 | set(dso_SOURCE_FILES 41 | ${PROJECT_SOURCE_DIR}/src/FullSystem/FullSystem.cpp 42 | ${PROJECT_SOURCE_DIR}/src/FullSystem/FullSystemOptimize.cpp 43 | ${PROJECT_SOURCE_DIR}/src/FullSystem/FullSystemOptPoint.cpp 44 | ${PROJECT_SOURCE_DIR}/src/FullSystem/FullSystemDebugStuff.cpp 45 | ${PROJECT_SOURCE_DIR}/src/FullSystem/FullSystemMarginalize.cpp 46 | ${PROJECT_SOURCE_DIR}/src/FullSystem/Residuals.cpp 47 | ${PROJECT_SOURCE_DIR}/src/FullSystem/CoarseTracker.cpp 48 | ${PROJECT_SOURCE_DIR}/src/FullSystem/CoarseInitializer.cpp 49 | ${PROJECT_SOURCE_DIR}/src/FullSystem/ImmaturePoint.cpp 50 | ${PROJECT_SOURCE_DIR}/src/FullSystem/HessianBlocks.cpp 51 | ${PROJECT_SOURCE_DIR}/src/FullSystem/PixelSelector2.cpp 52 | ${PROJECT_SOURCE_DIR}/src/FullSystem/IMUPreintegrator.cpp 53 | ${PROJECT_SOURCE_DIR}/src/OptimizationBackend/EnergyFunctional.cpp 54 | ${PROJECT_SOURCE_DIR}/src/OptimizationBackend/AccumulatedTopHessian.cpp 55 | ${PROJECT_SOURCE_DIR}/src/OptimizationBackend/AccumulatedSCHessian.cpp 56 | ${PROJECT_SOURCE_DIR}/src/OptimizationBackend/EnergyFunctionalStructs.cpp 57 | ${PROJECT_SOURCE_DIR}/src/util/settings.cpp 58 | ${PROJECT_SOURCE_DIR}/src/util/Undistort.cpp 59 | ${PROJECT_SOURCE_DIR}/src/util/globalCalib.cpp 60 | ) 61 | 62 | 63 | 64 | include_directories( 65 | ${PROJECT_SOURCE_DIR}/src 66 | ${PROJECT_SOURCE_DIR}/thirdparty/Sophus 67 | ${PROJECT_SOURCE_DIR}/thirdparty/sse2neon 68 | ${EIGEN3_INCLUDE_DIR} 69 | ) 70 | 71 | 72 | # decide if we have pangolin 73 | if (Pangolin_FOUND) 74 | message("--- found PANGOLIN, compiling dso_pangolin library.") 75 | include_directories( ${Pangolin_INCLUDE_DIRS} ) 76 | set(dso_pangolin_SOURCE_FILES 77 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/Pangolin/KeyFrameDisplay.cpp 78 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/Pangolin/PangolinDSOViewer.cpp) 79 | set(HAS_PANGOLIN 1) 80 | else () 81 | message("--- could not find PANGOLIN, not compiling dso_pangolin library.") 82 | message(" this means there will be no 3D display / GUI available for dso_dataset.") 83 | set(dso_pangolin_SOURCE_FILES ) 84 | set(HAS_PANGOLIN 0) 85 | endif () 86 | 87 | # decide if we have openCV 88 | if (OpenCV_FOUND) 89 | message("--- found OpenCV, compiling dso_opencv library.") 90 | include_directories( ${OpenCV_INCLUDE_DIRS} ) 91 | set(dso_opencv_SOURCE_FILES 92 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/OpenCV/ImageDisplay_OpenCV.cpp 93 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/OpenCV/ImageRW_OpenCV.cpp) 94 | set(HAS_OPENCV 1) 95 | else () 96 | message("--- could not find OpenCV, not compiling dso_opencv library.") 97 | message(" this means there will be no image display, and image read / load functionality.") 98 | set(dso_opencv_SOURCE_FILES 99 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/ImageDisplay_dummy.cpp 100 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/ImageRW_dummy.cpp) 101 | set(HAS_OPENCV 0) 102 | endif () 103 | 104 | # decide if we have ziplib. 105 | if (LIBZIP_LIBRARY) 106 | message("--- found ziplib (${LIBZIP_VERSION}), compiling with zip capability.") 107 | add_definitions(-DHAS_ZIPLIB=1) 108 | include_directories( ${LIBZIP_INCLUDE_DIR_ZIP} ${LIBZIP_INCLUDE_DIR_ZIPCONF} ) 109 | else() 110 | message("--- not found ziplib (${LIBZIP_LIBRARY}), compiling without zip capability.") 111 | set(LIBZIP_LIBRARY "") 112 | endif() 113 | 114 | 115 | # compile main library. 116 | include_directories( ${CSPARSE_INCLUDE_DIR} ${CHOLMOD_INCLUDE_DIR}) 117 | add_library(dso ${dso_SOURCE_FILES} ${dso_opencv_SOURCE_FILES} ${dso_pangolin_SOURCE_FILES}) 118 | 119 | #set_property( TARGET dso APPEND_STRING PROPERTY COMPILE_FLAGS -Wall ) 120 | 121 | 122 | if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") # OSX 123 | set(BOOST_THREAD_LIBRARY boost_thread-mt) 124 | else() 125 | set(BOOST_THREAD_LIBRARY boost_thread) 126 | endif() 127 | 128 | # build main executable (only if we have both OpenCV and Pangolin) 129 | if (OpenCV_FOUND AND Pangolin_FOUND) 130 | message("--- compiling dso_dataset.") 131 | add_executable(dso_dataset ${PROJECT_SOURCE_DIR}/src/main_dso_pangolin.cpp ) 132 | target_link_libraries(dso_dataset dso boost_system cxsparse ${BOOST_THREAD_LIBRARY} ${LIBZIP_LIBRARY} ${Pangolin_LIBRARIES} ${OpenCV_LIBS} ${GLOG_LIBRARY}) 133 | else() 134 | message("--- not building dso_dataset, since either don't have openCV or Pangolin.") 135 | endif() 136 | 137 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # VI-Stereo-DSO 2 | 3 | ## Related Papers: 4 | - **Direct Sparse Odometry**, *J. Engel, V. Koltun, D. Cremers*, In arXiv:1607.02565, 2016 5 | 6 | - **Stereo DSO:Large-Scale Direct Sparse Visual Odometry with Stereo Cameras**, *Rui Wang, Martin Schwörer, Daniel Cremers*, 2017 IEEE International Conference on Computer Vision 7 | 8 | - **Direct Sparse Visual-Inertial Odometry using Dynamic Marginalization**, *Lukas von Stumberg, Vladyslav Usenko, Daniel Cremers*, 2018 IEEE International Conference on Robotics and Automation (ICRA) 9 | 10 | ## 1. Installation 11 | Please follow https://github.com/JakobEngel/dso. 12 | 13 | ## 2. Usage 14 | 1. Modify the bash file 15 | 16 | 2. For stereo: 17 | 18 | ``` 19 | bash ./run.bash 20 | ``` 21 | 22 | 3. For mono: 23 | 24 | ``` 25 | bash ./run_mono.bash 26 | ``` 27 | 28 | - T_stereo file: 29 | 30 | ``` 31 | transform matrix from right camera to left camera (4x4). 32 | ``` 33 | - imu_info file: 34 | ``` 35 | transform matrix from left camera to imu (4x4). 36 | gyroscope noise density [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) 37 | accelerometer noise density [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) 38 | gyroscope random walk [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) 39 | accelerometer random walk [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) 40 | ``` 41 | ## Experiments 42 | 43 | - 20190424 44 | 45 | SE(3) Umeyama alignment： 46 | 47 | | weight 6,0.6,0.5 | MH01 | MH02 | MH03 | MH04 | MH05 | V101 | V102 | V103 | V201 | V202 | V203 | 48 | | ------ | ------ | ------ | ------ | ------ | ------ | ------ | ------ | ------ | ------ | ------ | ------ | 49 | | ape rmse(m)| 0.0321 | 0.0288 | 0.0743 | 0.119 | 0.072 | 0.0599 | 0.105 | 0.168 | 0.0852 | 0.0667 | 0.211 | 50 | 51 | ![](https://github.com/RonaldSun/VI-Stereo-DSO/blob/master/pic/MH03_1.png) 52 | ![](https://github.com/RonaldSun/VI-Stereo-DSO/blob/master/pic/MH04_1.png) 53 | ![](https://github.com/RonaldSun/VI-Stereo-DSO/blob/master/pic/MH05_1.png) 54 | ![](https://github.com/RonaldSun/VI-Stereo-DSO/blob/master/pic/V102_1.png) 55 | 56 | 57 | 58 | No alignment（Initialization may have an impact on the results）: 59 | 60 | | weight 6,0.6,0.5 | MH01 | MH02 | MH03 | MH04 | MH05 | V101 | V102 | V103 | V201 | V202 | V203 | 61 | | ------ | ------ | ------ | ------ | ------ | ------ | ------ | ------ | ------ | ------ | ------ | ------ | 62 | | ape rmse(m)| 0.0993 | 0.0557 | 0.234 | 0.194 | 0.111 | 0.232 |0.202 | 0.2777 | 0.102 | 0.114 | 0.263| 63 | 64 | - 20190416 65 | 66 | Apply First Estimates Jacobians to scale to maintain consistency. 67 | 68 | EuRoC MH01: 69 | 70 | ![](https://github.com/RonaldSun/VI-Stereo-DSO/blob/master/pic/MH01.png) 71 | 72 | - EuRoC V1_03: 73 | 74 | ![](https://github.com/RonaldSun/VI-Stereo-DSO/blob/master/pic/euroc_v1_03.png) 75 | 76 | - EuRoC V2_03: 77 | 78 | ![](https://github.com/RonaldSun/VI-Stereo-DSO/blob/master/pic/euroc_V2_03.png) 79 | 80 | - 20190409: 81 | 82 | ![](https://github.com/RonaldSun/VI-Stereo-DSO/blob/master/pic/2019-04-09-V203.png) 83 | 84 | green line: groundtruth, redline: VI-Stereo-DSO 85 | 86 | ## P.S. 87 | 88 | This project aims at verifying the location performance of VI-Stero-DSO. The computational efficiency is to be improved. Actually in current code, many preintergration operations are redundant and can be simplified. -------------------------------------------------------------------------------- /calib/euroc/IMU_info.txt: -------------------------------------------------------------------------------- 1 | 0.0148655429818 -0.999880929698 0.00414029679422 -0.0216401454975 2 | 0.999557249008 0.0149672133247 0.025715529948 -0.064676986768 3 | -0.0257744366974 0.00375618835797 0.999660727178 0.00981073058949 4 | 0 0 0 1 5 | 1.6968e-04 6 | 2.0000e-3 7 | 1.9393e-05 8 | 3.0000e-3 9 | -------------------------------------------------------------------------------- /calib/euroc/T_C0C1.txt: -------------------------------------------------------------------------------- 1 | 1.00000 -0.00232 -0.00034 0.11007 2 | 0.00231 0.99990 -0.01409 -0.00016 3 | 0.00038 0.01409 0.99990 0.00089 4 | 0.00000 0.00000 0.00000 1.00000 5 | -------------------------------------------------------------------------------- /calib/euroc/cam0.txt: -------------------------------------------------------------------------------- 1 | RadTan 458.654 457.296 367.215 248.375 -0.28340811 0.07395907 0.00019359 1.76187114e-05 2 | 752 480 3 | crop 4 | 752 480 5 | -------------------------------------------------------------------------------- /calib/euroc/cam1.txt: -------------------------------------------------------------------------------- 1 | RadTan 457.587 456.134 379.999 255.238 -0.28368365 0.07451284 -0.00010473 -3.55590700e-05 2 | 752 480 3 | crop 4 | 752 480 5 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /pic/2019-04-09-V203.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RonaldSun/VI-Stereo-DSO/158cc08034a87e3dc4387674976d7cbcdc97034d/pic/2019-04-09-V203.png -------------------------------------------------------------------------------- /pic/MH01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RonaldSun/VI-Stereo-DSO/158cc08034a87e3dc4387674976d7cbcdc97034d/pic/MH01.png -------------------------------------------------------------------------------- /pic/MH02_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RonaldSun/VI-Stereo-DSO/158cc08034a87e3dc4387674976d7cbcdc97034d/pic/MH02_1.png -------------------------------------------------------------------------------- /pic/MH03_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RonaldSun/VI-Stereo-DSO/158cc08034a87e3dc4387674976d7cbcdc97034d/pic/MH03_1.png -------------------------------------------------------------------------------- /pic/MH04_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RonaldSun/VI-Stereo-DSO/158cc08034a87e3dc4387674976d7cbcdc97034d/pic/MH04_1.png -------------------------------------------------------------------------------- /pic/MH05_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RonaldSun/VI-Stereo-DSO/158cc08034a87e3dc4387674976d7cbcdc97034d/pic/MH05_1.png -------------------------------------------------------------------------------- /pic/V102_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RonaldSun/VI-Stereo-DSO/158cc08034a87e3dc4387674976d7cbcdc97034d/pic/V102_1.png -------------------------------------------------------------------------------- /pic/euroc_V2_03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RonaldSun/VI-Stereo-DSO/158cc08034a87e3dc4387674976d7cbcdc97034d/pic/euroc_V2_03.png -------------------------------------------------------------------------------- /pic/euroc_v1_03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RonaldSun/VI-Stereo-DSO/158cc08034a87e3dc4387674976d7cbcdc97034d/pic/euroc_v1_03.png -------------------------------------------------------------------------------- /run.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | #usage: python xxx.py file_name 3 | dataname="MH_01_easy" 4 | #dataname="MH_02_easy" 5 | #dataname="MH_03_medium" 6 | #dataname="MH_04_difficult" 7 | #dataname="MH_05_difficult" 8 | #dataname="V2_03_difficult" 9 | #dataname="V1_03_difficult" 10 | # run dso 11 | ./build/bin/dso_dataset \ 12 | files0=/media/sjm/SJM_WIN/AILEARN/SLAM/data_set/${dataname}/mav0/cam0/data \ 13 | files1=/media/sjm/SJM_WIN/AILEARN/SLAM/data_set/${dataname}/mav0/cam1/data \ 14 | calib0=/home/sjm/projects/VI-Stereo-DSO/calib/euroc/cam0.txt \ 15 | calib1=/home/sjm/projects/VI-Stereo-DSO/calib/euroc/cam1.txt \ 16 | T_stereo=/home/sjm/projects/VI-Stereo-DSO/calib/euroc/T_C0C1.txt \ 17 | imu_info=/home/sjm/projects/VI-Stereo-DSO/calib/euroc/IMU_info.txt \ 18 | groundtruth=/media/sjm/SJM_WIN/AILEARN/SLAM/data_set/${dataname}/mav0/state_groundtruth_estimate0/data.csv \ 19 | imudata=/media/sjm/SJM_WIN/AILEARN/SLAM/data_set/${dataname}/mav0/imu0/data.csv \ 20 | pic_timestamp=/media/sjm/SJM_WIN/AILEARN/SLAM/data_set/${dataname}/mav0/cam0/data.csv \ 21 | pic_timestamp1=/media/sjm/SJM_WIN/AILEARN/SLAM/data_set/${dataname}/mav0/cam1/data.csv \ 22 | preset=0 mode=1 \ 23 | quiet=1 nomt=1 \ 24 | savefile_tail=nt_${dataname}\ 25 | use_stereo=1\ 26 | imu_weight=6 imu_weight_tracker=0.6 stereo_weight=0.5 27 | 28 | 29 | -------------------------------------------------------------------------------- /run_euroc.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | #usage: python xxx.py file_name 3 | #dataname="MH_01_easy" 4 | #dataname="MH_02_easy" 5 | #dataname="MH_03_medium" 6 | #dataname="MH_04_difficult" 7 | #dataname="MH_05_difficult" 8 | dataname="V2_03_difficult" 9 | #dataname="V1_03_difficult" 10 | # run dso 11 | ./build/bin/dso_dataset \ 12 | files0=/media/sjm/SJM_WIN/AILEARN/SLAM/data_set/${dataname}/mav0/cam0/data \ 13 | files1=/media/sjm/SJM_WIN/AILEARN/SLAM/data_set/${dataname}/mav0/cam1/data \ 14 | calib0=/home/sjm/projects/VI-Stereo-DSO/calib/euroc/cam0.txt \ 15 | calib1=/home/sjm/projects/VI-Stereo-DSO/calib/euroc/cam1.txt \ 16 | T_stereo=/home/sjm/projects/VI-Stereo-DSO/calib/euroc/T_C0C1.txt \ 17 | groundtruth=/media/sjm/SJM_WIN/AILEARN/SLAM/data_set/${dataname}/mav0/state_groundtruth_estimate0/data.csv \ 18 | imudata=/media/sjm/SJM_WIN/AILEARN/SLAM/data_set/${dataname}/mav0/imu0/data.csv \ 19 | pic_timestamp=/media/sjm/SJM_WIN/AILEARN/SLAM/data_set/${dataname}/mav0/cam0/data.csv \ 20 | pic_timestamp1=/media/sjm/SJM_WIN/AILEARN/SLAM/data_set/${dataname}/mav0/cam1/data.csv \ 21 | preset=0 mode=1 \ 22 | quiet=1 nomt=1 \ 23 | savefile_tail=nt_${dataname}\ 24 | imu_weight=6 imu_weight_tracker=0.6 stereo_weight=0.5 25 | 26 | 27 | -------------------------------------------------------------------------------- /run_mono.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | #usage: python xxx.py file_name 3 | dataname="MH_01_easy" 4 | #dataname="MH_02_easy" 5 | #dataname="MH_03_medium" 6 | #dataname="MH_04_difficult" 7 | #dataname="MH_05_difficult" 8 | #dataname="V2_03_difficult" 9 | #dataname="V2_01_easy" 10 | #dataname="V2_02_medium" 11 | #dataname="V1_02_medium" 12 | #dataname="V1_01_easy" 13 | #dataname="V1_03_difficult" 14 | # run dso 15 | ./build/bin/dso_dataset \ 16 | files0=/media/sjm/SJM_WIN/AILEARN/SLAM/data_set/${dataname}/mav0/cam0/data \ 17 | calib0=/home/sjm/projects/VI-Stereo-DSO/calib/euroc/cam0.txt \ 18 | imu_info=/home/sjm/projects/VI-Stereo-DSO/calib/euroc/IMU_info.txt \ 19 | groundtruth=/media/sjm/SJM_WIN/AILEARN/SLAM/data_set/${dataname}/mav0/state_groundtruth_estimate0/data.csv \ 20 | imudata=/media/sjm/SJM_WIN/AILEARN/SLAM/data_set/${dataname}/mav0/imu0/data.csv \ 21 | pic_timestamp=/media/sjm/SJM_WIN/AILEARN/SLAM/data_set/${dataname}/mav0/cam0/data.csv \ 22 | preset=0 mode=1 \ 23 | quiet=1 nomt=1 \ 24 | savefile_tail=nt_${dataname}\ 25 | use_stereo=0\ 26 | imu_weight=6 imu_weight_tracker=0.6 stereo_weight=0 27 | 28 | -------------------------------------------------------------------------------- /src/FullSystem/CoarseInitializer.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 "OptimizationBackend/MatrixAccumulators.h" 29 | #include "IOWrapper/Output3DWrapper.h" 30 | #include "util/settings.h" 31 | #include "vector" 32 | #include 33 | 34 | 35 | 36 | 37 | namespace dso 38 | { 39 | struct CalibHessian; 40 | struct FrameHessian; 41 | 42 | 43 | struct Pnt 44 | { 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 47 | // index in jacobian. never changes (actually, there is no reason why). 48 | float u,v; 49 | 50 | // idepth / isgood / energy during optimization. 51 | float idepth; 52 | bool isGood; 53 | Vec2f energy; // (UenergyPhotometric, energyRegularizer) 54 | bool isGood_new; 55 | float idepth_new; 56 | Vec2f energy_new; 57 | 58 | float iR; 59 | float iRSumNum; 60 | 61 | float lastHessian; 62 | float lastHessian_new; 63 | 64 | // max stepsize for idepth (corresponding to max. movement in pixel-space). 65 | float maxstep; 66 | 67 | // idx (x+y*w) of closest point one pyramid level above. 68 | int parent; 69 | float parentDist; 70 | 71 | // idx (x+y*w) of up to 10 nearest points in pixel space. 72 | int neighbours[10]; 73 | float neighboursDist[10]; 74 | 75 | float my_type; 76 | float outlierTH; 77 | }; 78 | 79 | class CoarseInitializer { 80 | public: 81 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 82 | CoarseInitializer(int w, int h); 83 | ~CoarseInitializer(); 84 | 85 | 86 | void setFirst( CalibHessian* HCalib, FrameHessian* newFrameHessian); 87 | void setFirstStereo( CalibHessian* HCalib, FrameHessian* newFrameHessian, FrameHessian* newFrameHessian_right); 88 | bool trackFrame(FrameHessian* newFrameHessian, std::vector &wraps); 89 | void calcTGrads(FrameHessian* newFrameHessian); 90 | 91 | int frameID; 92 | bool fixAffine; 93 | bool printDebug; 94 | 95 | Pnt* points[PYR_LEVELS]; 96 | int numPoints[PYR_LEVELS]; 97 | AffLight thisToNext_aff; 98 | SE3 thisToNext; 99 | 100 | 101 | FrameHessian* firstFrame; 102 | FrameHessian* firstFrame_right; 103 | FrameHessian* newFrame; 104 | private: 105 | Mat33 K[PYR_LEVELS]; 106 | Mat33 Ki[PYR_LEVELS]; 107 | double fx[PYR_LEVELS]; 108 | double fy[PYR_LEVELS]; 109 | double fxi[PYR_LEVELS]; 110 | double fyi[PYR_LEVELS]; 111 | double cx[PYR_LEVELS]; 112 | double cy[PYR_LEVELS]; 113 | double cxi[PYR_LEVELS]; 114 | double cyi[PYR_LEVELS]; 115 | int w[PYR_LEVELS]; 116 | int h[PYR_LEVELS]; 117 | void makeK(CalibHessian* HCalib); 118 | float* idepth[PYR_LEVELS]; 119 | 120 | bool snapped; 121 | int snappedAt; 122 | 123 | // pyramid images & levels on all levels 124 | Eigen::Vector3f* dINew[PYR_LEVELS]; 125 | Eigen::Vector3f* dIFist[PYR_LEVELS]; 126 | 127 | Eigen::DiagonalMatrix wM; 128 | 129 | // temporary buffers for H and b. 130 | Vec10f* JbBuffer; // 0-7: sum(dd * dp). 8: sum(res*dd). 9: 1/(1+sum(dd*dd))=inverse hessian entry. 131 | Vec10f* JbBuffer_new; 132 | 133 | Accumulator9 acc9; 134 | Accumulator9 acc9SC; 135 | 136 | 137 | Vec3f dGrads[PYR_LEVELS]; 138 | 139 | float alphaK; 140 | float alphaW; 141 | float regWeight; 142 | float couplingWeight; 143 | 144 | Vec3f calcResAndGS( 145 | int lvl, 146 | Mat88f &H_out, Vec8f &b_out, 147 | Mat88f &H_out_sc, Vec8f &b_out_sc, 148 | const SE3 &refToNew, AffLight refToNew_aff, 149 | bool plot); 150 | Vec3f calcEC(int lvl); // returns OLD NERGY, NEW ENERGY, NUM TERMS. 151 | void optReg(int lvl); 152 | 153 | void propagateUp(int srcLvl); 154 | void propagateDown(int srcLvl); 155 | float rescale(); 156 | 157 | void resetPoints(int lvl); 158 | void doStep(int lvl, float lambda, Vec8f inc); 159 | void applyStep(int lvl); 160 | 161 | void makeGradients(Eigen::Vector3f** data); 162 | 163 | void debugPlot(int lvl, std::vector &wraps); 164 | void makeNN(); 165 | }; 166 | 167 | 168 | 169 | 170 | struct FLANNPointcloud 171 | { 172 | inline FLANNPointcloud() {num=0; points=0;} 173 | inline FLANNPointcloud(int n, Pnt* p) : num(n), points(p) {} 174 | int num; 175 | Pnt* points; 176 | inline size_t kdtree_get_point_count() const { return num; } 177 | inline float kdtree_distance(const float *p1, const size_t idx_p2,size_t /*size*/) const 178 | { 179 | const float d0=p1[0]-points[idx_p2].u; 180 | const float d1=p1[1]-points[idx_p2].v; 181 | return d0*d0+d1*d1; 182 | } 183 | 184 | inline float kdtree_get_pt(const size_t idx, int dim) const 185 | { 186 | if (dim==0) return points[idx].u; 187 | else return points[idx].v; 188 | } 189 | template 190 | bool kdtree_get_bbox(BBOX& /* bb */) const { return false; } 191 | }; 192 | 193 | } 194 | 195 | 196 | -------------------------------------------------------------------------------- /src/FullSystem/CoarseTracker.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 "util/settings.h" 32 | #include "OptimizationBackend/MatrixAccumulators.h" 33 | #include "IOWrapper/Output3DWrapper.h" 34 | #include "FullSystem/IMUPreintegrator.h" 35 | 36 | 37 | 38 | 39 | namespace dso 40 | { 41 | struct CalibHessian; 42 | struct FrameHessian; 43 | struct PointFrameResidual; 44 | 45 | class CoarseTracker { 46 | public: 47 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 48 | 49 | CoarseTracker(int w, int h); 50 | ~CoarseTracker(); 51 | 52 | bool trackNewestCoarse( 53 | FrameHessian* newFrameHessian, 54 | SE3 &lastToNew_out, AffLight &aff_g2l_out, 55 | int coarsestLvl, Vec5 minResForAbort, 56 | IOWrap::Output3DWrapper* wrap=0); 57 | 58 | void setCTRefForFirstFrame( 59 | std::vector frameHessians); 60 | 61 | void setCoarseTrackingRef( 62 | std::vector frameHessians, FrameHessian* fh_right, CalibHessian Hcalib); 63 | 64 | void makeCoarseDepthForFirstFrame(FrameHessian* fh); 65 | 66 | void makeK( 67 | CalibHessian* HCalib); 68 | 69 | bool debugPrint, debugPlot; 70 | 71 | Mat33f K[PYR_LEVELS]; 72 | Mat33f Ki[PYR_LEVELS]; 73 | float fx[PYR_LEVELS]; 74 | float fy[PYR_LEVELS]; 75 | float fxi[PYR_LEVELS]; 76 | float fyi[PYR_LEVELS]; 77 | float cx[PYR_LEVELS]; 78 | float cy[PYR_LEVELS]; 79 | float cxi[PYR_LEVELS]; 80 | float cyi[PYR_LEVELS]; 81 | int w[PYR_LEVELS]; 82 | int h[PYR_LEVELS]; 83 | 84 | void debugPlotIDepthMap(float* minID, float* maxID, std::vector &wraps); 85 | void debugPlotIDepthMapFloat(std::vector &wraps); 86 | 87 | FrameHessian* lastRef; 88 | AffLight lastRef_aff_g2l; 89 | FrameHessian* newFrame; 90 | int refFrameID; 91 | 92 | // act as pure ouptut 93 | Vec5 lastResiduals; 94 | Vec3 lastFlowIndicators; 95 | double firstCoarseRMSE; 96 | int pc_n[PYR_LEVELS]; 97 | private: 98 | 99 | 100 | void makeCoarseDepthL0(std::vector frameHessians, FrameHessian* fh_right, CalibHessian Hcalib); 101 | float* idepth[PYR_LEVELS]; 102 | float* weightSums[PYR_LEVELS]; 103 | float* weightSums_bak[PYR_LEVELS]; 104 | 105 | 106 | Vec6 calcResAndGS(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH); 107 | double calcIMUResAndGS(Mat66 &H_out, Vec6 &b_out, SE3 &refToNew, const IMUPreintegrator &IMU_preintegrator, Vec9 &res_PVPhi, double PointEnergy, double imu_track_weight); 108 | Vec6 calcRes(int lvl, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH); 109 | void calcGSSSE(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l); 110 | void calcGS(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l); 111 | 112 | // pc buffers 113 | float* pc_u[PYR_LEVELS]; 114 | float* pc_v[PYR_LEVELS]; 115 | float* pc_idepth[PYR_LEVELS]; 116 | float* pc_color[PYR_LEVELS]; 117 | 118 | 119 | // warped buffers 120 | float* buf_warped_idepth; 121 | float* buf_warped_u; 122 | float* buf_warped_v; 123 | float* buf_warped_dx; 124 | float* buf_warped_dy; 125 | float* buf_warped_residual; 126 | float* buf_warped_weight; 127 | float* buf_warped_refColor; 128 | int buf_warped_n; 129 | 130 | 131 | std::vector ptrToDelete; 132 | 133 | 134 | Accumulator9 acc; 135 | }; 136 | 137 | 138 | class CoarseDistanceMap { 139 | public: 140 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 141 | 142 | CoarseDistanceMap(int w, int h); 143 | ~CoarseDistanceMap(); 144 | 145 | void makeDistanceMap( 146 | std::vector frameHessians, 147 | FrameHessian* frame); 148 | 149 | void makeInlierVotes( 150 | std::vector frameHessians); 151 | 152 | void makeK( CalibHessian* HCalib); 153 | 154 | 155 | float* fwdWarpedIDDistFinal; 156 | 157 | Mat33f K[PYR_LEVELS]; 158 | Mat33f Ki[PYR_LEVELS]; 159 | float fx[PYR_LEVELS]; 160 | float fy[PYR_LEVELS]; 161 | float fxi[PYR_LEVELS]; 162 | float fyi[PYR_LEVELS]; 163 | float cx[PYR_LEVELS]; 164 | float cy[PYR_LEVELS]; 165 | float cxi[PYR_LEVELS]; 166 | float cyi[PYR_LEVELS]; 167 | int w[PYR_LEVELS]; 168 | int h[PYR_LEVELS]; 169 | 170 | void addIntoDistFinal(int u, int v); 171 | 172 | 173 | private: 174 | 175 | PointFrameResidual** coarseProjectionGrid; 176 | int* coarseProjectionGridNum; 177 | Eigen::Vector2i* bfsList1; 178 | Eigen::Vector2i* bfsList2; 179 | 180 | void growDistBFS(int bfsNum); 181 | }; 182 | 183 | } 184 | 185 | -------------------------------------------------------------------------------- /src/FullSystem/FullSystemMarginalize.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 | * KFBuffer.cpp 27 | * 28 | * Created on: Jan 7, 2014 29 | * Author: engelj 30 | */ 31 | 32 | #include "FullSystem/FullSystem.h" 33 | 34 | #include "stdio.h" 35 | #include "util/globalFuncs.h" 36 | #include 37 | #include 38 | #include "IOWrapper/ImageDisplay.h" 39 | #include "util/globalCalib.h" 40 | 41 | #include 42 | #include 43 | #include "FullSystem/ResidualProjections.h" 44 | #include "FullSystem/ImmaturePoint.h" 45 | 46 | #include "OptimizationBackend/EnergyFunctional.h" 47 | #include "OptimizationBackend/EnergyFunctionalStructs.h" 48 | 49 | #include "IOWrapper/Output3DWrapper.h" 50 | 51 | #include "FullSystem/CoarseTracker.h" 52 | 53 | namespace dso 54 | { 55 | 56 | 57 | 58 | void FullSystem::flagFramesForMarginalization(FrameHessian* newFH) 59 | { 60 | if(setting_minFrameAge > setting_maxFrames) 61 | { 62 | for(int i=setting_maxFrames;i<(int)frameHessians.size();i++) 63 | { 64 | FrameHessian* fh = frameHessians[i-setting_maxFrames]; 65 | fh->flaggedForMarginalization = true; 66 | } 67 | return; 68 | } 69 | 70 | 71 | int flagged = 0; 72 | // marginalize all frames that have not enough points. 73 | for(int i=0;i<(int)frameHessians.size();i++) 74 | { 75 | FrameHessian* fh = frameHessians[i]; 76 | int in = fh->pointHessians.size() + fh->immaturePoints.size(); 77 | int out = fh->pointHessiansMarginalized.size() + fh->pointHessiansOut.size(); 78 | 79 | 80 | Vec2 refToFh=AffLight::fromToVecExposure(frameHessians.back()->ab_exposure, fh->ab_exposure, 81 | frameHessians.back()->aff_g2l(), fh->aff_g2l()); 82 | 83 | 84 | if( (in < setting_minPointsRemaining *(in+out) || fabs(logf((float)refToFh[0])) > setting_maxLogAffFacInWindow) 85 | && ((int)frameHessians.size())-flagged > setting_minFrames) 86 | { 87 | // printf("MARGINALIZE frame %d, as only %'d/%'d points remaining (%'d %'d %'d %'d). VisInLast %'d / %'d. traces %d, activated %d!\n", 88 | // fh->frameID, in, in+out, 89 | // (int)fh->pointHessians.size(), (int)fh->immaturePoints.size(), 90 | // (int)fh->pointHessiansMarginalized.size(), (int)fh->pointHessiansOut.size(), 91 | // visInLast, outInLast, 92 | // fh->statistics_tracesCreatedForThisFrame, fh->statistics_pointsActivatedForThisFrame); 93 | fh->flaggedForMarginalization = true; 94 | flagged++; 95 | } 96 | else 97 | { 98 | // printf("May Keep frame %d, as %'d/%'d points remaining (%'d %'d %'d %'d). VisInLast %'d / %'d. traces %d, activated %d!\n", 99 | // fh->frameID, in, in+out, 100 | // (int)fh->pointHessians.size(), (int)fh->immaturePoints.size(), 101 | // (int)fh->pointHessiansMarginalized.size(), (int)fh->pointHessiansOut.size(), 102 | // visInLast, outInLast, 103 | // fh->statistics_tracesCreatedForThisFrame, fh->statistics_pointsActivatedForThisFrame); 104 | } 105 | } 106 | 107 | // marginalize one. 108 | if((int)frameHessians.size()-flagged >= setting_maxFrames) 109 | { 110 | double smallestScore = 1; 111 | FrameHessian* toMarginalize=0; 112 | FrameHessian* latest = frameHessians.back(); 113 | 114 | 115 | for(FrameHessian* fh : frameHessians) 116 | { 117 | if(fh->frameID > latest->frameID-setting_minFrameAge || fh->frameID == 0) continue; 118 | //if(fh==frameHessians.front() == 0) continue; 119 | 120 | double distScore = 0; 121 | for(FrameFramePrecalc &ffh : fh->targetPrecalc) 122 | { 123 | if(ffh.target->frameID > latest->frameID-setting_minFrameAge+1 || ffh.target == ffh.host) continue; 124 | distScore += 1/(1e-5+ffh.distanceLL); 125 | 126 | } 127 | distScore *= -sqrtf(fh->targetPrecalc.back().distanceLL); 128 | 129 | 130 | if(distScore < smallestScore) 131 | { 132 | smallestScore = distScore; 133 | toMarginalize = fh; 134 | } 135 | } 136 | 137 | // printf("MARGINALIZE frame %d, as it is the closest (score %.2f)!\n", 138 | // toMarginalize->frameID, smallestScore); 139 | toMarginalize->flaggedForMarginalization = true; 140 | flagged++; 141 | } 142 | // if((int)frameHessians.size()-flagged >= setting_maxFrames){ 143 | // frameHessians[0]->flaggedForMarginalization = true; 144 | // flagged++; 145 | // 146 | // } 147 | 148 | // printf("FRAMES LEFT: "); 149 | // for(FrameHessian* fh : frameHessians) 150 | // printf("%d ", fh->frameID); 151 | // printf("\n"); 152 | } 153 | 154 | 155 | 156 | 157 | void FullSystem::marginalizeFrame(FrameHessian* frame) 158 | { 159 | // marginalize or remove all this frames points. 160 | 161 | assert((int)frame->pointHessians.size()==0); 162 | 163 | delete frame->frame_right->efFrame; 164 | frame->frame_right->efFrame=0; 165 | delete frame->frame_right; 166 | ef->marginalizeFrame(frame->efFrame); 167 | // drop all observations of existing points in that frame. 168 | 169 | for(FrameHessian* fh : frameHessians) 170 | { 171 | if(fh==frame) continue; 172 | 173 | for(PointHessian* ph : fh->pointHessians) 174 | { 175 | for(unsigned int i=0;iresiduals.size();i++) 176 | { 177 | PointFrameResidual* r = ph->residuals[i]; 178 | if(r->target == frame) 179 | { 180 | if(ph->lastResiduals[0].first == r) 181 | ph->lastResiduals[0].first=0; 182 | else if(ph->lastResiduals[1].first == r) 183 | ph->lastResiduals[1].first=0; 184 | 185 | 186 | if(r->host->frameID < r->target->frameID) 187 | statistics_numForceDroppedResFwd++; 188 | else 189 | statistics_numForceDroppedResBwd++; 190 | 191 | ef->dropResidual(r->efResidual); 192 | deleteOut(ph->residuals,i); 193 | break; 194 | } 195 | } 196 | } 197 | } 198 | 199 | 200 | { 201 | std::vector v; 202 | v.push_back(frame); 203 | for(IOWrap::Output3DWrapper* ow : outputWrapper) 204 | ow->publishKeyframes(v, true, &Hcalib); 205 | } 206 | 207 | frame->shell->marginalizedAt = frameHessians.back()->shell->id; 208 | frame->shell->movedByOpt = frame->w2c_leftEps().norm(); 209 | 210 | deleteOutOrder(frameHessians, frame); 211 | for(unsigned int i=0;iidx = i; 213 | 214 | 215 | 216 | setPrecalcValues(); 217 | ef->setAdjointsF(&Hcalib); 218 | } 219 | 220 | 221 | 222 | 223 | } 224 | -------------------------------------------------------------------------------- /src/FullSystem/FullSystemOptPoint.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 | * KFBuffer.cpp 27 | * 28 | * Created on: Jan 7, 2014 29 | * Author: engelj 30 | */ 31 | 32 | #include "FullSystem/FullSystem.h" 33 | 34 | #include "stdio.h" 35 | #include "util/globalFuncs.h" 36 | #include 37 | #include 38 | #include "IOWrapper/ImageDisplay.h" 39 | #include "util/globalCalib.h" 40 | 41 | #include 42 | #include 43 | #include "FullSystem/ImmaturePoint.h" 44 | #include "math.h" 45 | 46 | namespace dso 47 | { 48 | 49 | 50 | 51 | PointHessian* FullSystem::optimizeImmaturePoint( 52 | ImmaturePoint* point, int minObs, 53 | ImmaturePointTemporaryResidual* residuals) 54 | { 55 | int nres = 0; 56 | for(FrameHessian* fh : frameHessians) 57 | { 58 | if(fh != point->host) 59 | { 60 | residuals[nres].state_NewEnergy = residuals[nres].state_energy = 0; 61 | residuals[nres].state_NewState = ResState::OUTLIER; 62 | residuals[nres].state_state = ResState::IN; 63 | residuals[nres].target = fh; 64 | nres++; 65 | } 66 | } 67 | assert(nres == ((int)frameHessians.size())-1); 68 | 69 | bool print = false;//rand()%50==0; 70 | 71 | float lastEnergy = 0; 72 | float lastHdd=0; 73 | float lastbd=0; 74 | float currentIdepth=(point->idepth_max+point->idepth_min)*0.5f; 75 | 76 | 77 | 78 | 79 | 80 | 81 | for(int i=0;ilinearizeResidual(&Hcalib, 1000, residuals+i,lastHdd, lastbd, currentIdepth); 84 | residuals[i].state_state = residuals[i].state_NewState; 85 | residuals[i].state_energy = residuals[i].state_NewEnergy; 86 | } 87 | 88 | if(!std::isfinite(lastEnergy) || lastHdd < setting_minIdepthH_act) 89 | { 90 | if(print) 91 | printf("OptPoint: Not well-constrained (%d res, H=%.1f). E=%f. SKIP!\n", 92 | nres, lastHdd, lastEnergy); 93 | return 0; 94 | } 95 | 96 | if(print) printf("Activate point. %d residuals. H=%f. Initial Energy: %f. Initial Id=%f\n" , 97 | nres, lastHdd,lastEnergy,currentIdepth); 98 | 99 | float lambda = 0.1; 100 | for(int iteration=0;iterationlinearizeResidual(&Hcalib, 1, residuals+i,newHdd, newbd, newIdepth); 110 | 111 | if(!std::isfinite(lastEnergy) || newHdd < setting_minIdepthH_act) 112 | { 113 | if(print) printf("OptPoint: Not well-constrained (%d res, H=%.1f). E=%f. SKIP!\n", 114 | nres, 115 | newHdd, 116 | lastEnergy); 117 | return 0; 118 | } 119 | 120 | if(print) printf("%s %d (L %.2f) %s: %f -> %f (idepth %f)!\n", 121 | (true || newEnergy < lastEnergy) ? "ACCEPT" : "REJECT", 122 | iteration, 123 | log10(lambda), 124 | "", 125 | lastEnergy, newEnergy, newIdepth); 126 | 127 | if(newEnergy < lastEnergy) 128 | { 129 | currentIdepth = newIdepth; 130 | lastHdd = newHdd; 131 | lastbd = newbd; 132 | lastEnergy = newEnergy; 133 | for(int i=0;ienergyTH)) {delete p; return (PointHessian*)((long)(-1));} 171 | 172 | p->lastResiduals[0].first = 0; 173 | p->lastResiduals[0].second = ResState::OOB; 174 | p->lastResiduals[1].first = 0; 175 | p->lastResiduals[1].second = ResState::OOB; 176 | p->setIdepthZero(currentIdepth); 177 | p->setIdepth(currentIdepth); 178 | p->setPointStatus(PointHessian::ACTIVE); 179 | 180 | PointFrameResidual* r = new PointFrameResidual(p, p->host, p->host->frame_right); 181 | r->state_NewEnergy = r->state_energy = 0; 182 | r->state_NewState = ResState::OUTLIER; 183 | r->setState(ResState::IN); 184 | r->stereoResidualFlag = true; 185 | p->residuals.push_back(r); 186 | for(int i=0;ihost, residuals[i].target); 190 | r->state_NewEnergy = r->state_energy = 0; 191 | r->state_NewState = ResState::OUTLIER; 192 | r->setState(ResState::IN); 193 | p->residuals.push_back(r); 194 | 195 | if(r->target == frameHessians.back()) 196 | { 197 | p->lastResiduals[0].first = r; 198 | p->lastResiduals[0].second = ResState::IN; 199 | } 200 | else if(r->target == (frameHessians.size()<2 ? 0 : frameHessians[frameHessians.size()-2])) 201 | { 202 | p->lastResiduals[1].first = r; 203 | p->lastResiduals[1].second = ResState::IN; 204 | } 205 | } 206 | 207 | if(print) printf("point activated!\n"); 208 | 209 | statistics_numActivatedPoints++; 210 | return p; 211 | } 212 | 213 | 214 | 215 | } 216 | -------------------------------------------------------------------------------- /src/FullSystem/HessianBlocks.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 "FullSystem/HessianBlocks.h" 27 | #include "util/FrameShell.h" 28 | #include "FullSystem/ImmaturePoint.h" 29 | #include "OptimizationBackend/EnergyFunctionalStructs.h" 30 | 31 | namespace dso 32 | { 33 | 34 | 35 | PointHessian::PointHessian(const ImmaturePoint* const rawPoint, CalibHessian* Hcalib) 36 | { 37 | instanceCounter++; 38 | host = rawPoint->host; 39 | hasDepthPrior=false; 40 | 41 | idepth_hessian=0; 42 | maxRelBaseline=0; 43 | numGoodResiduals=0; 44 | 45 | // set static values & initialization. 46 | u = rawPoint->u; 47 | v = rawPoint->v; 48 | assert(std::isfinite(rawPoint->idepth_max)); 49 | //idepth_init = rawPoint->idepth_GT; 50 | 51 | my_type = rawPoint->my_type; 52 | 53 | setIdepthScaled((rawPoint->idepth_max + rawPoint->idepth_min)*0.5); 54 | setPointStatus(PointHessian::INACTIVE); 55 | 56 | int n = patternNum; 57 | memcpy(color, rawPoint->color, sizeof(float)*n); 58 | memcpy(weights, rawPoint->weights, sizeof(float)*n); 59 | energyTH = rawPoint->energyTH; 60 | 61 | efPoint=0; 62 | 63 | 64 | } 65 | 66 | 67 | void PointHessian::release() 68 | { 69 | for(unsigned int i=0;i().squaredNorm() < 1e-20); 77 | 78 | this->state_zero = state_zero; 79 | 80 | 81 | for(int i=0;i<6;i++) 82 | { 83 | Vec6 eps; eps.setZero(); eps[i] = 1e-3; 84 | SE3 EepsP = Sophus::SE3::exp(eps); 85 | SE3 EepsM = Sophus::SE3::exp(-eps); 86 | SE3 w2c_leftEps_P_x0 = (get_worldToCam_evalPT() * EepsP) * get_worldToCam_evalPT().inverse(); 87 | SE3 w2c_leftEps_M_x0 = (get_worldToCam_evalPT() * EepsM) * get_worldToCam_evalPT().inverse(); 88 | nullspaces_pose.col(i) = (w2c_leftEps_P_x0.log() - w2c_leftEps_M_x0.log())/(2e-3); 89 | } 90 | //nullspaces_pose.topRows<3>() *= SCALE_XI_TRANS_INVERSE; 91 | //nullspaces_pose.bottomRows<3>() *= SCALE_XI_ROT_INVERSE; 92 | 93 | // scale change 94 | SE3 w2c_leftEps_P_x0 = (get_worldToCam_evalPT()); 95 | w2c_leftEps_P_x0.translation() *= 1.00001; 96 | w2c_leftEps_P_x0 = w2c_leftEps_P_x0 * get_worldToCam_evalPT().inverse(); 97 | SE3 w2c_leftEps_M_x0 = (get_worldToCam_evalPT()); 98 | w2c_leftEps_M_x0.translation() /= 1.00001; 99 | w2c_leftEps_M_x0 = w2c_leftEps_M_x0 * get_worldToCam_evalPT().inverse(); 100 | nullspaces_scale = (w2c_leftEps_P_x0.log() - w2c_leftEps_M_x0.log())/(2e-3); 101 | 102 | 103 | nullspaces_affine.setZero(); 104 | nullspaces_affine.topLeftCorner<2,1>() = Vec2(1,0); 105 | assert(ab_exposure > 0); 106 | nullspaces_affine.topRightCorner<2,1>() = Vec2(0, expf(aff_g2l_0().a)*ab_exposure); 107 | }; 108 | 109 | 110 | 111 | void FrameHessian::release() 112 | { 113 | // DELETE POINT 114 | // DELETE RESIDUAL 115 | for(unsigned int i=0;i0) 152 | { 153 | int lvlm1 = lvl-1; 154 | int wlm1 = wG[lvlm1]; 155 | Eigen::Vector3f* dI_lm = dIp[lvlm1]; 156 | 157 | 158 | 159 | for(int y=0;ygetBGradOnly((float)(dI_l[idx][0])); 187 | dabs_l[idx] *= gw*gw; // convert to gradient of original color space (before removing response). 188 | } 189 | } 190 | } 191 | } 192 | 193 | void FrameFramePrecalc::set(FrameHessian* host, FrameHessian* target, CalibHessian* HCalib ) 194 | { 195 | this->host = host; 196 | this->target = target; 197 | 198 | SE3 leftToLeft_0 = target->get_worldToCam_evalPT() * host->get_worldToCam_evalPT().inverse(); 199 | PRE_RTll_0 = (leftToLeft_0.rotationMatrix()).cast(); 200 | PRE_tTll_0 = (leftToLeft_0.translation()).cast(); 201 | 202 | 203 | 204 | SE3 leftToLeft = target->PRE_worldToCam * host->PRE_camToWorld; 205 | PRE_RTll = (leftToLeft.rotationMatrix()).cast(); 206 | PRE_tTll = (leftToLeft.translation()).cast(); 207 | distanceLL = leftToLeft.translation().norm(); 208 | 209 | 210 | Mat33f K = Mat33f::Zero(); 211 | K(0,0) = HCalib->fxl(); 212 | K(1,1) = HCalib->fyl(); 213 | K(0,2) = HCalib->cxl(); 214 | K(1,2) = HCalib->cyl(); 215 | K(2,2) = 1; 216 | PRE_KRKiTll = K * PRE_RTll * K.inverse(); 217 | PRE_RKiTll = PRE_RTll * K.inverse(); 218 | PRE_KtTll = K * PRE_tTll; 219 | 220 | 221 | PRE_aff_mode = AffLight::fromToVecExposure(host->ab_exposure, target->ab_exposure, host->aff_g2l(), target->aff_g2l()).cast(); 222 | PRE_b0_mode = host->aff_g2l_0().b; 223 | } 224 | 225 | } 226 | 227 | -------------------------------------------------------------------------------- /src/FullSystem/IMUPreintegrator.cpp: -------------------------------------------------------------------------------- 1 | #include "FullSystem/IMUPreintegrator.h" 2 | #include "util/settings.h" 3 | 4 | namespace dso 5 | { 6 | IMUPreintegrator::IMUPreintegrator(const IMUPreintegrator& pre): 7 | _delta_P(pre._delta_P), 8 | _delta_V(pre._delta_V), 9 | _delta_R(pre._delta_R), 10 | _J_P_Biasg(pre._J_P_Biasg), 11 | _J_P_Biasa(pre._J_P_Biasa), 12 | _J_V_Biasg(pre._J_V_Biasg), 13 | _J_V_Biasa(pre._J_V_Biasa), 14 | _J_R_Biasg(pre._J_R_Biasg), 15 | _cov_P_V_Phi(pre._cov_P_V_Phi), 16 | _delta_time(pre._delta_time) 17 | { 18 | 19 | } 20 | 21 | IMUPreintegrator::IMUPreintegrator() 22 | { 23 | // delta measurements, position/velocity/rotation(matrix) 24 | _delta_P.setZero(); // P_k+1 = P_k + V_k*dt + R_k*a_k*dt*dt/2 25 | _delta_V.setZero(); // V_k+1 = V_k + R_k*a_k*dt 26 | _delta_R.setIdentity(); // R_k+1 = R_k*exp(w_k*dt). note: Rwc, Rwc'=Rwc*[w_body]x 27 | 28 | // jacobian of delta measurements w.r.t bias of gyro/acc 29 | _J_P_Biasg.setZero(); // position / gyro 30 | _J_P_Biasa.setZero(); // position / acc 31 | _J_V_Biasg.setZero(); // velocity / gyro 32 | _J_V_Biasa.setZero(); // velocity / acc 33 | _J_R_Biasg.setZero(); // rotation / gyro 34 | 35 | // noise covariance propagation of delta measurements 36 | _cov_P_V_Phi.setZero(); 37 | 38 | _delta_time = 0; 39 | } 40 | 41 | void IMUPreintegrator::reset() 42 | { 43 | // delta measurements, position/velocity/rotation(matrix) 44 | _delta_P.setZero(); // P_k+1 = P_k + V_k*dt + R_k*a_k*dt*dt/2 45 | _delta_V.setZero(); // V_k+1 = V_k + R_k*a_k*dt 46 | _delta_R.setIdentity(); // R_k+1 = R_k*exp(w_k*dt). note: Rwc, Rwc'=Rwc*[w_body]x 47 | 48 | // jacobian of delta measurements w.r.t bias of gyro/acc 49 | _J_P_Biasg.setZero(); // position / gyro 50 | _J_P_Biasa.setZero(); // position / acc 51 | _J_V_Biasg.setZero(); // velocity / gyro 52 | _J_V_Biasa.setZero(); // velocity / acc 53 | _J_R_Biasg.setZero(); // rotation / gyro 54 | 55 | // noise covariance propagation of delta measurements 56 | _cov_P_V_Phi.setZero(); 57 | 58 | _delta_time = 0; 59 | 60 | } 61 | 62 | // incrementally update 1)delta measurements, 2)jacobians, 3)covariance matrix 63 | // acc: acc_measurement - bias_a, last measurement!! not current measurement 64 | // omega: gyro_measurement - bias_g, last measurement!! not current measurement 65 | void IMUPreintegrator::update(const Vec3& omega, const Vec3& acc, const double& dt) 66 | { 67 | double dt2 = dt*dt; 68 | 69 | Mat33 dR = Expmap(omega*dt); 70 | Mat33 Jr = JacobianR(omega*dt); 71 | 72 | // noise covariance propagation of delta measurements 73 | // err_k+1 = A*err_k + B*err_gyro + C*err_acc 74 | Mat33 I3x3 = Mat33::Identity(); 75 | Mat99 A = Mat99::Identity(); 76 | A.block<3,3>(6,6) = dR.transpose(); 77 | A.block<3,3>(3,6) = -_delta_R*skew(acc)*dt; 78 | A.block<3,3>(0,6) = -0.5*_delta_R*skew(acc)*dt2; 79 | A.block<3,3>(0,3) = I3x3*dt; 80 | Mat93 Bg = Mat93::Zero(); 81 | Bg.block<3,3>(6,0) = Jr*dt; 82 | Mat93 Ca = Mat93::Zero(); 83 | Ca.block<3,3>(3,0) = _delta_R*dt; 84 | Ca.block<3,3>(0,0) = 0.5*_delta_R*dt2; 85 | _cov_P_V_Phi = A*_cov_P_V_Phi*A.transpose() + 86 | Bg*GyrCov*Bg.transpose() + 87 | Ca*AccCov*Ca.transpose(); 88 | 89 | 90 | // jacobian of delta measurements w.r.t bias of gyro/acc 91 | // update P first, then V, then R 92 | _J_P_Biasa += _J_V_Biasa*dt - 0.5*_delta_R*dt2; 93 | _J_P_Biasg += _J_V_Biasg*dt - 0.5*_delta_R*skew(acc)*_J_R_Biasg*dt2; 94 | _J_V_Biasa += -_delta_R*dt; 95 | _J_V_Biasg += -_delta_R*skew(acc)*_J_R_Biasg*dt; 96 | _J_R_Biasg = dR.transpose()*_J_R_Biasg - Jr*dt; 97 | 98 | // delta measurements, position/velocity/rotation(matrix) 99 | // update P first, then V, then R. because P's update need V&R's previous state 100 | _delta_P += _delta_V*dt + 0.5*_delta_R*acc*dt2; // P_k+1 = P_k + V_k*dt + R_k*a_k*dt*dt/2 101 | _delta_V += _delta_R*acc*dt; 102 | _delta_R = normalizeRotationM(_delta_R*dR); // normalize rotation, in case of numerical error accumulation 103 | 104 | 105 | // // noise covariance propagation of delta measurements 106 | // // err_k+1 = A*err_k + B*err_gyro + C*err_acc 107 | // Mat33 I3x3 = Mat33::Identity(); 108 | // MatrixXd A = MatrixXd::Identity(9,9); 109 | // A.block<3,3>(6,6) = dR.transpose(); 110 | // A.block<3,3>(3,6) = -_delta_R*skew(acc)*dt; 111 | // A.block<3,3>(0,6) = -0.5*_delta_R*skew(acc)*dt2; 112 | // A.block<3,3>(0,3) = I3x3*dt; 113 | // MatrixXd Bg = MatrixXd::Zero(9,3); 114 | // Bg.block<3,3>(6,0) = Jr*dt; 115 | // MatrixXd Ca = MatrixXd::Zero(9,3); 116 | // Ca.block<3,3>(3,0) = _delta_R*dt; 117 | // Ca.block<3,3>(0,0) = 0.5*_delta_R*dt2; 118 | // _cov_P_V_Phi = A*_cov_P_V_Phi*A.transpose() + 119 | // Bg*IMUData::getGyrMeasCov*Bg.transpose() + 120 | // Ca*IMUData::getAccMeasCov()*Ca.transpose(); 121 | 122 | // delta time 123 | _delta_time += dt; 124 | 125 | } 126 | 127 | } -------------------------------------------------------------------------------- /src/FullSystem/IMUPreintegrator.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_IMUPREINTEGRATOR_H 2 | #define SLAM_IMUPREINTEGRATOR_H 3 | 4 | #include "util/NumType.h" 5 | #include "sophus/se3.hpp" 6 | #include "sophus/so3.hpp" 7 | #include 8 | namespace dso 9 | { 10 | 11 | class IMUPreintegrator{ 12 | public: 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | 15 | IMUPreintegrator(); 16 | IMUPreintegrator(const IMUPreintegrator& pre); 17 | 18 | // reset to initial state 19 | void reset(); 20 | 21 | // incrementally update 1)delta measurements, 2)jacobians, 3)covariance matrix 22 | void update(const Vec3& omega, const Vec3& acc, const double& dt); 23 | 24 | // delta measurements, position/velocity/rotation(matrix) 25 | inline Eigen::Vector3d getDeltaP() const // P_k+1 = P_k + V_k*dt + R_k*a_k*dt*dt/2 26 | { 27 | return _delta_P; 28 | } 29 | inline Eigen::Vector3d getDeltaV() const // V_k+1 = V_k + R_k*a_k*dt 30 | { 31 | return _delta_V; 32 | } 33 | inline Eigen::Matrix3d getDeltaR() const // R_k+1 = R_k*exp(w_k*dt). NOTE: Rwc, Rwc'=Rwc*[w_body]x 34 | { 35 | return _delta_R; 36 | } 37 | 38 | // jacobian of delta measurements w.r.t bias of gyro/acc 39 | inline Eigen::Matrix3d getJPBiasg() const // position / gyro 40 | { 41 | return _J_P_Biasg; 42 | } 43 | inline Eigen::Matrix3d getJPBiasa() const // position / acc 44 | { 45 | return _J_P_Biasa; 46 | } 47 | inline Eigen::Matrix3d getJVBiasg() const // velocity / gyro 48 | { 49 | return _J_V_Biasg; 50 | } 51 | inline Eigen::Matrix3d getJVBiasa() const // velocity / acc 52 | { 53 | return _J_V_Biasa; 54 | } 55 | inline Eigen::Matrix3d getJRBiasg() const // rotation / gyro 56 | { 57 | return _J_R_Biasg; 58 | } 59 | 60 | // noise covariance propagation of delta measurements 61 | // note: the order is rotation-velocity-position here 62 | inline Mat99 getCovPVPhi() const 63 | { 64 | return _cov_P_V_Phi; 65 | } 66 | 67 | inline double getDeltaTime() const { 68 | return _delta_time; 69 | } 70 | 71 | // skew-symmetric matrix 72 | static Mat33 skew(const Vec3& v) 73 | { 74 | return Sophus::SO3::hat( v ); 75 | } 76 | 77 | // exponential map from Vec3 to mat3x3 (Rodrigues formula) 78 | static Mat33 Expmap(const Vec3& v) 79 | { 80 | return Sophus::SO3::exp(v).matrix(); 81 | } 82 | 83 | // right jacobian of SO(3) 84 | static Mat33 JacobianR(const Vec3& w) 85 | { 86 | Mat33 Jr = Mat33::Identity(); 87 | double theta = w.norm(); 88 | if(theta<0.00001) 89 | { 90 | return Jr;// = Matrix3d::Identity(); 91 | } 92 | else 93 | { 94 | Vec3 k = w.normalized(); // k - unit direction vector of w 95 | Mat33 K = skew(k); 96 | // Jr = Mat33::Identity() 97 | // - (1-cos(theta))/theta*K 98 | // + (1-sin(theta)/theta)*K*K; 99 | Jr = sin(theta)/theta*Mat33::Identity()+(1-sin(theta)/theta)*k*k.transpose()-(1-cos(theta))/theta*K; 100 | } 101 | return Jr; 102 | } 103 | 104 | static Mat33 JacobianRInv(const Vec3& w) 105 | { 106 | Mat33 Jrinv = Mat33::Identity(); 107 | double theta = w.norm(); 108 | 109 | // very small angle 110 | if(theta < 0.00001) 111 | { 112 | return Jrinv; 113 | } 114 | else 115 | { 116 | Vec3 k = w.normalized(); // k - unit direction vector of w 117 | Mat33 K = Sophus::SO3::hat(k); 118 | // Jrinv = Mat33::Identity() 119 | // + 0.5*Sophus::SO3::hat(w) 120 | // + ( 1.0 - (1.0+cos(theta))*theta / (2.0*sin(theta)) ) *K*K; 121 | double cot = cos(theta/2)/sin(theta/2); 122 | Jrinv = theta/2*cot*Mat33::Identity()+(1-theta/2*cot)*k*k.transpose()+theta/2*K; 123 | } 124 | 125 | return Jrinv; 126 | } 127 | 128 | // left jacobian of SO(3), Jl(x) = Jr(-x) 129 | static Mat33 JacobianL(const Vec3& w) 130 | { 131 | return JacobianR(-w); 132 | } 133 | // left jacobian inverse 134 | static Mat33 JacobianLInv(const Vec3& w) 135 | { 136 | return JacobianRInv(-w); 137 | } 138 | 139 | 140 | inline Sophus::Quaterniond normalizeRotationQ(const Sophus::Quaterniond& r) 141 | { 142 | Sophus::Quaterniond _r(r); 143 | if (_r.w()<0) 144 | { 145 | _r.coeffs() *= -1; 146 | } 147 | return _r.normalized(); 148 | } 149 | 150 | inline Mat33 normalizeRotationM (const Mat33& R) 151 | { 152 | Sophus::Quaterniond qr(R); 153 | return normalizeRotationQ(qr).toRotationMatrix(); 154 | } 155 | private: 156 | /* 157 | * NOTE: 158 | * don't add pointer as member variable. 159 | * operator = is used in g2o 160 | */ 161 | 162 | // delta measurements, position/velocity/rotation(matrix) 163 | Eigen::Vector3d _delta_P; // P_k+1 = P_k + V_k*dt + R_k*a_k*dt*dt/2 164 | Eigen::Vector3d _delta_V; // V_k+1 = V_k + R_k*a_k*dt 165 | Eigen::Matrix3d _delta_R; // R_k+1 = R_k*exp(w_k*dt). note: Rwc, Rwc'=Rwc*[w_body]x 166 | 167 | // jacobian of delta measurements w.r.t bias of gyro/acc 168 | Eigen::Matrix3d _J_P_Biasg; // position / gyro 169 | Eigen::Matrix3d _J_P_Biasa; // position / acc 170 | Eigen::Matrix3d _J_V_Biasg; // velocity / gyro 171 | Eigen::Matrix3d _J_V_Biasa; // velocity / acc 172 | Eigen::Matrix3d _J_R_Biasg; // rotation / gyro 173 | 174 | // noise covariance propagation of delta measurements 175 | Mat99 _cov_P_V_Phi; 176 | 177 | double _delta_time; 178 | 179 | }; 180 | } 181 | 182 | 183 | #endif -------------------------------------------------------------------------------- /src/FullSystem/ImmaturePoint.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 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 | float u_stereo, v_stereo; // u, v used to do static stereo matching 73 | FrameHessian* host; 74 | int idxInImmaturePoints; 75 | 76 | float quality; 77 | 78 | float my_type; 79 | 80 | float idepth_min; 81 | float idepth_max; 82 | float idepth_min_stereo; // idepth_min used to do static matching 83 | float idepth_max_stereo; // idepth_max used to do static matching 84 | float idepth_stereo; 85 | ImmaturePoint(int u_, int v_, FrameHessian* host_, float type, CalibHessian* HCalib); 86 | ImmaturePoint(float u_, float v_, FrameHessian* host_, CalibHessian* HCalib); 87 | ~ImmaturePoint(); 88 | 89 | ImmaturePointStatus traceOn(FrameHessian* frame, const Mat33f &hostToFrame_KRKi, const Vec3f &hostToFrame_Kt, const Vec2f &hostToFrame_affine, CalibHessian* HCalib, bool debugPrint=false); 90 | ImmaturePointStatus traceStereo(FrameHessian* frame, CalibHessian* HCalib, bool mode_right); 91 | 92 | ImmaturePointStatus lastTraceStatus; 93 | Vec2f lastTraceUV; 94 | float lastTracePixelInterval; 95 | 96 | float idepth_GT; 97 | 98 | double linearizeResidual( 99 | CalibHessian * HCalib, const float outlierTHSlack, 100 | ImmaturePointTemporaryResidual* tmpRes, 101 | float &Hdd, float &bd, 102 | float idepth); 103 | float getdPixdd( 104 | CalibHessian * HCalib, 105 | ImmaturePointTemporaryResidual* tmpRes, 106 | float idepth); 107 | 108 | float calcResidual( 109 | CalibHessian * HCalib, const float outlierTHSlack, 110 | ImmaturePointTemporaryResidual* tmpRes, 111 | float idepth); 112 | 113 | private: 114 | }; 115 | 116 | } 117 | 118 | -------------------------------------------------------------------------------- /src/FullSystem/PixelSelector.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 | 31 | 32 | 33 | namespace dso 34 | { 35 | 36 | 37 | const float minUseGrad_pixsel = 10; 38 | 39 | 40 | template 41 | inline int gridMaxSelection(Eigen::Vector3f* grads, bool* map_out, int w, int h, float THFac) 42 | { 43 | 44 | memset(map_out, 0, sizeof(bool)*w*h); 45 | 46 | int numGood = 0; 47 | for(int y=1;y().squaredNorm(); 65 | float TH = THFac*minUseGrad_pixsel * (0.75f); 66 | 67 | if(sqgd > TH*TH) 68 | { 69 | float agx = fabs((float)g[1]); 70 | if(agx > bestXX) {bestXX=agx; bestXXID=idx;} 71 | 72 | float agy = fabs((float)g[2]); 73 | if(agy > bestYY) {bestYY=agy; bestYYID=idx;} 74 | 75 | float gxpy = fabs((float)(g[1]-g[2])); 76 | if(gxpy > bestXY) {bestXY=gxpy; bestXYID=idx;} 77 | 78 | float gxmy = fabs((float)(g[1]+g[2])); 79 | if(gxmy > bestYX) {bestYX=gxmy; bestYXID=idx;} 80 | } 81 | } 82 | 83 | bool* map0 = map_out+x+y*w; 84 | 85 | if(bestXXID>=0) 86 | { 87 | if(!map0[bestXXID]) 88 | numGood++; 89 | map0[bestXXID] = true; 90 | 91 | } 92 | if(bestYYID>=0) 93 | { 94 | if(!map0[bestYYID]) 95 | numGood++; 96 | map0[bestYYID] = true; 97 | 98 | } 99 | if(bestXYID>=0) 100 | { 101 | if(!map0[bestXYID]) 102 | numGood++; 103 | map0[bestXYID] = true; 104 | 105 | } 106 | if(bestYXID>=0) 107 | { 108 | if(!map0[bestYXID]) 109 | numGood++; 110 | map0[bestYXID] = true; 111 | 112 | } 113 | } 114 | } 115 | 116 | return numGood; 117 | } 118 | 119 | //根据像素自身的梯度来选点 120 | inline int gridMaxSelection(Eigen::Vector3f* grads, bool* map_out, int w, int h, int pot, float THFac) 121 | { 122 | 123 | memset(map_out, 0, sizeof(bool)*w*h); 124 | 125 | int numGood = 0; 126 | for(int y=1;y().squaredNorm(); 144 | float TH = THFac*minUseGrad_pixsel * (0.75f); 145 | 146 | if(sqgd > TH*TH) 147 | { 148 | float agx = fabs((float)g[1]); 149 | if(agx > bestXX) {bestXX=agx; bestXXID=idx;} 150 | 151 | float agy = fabs((float)g[2]); 152 | if(agy > bestYY) {bestYY=agy; bestYYID=idx;} 153 | 154 | float gxpy = fabs((float)(g[1]-g[2])); 155 | if(gxpy > bestXY) {bestXY=gxpy; bestXYID=idx;} 156 | 157 | float gxmy = fabs((float)(g[1]+g[2])); 158 | if(gxmy > bestYX) {bestYX=gxmy; bestYXID=idx;} 159 | } 160 | } 161 | 162 | bool* map0 = map_out+x+y*w; 163 | 164 | if(bestXXID>=0) 165 | { 166 | if(!map0[bestXXID]) 167 | numGood++; 168 | map0[bestXXID] = true; 169 | 170 | } 171 | if(bestYYID>=0) 172 | { 173 | if(!map0[bestYYID]) 174 | numGood++; 175 | map0[bestYYID] = true; 176 | 177 | } 178 | if(bestXYID>=0) 179 | { 180 | if(!map0[bestXYID]) 181 | numGood++; 182 | map0[bestXYID] = true; 183 | 184 | } 185 | if(bestYXID>=0) 186 | { 187 | if(!map0[bestYXID]) 188 | numGood++; 189 | map0[bestYXID] = true; 190 | 191 | } 192 | } 193 | } 194 | 195 | return numGood; 196 | } 197 | 198 | 199 | inline int makePixelStatus(Eigen::Vector3f* grads, bool* map, int w, int h, float desiredDensity, int recsLeft=5, float THFac = 1) 200 | { 201 | if(sparsityFactor < 1) sparsityFactor = 1; 202 | 203 | int numGoodPoints; 204 | 205 | 206 | if(sparsityFactor==1) numGoodPoints = gridMaxSelection<1>(grads, map, w, h, THFac); 207 | else if(sparsityFactor==2) numGoodPoints = gridMaxSelection<2>(grads, map, w, h, THFac); 208 | else if(sparsityFactor==3) numGoodPoints = gridMaxSelection<3>(grads, map, w, h, THFac); 209 | else if(sparsityFactor==4) numGoodPoints = gridMaxSelection<4>(grads, map, w, h, THFac); 210 | else if(sparsityFactor==5) numGoodPoints = gridMaxSelection<5>(grads, map, w, h, THFac); 211 | else if(sparsityFactor==6) numGoodPoints = gridMaxSelection<6>(grads, map, w, h, THFac); 212 | else if(sparsityFactor==7) numGoodPoints = gridMaxSelection<7>(grads, map, w, h, THFac); 213 | else if(sparsityFactor==8) numGoodPoints = gridMaxSelection<8>(grads, map, w, h, THFac); 214 | else if(sparsityFactor==9) numGoodPoints = gridMaxSelection<9>(grads, map, w, h, THFac); 215 | else if(sparsityFactor==10) numGoodPoints = gridMaxSelection<10>(grads, map, w, h, THFac); 216 | else if(sparsityFactor==11) numGoodPoints = gridMaxSelection<11>(grads, map, w, h, THFac); 217 | else numGoodPoints = gridMaxSelection(grads, map, w, h, sparsityFactor, THFac); 218 | 219 | 220 | /* 221 | * #points is approximately proportional to sparsityFactor^2. 222 | */ 223 | 224 | float quotia = numGoodPoints / (float)(desiredDensity); 225 | 226 | int newSparsity = (sparsityFactor * sqrtf(quotia))+0.7f; 227 | 228 | 229 | if(newSparsity < 1) newSparsity=1; 230 | 231 | 232 | float oldTHFac = THFac; 233 | if(newSparsity==1 && sparsityFactor==1) THFac = 0.5; 234 | 235 | 236 | if((abs(newSparsity-sparsityFactor) < 1 && THFac==oldTHFac) || 237 | ( quotia > 0.8 && 1.0f / quotia > 0.8) || 238 | recsLeft == 0) 239 | { 240 | 241 | // printf(" \n"); 242 | //all good 243 | sparsityFactor = newSparsity; 244 | return numGoodPoints; 245 | } 246 | else 247 | { 248 | // printf(" -> re-evaluate! \n"); 249 | // re-evaluate. 250 | sparsityFactor = newSparsity; 251 | return makePixelStatus(grads, map, w,h, desiredDensity, recsLeft-1, THFac); 252 | } 253 | } 254 | 255 | } 256 | 257 | -------------------------------------------------------------------------------- /src/FullSystem/PixelSelector2.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | #include "util/NumType.h" 28 | 29 | namespace dso 30 | { 31 | 32 | enum PixelSelectorStatus {PIXSEL_VOID=0, PIXSEL_1, PIXSEL_2, PIXSEL_3}; 33 | 34 | 35 | class FrameHessian; 36 | 37 | class PixelSelector 38 | { 39 | public: 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 41 | int makeMaps( 42 | const FrameHessian* const fh, 43 | float* map_out, float density, int recursionsLeft=1, bool plot=false, float thFactor=1); 44 | 45 | PixelSelector(int w, int h); 46 | ~PixelSelector(); 47 | int currentPotential; 48 | 49 | 50 | bool allowFast; 51 | void makeHists(const FrameHessian* const fh); 52 | private: 53 | 54 | Eigen::Vector3i select(const FrameHessian* const fh, 55 | float* map_out, int pot, float thFactor=1); 56 | 57 | 58 | unsigned char* randomPattern; 59 | 60 | 61 | int* gradHist; 62 | float* ths; 63 | float* thsSmoothed; 64 | int thsStep; 65 | const FrameHessian* gradHistFrame; 66 | }; 67 | 68 | 69 | 70 | 71 | } 72 | 73 | -------------------------------------------------------------------------------- /src/FullSystem/ResidualProjections.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 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 | bool stereoResidualFlag = false; 62 | 63 | static int instanceCounter; 64 | 65 | 66 | ResState state_state; 67 | double state_energy; 68 | ResState state_NewState; 69 | double state_NewEnergy; 70 | double state_NewEnergyWithOutlier; 71 | 72 | 73 | void setState(ResState s) {state_state = s;} 74 | 75 | 76 | PointHessian* point; 77 | FrameHessian* host; 78 | FrameHessian* target; 79 | RawResidualJacobian* J; 80 | 81 | 82 | bool isNew; 83 | 84 | 85 | Eigen::Vector2f projectedTo[MAX_RES_PER_POINT]; 86 | Vec3f centerProjectedTo; 87 | 88 | ~PointFrameResidual(); 89 | PointFrameResidual(); 90 | PointFrameResidual(PointHessian* point_, FrameHessian* host_, FrameHessian* target_); 91 | double linearize(CalibHessian* HCalib); 92 | double linearizeStereo(CalibHessian* HCalib); 93 | 94 | 95 | void resetOOB() 96 | { 97 | state_NewEnergy = state_energy = 0; 98 | state_NewState = ResState::OUTLIER; 99 | 100 | setState(ResState::IN); 101 | }; 102 | void applyRes( bool copyJacobians); 103 | 104 | void debugPlot(); 105 | 106 | void printRows(std::vector &v, VecX &r, int nFrames, int nPoints, int M, int res); 107 | }; 108 | } 109 | 110 | -------------------------------------------------------------------------------- /src/IOWrapper/ImageDisplay.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 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/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/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/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/IOWrapper/OpenCV/ImageDisplay_OpenCV.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 | #include 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | #include "util/settings.h" 36 | 37 | namespace dso 38 | { 39 | 40 | 41 | namespace IOWrap 42 | { 43 | 44 | std::unordered_set openWindows; 45 | boost::mutex openCVdisplayMutex; 46 | 47 | 48 | 49 | void displayImage(const char* windowName, const cv::Mat& image, bool autoSize) 50 | { 51 | if(disableAllDisplay) return; 52 | 53 | boost::unique_lock lock(openCVdisplayMutex); 54 | if(!autoSize) 55 | { 56 | if(openWindows.find(windowName) == openWindows.end()) 57 | { 58 | cv::namedWindow(windowName, cv::WINDOW_NORMAL); 59 | cv::resizeWindow(windowName, image.cols, image.rows); 60 | openWindows.insert(windowName); 61 | } 62 | } 63 | cv::imshow(windowName, image); 64 | } 65 | 66 | 67 | void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) 68 | { 69 | if(disableAllDisplay) return; 70 | if(images.size() == 0) return; 71 | 72 | // get dimensions. 73 | int w = images[0]->cols; 74 | int h = images[0]->rows; 75 | 76 | int num = std::max((int)setting_maxFrames, (int)images.size()); 77 | 78 | // get optimal dimensions. 79 | int bestCC = 0; 80 | float bestLoss = 1e10; 81 | for(int cc=1;cc<10;cc++) 82 | { 83 | int ww = w * cc; 84 | int hh = h * ((num+cc-1)/cc); 85 | 86 | 87 | float wLoss = ww/16.0f; 88 | float hLoss = hh/10.0f; 89 | float loss = std::max(wLoss, hLoss); 90 | 91 | if(loss < bestLoss) 92 | { 93 | bestLoss = loss; 94 | bestCC = cc; 95 | } 96 | } 97 | 98 | int bestRC = ((num+bestCC-1)/bestCC); 99 | if(cc != 0) 100 | { 101 | bestCC = cc; 102 | bestRC= rc; 103 | } 104 | cv::Mat stitch = cv::Mat(bestRC*h, bestCC*w, images[0]->type()); 105 | stitch.setTo(0); 106 | for(int i=0;i<(int)images.size() && i < bestCC*bestRC;i++) 107 | { 108 | int c = i%bestCC; 109 | int r = i/bestCC; 110 | 111 | cv::Mat roi = stitch(cv::Rect(c*w, r*h, w,h)); 112 | images[i]->copyTo(roi); 113 | } 114 | displayImage(windowName, stitch, false); 115 | } 116 | 117 | 118 | 119 | void displayImage(const char* windowName, const MinimalImageB* img, bool autoSize) 120 | { 121 | displayImage(windowName, cv::Mat(img->h, img->w, CV_8U, img->data), autoSize); 122 | } 123 | void displayImage(const char* windowName, const MinimalImageB3* img, bool autoSize) 124 | { 125 | displayImage(windowName, cv::Mat(img->h, img->w, CV_8UC3, img->data), autoSize); 126 | } 127 | void displayImage(const char* windowName, const MinimalImageF* img, bool autoSize) 128 | { 129 | displayImage(windowName, cv::Mat(img->h, img->w, CV_32F, img->data)*(1/254.0f), autoSize); 130 | } 131 | void displayImage(const char* windowName, const MinimalImageF3* img, bool autoSize) 132 | { 133 | displayImage(windowName, cv::Mat(img->h, img->w, CV_32FC3, img->data)*(1/254.0f), autoSize); 134 | } 135 | void displayImage(const char* windowName, const MinimalImageB16* img, bool autoSize) 136 | { 137 | displayImage(windowName, cv::Mat(img->h, img->w, CV_16U, img->data), autoSize); 138 | } 139 | 140 | 141 | void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) 142 | { 143 | std::vector imagesCV; 144 | for(size_t i=0; i < images.size();i++) 145 | imagesCV.push_back(new cv::Mat(images[i]->h, images[i]->w, CV_8U, images[i]->data)); 146 | displayImageStitch(windowName, imagesCV, cc, rc); 147 | for(size_t i=0; i < images.size();i++) 148 | delete imagesCV[i]; 149 | } 150 | void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) 151 | { 152 | std::vector imagesCV; 153 | for(size_t i=0; i < images.size();i++) 154 | imagesCV.push_back(new cv::Mat(images[i]->h, images[i]->w, CV_8UC3, images[i]->data)); 155 | displayImageStitch(windowName, imagesCV, cc, rc); 156 | for(size_t i=0; i < images.size();i++) 157 | delete imagesCV[i]; 158 | } 159 | void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) 160 | { 161 | std::vector imagesCV; 162 | for(size_t i=0; i < images.size();i++) 163 | imagesCV.push_back(new cv::Mat(images[i]->h, images[i]->w, CV_32F, images[i]->data)); 164 | displayImageStitch(windowName, imagesCV, cc, rc); 165 | for(size_t i=0; i < images.size();i++) 166 | delete imagesCV[i]; 167 | } 168 | void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) 169 | { 170 | std::vector imagesCV; 171 | for(size_t i=0; i < images.size();i++) 172 | imagesCV.push_back(new cv::Mat(images[i]->h, images[i]->w, CV_32FC3, images[i]->data)); 173 | displayImageStitch(windowName, imagesCV, cc, rc); 174 | for(size_t i=0; i < images.size();i++) 175 | delete imagesCV[i]; 176 | } 177 | 178 | 179 | 180 | int waitKey(int milliseconds) 181 | { 182 | if(disableAllDisplay) return 0; 183 | 184 | boost::unique_lock lock(openCVdisplayMutex); 185 | return cv::waitKey(milliseconds); 186 | } 187 | 188 | void closeAllWindows() 189 | { 190 | if(disableAllDisplay) return; 191 | boost::unique_lock lock(openCVdisplayMutex); 192 | cv::destroyAllWindows(); 193 | openWindows.clear(); 194 | } 195 | } 196 | 197 | } 198 | -------------------------------------------------------------------------------- /src/IOWrapper/OpenCV/ImageRW_OpenCV.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 | #include 28 | 29 | 30 | namespace dso 31 | { 32 | 33 | namespace IOWrap 34 | { 35 | MinimalImageB* readImageBW_8U(std::string filename) 36 | { 37 | cv::Mat m = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE); 38 | if(m.rows*m.cols==0) 39 | { 40 | printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str()); 41 | return 0; 42 | } 43 | if(m.type() != CV_8U) 44 | { 45 | printf("cv::imread did something strange! this may segfault. \n"); 46 | return 0; 47 | } 48 | MinimalImageB* img = new MinimalImageB(m.cols, m.rows); 49 | memcpy(img->data, m.data, m.rows*m.cols); 50 | return img; 51 | } 52 | 53 | MinimalImageB3* readImageRGB_8U(std::string filename) 54 | { 55 | cv::Mat m = cv::imread(filename, CV_LOAD_IMAGE_COLOR); 56 | if(m.rows*m.cols==0) 57 | { 58 | printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str()); 59 | return 0; 60 | } 61 | if(m.type() != CV_8UC3) 62 | { 63 | printf("cv::imread did something strange! this may segfault. \n"); 64 | return 0; 65 | } 66 | MinimalImageB3* img = new MinimalImageB3(m.cols, m.rows); 67 | memcpy(img->data, m.data, 3*m.rows*m.cols); 68 | return img; 69 | } 70 | 71 | MinimalImage* readImageBW_16U(std::string filename) 72 | { 73 | cv::Mat m = cv::imread(filename, CV_LOAD_IMAGE_UNCHANGED); 74 | if(m.rows*m.cols==0) 75 | { 76 | printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str()); 77 | return 0; 78 | } 79 | if(m.type() != CV_16U) 80 | { 81 | printf("readImageBW_16U called on image that is not a 16bit grayscale image. this may segfault. \n"); 82 | return 0; 83 | } 84 | MinimalImage* img = new MinimalImage(m.cols, m.rows); 85 | memcpy(img->data, m.data, 2*m.rows*m.cols); 86 | return img; 87 | } 88 | 89 | MinimalImageB* readStreamBW_8U(char* data, int numBytes) 90 | { 91 | cv::Mat m = cv::imdecode(cv::Mat(numBytes,1,CV_8U, data), CV_LOAD_IMAGE_GRAYSCALE); 92 | if(m.rows*m.cols==0) 93 | { 94 | printf("cv::imdecode could not read stream (%d bytes)! this may segfault. \n", numBytes); 95 | return 0; 96 | } 97 | if(m.type() != CV_8U) 98 | { 99 | printf("cv::imdecode did something strange! this may segfault. \n"); 100 | return 0; 101 | } 102 | MinimalImageB* img = new MinimalImageB(m.cols, m.rows); 103 | memcpy(img->data, m.data, m.rows*m.cols); 104 | return img; 105 | } 106 | 107 | 108 | 109 | void writeImage(std::string filename, MinimalImageB* img) 110 | { 111 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_8U, img->data)); 112 | } 113 | void writeImage(std::string filename, MinimalImageB3* img) 114 | { 115 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_8UC3, img->data)); 116 | } 117 | void writeImage(std::string filename, MinimalImageF* img) 118 | { 119 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_32F, img->data)); 120 | } 121 | void writeImage(std::string filename, MinimalImageF3* img) 122 | { 123 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_32FC3, img->data)); 124 | } 125 | 126 | } 127 | 128 | } 129 | -------------------------------------------------------------------------------- /src/IOWrapper/OutputWrapper/SampleOutputWrapper.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 "boost/thread.hpp" 27 | #include "util/MinimalImage.h" 28 | #include "IOWrapper/Output3DWrapper.h" 29 | 30 | 31 | 32 | #include "FullSystem/HessianBlocks.h" 33 | #include "util/FrameShell.h" 34 | 35 | namespace dso 36 | { 37 | 38 | class FrameHessian; 39 | class CalibHessian; 40 | class FrameShell; 41 | 42 | 43 | namespace IOWrap 44 | { 45 | 46 | class SampleOutputWrapper : public Output3DWrapper 47 | { 48 | public: 49 | inline SampleOutputWrapper() 50 | { 51 | printf("OUT: Created SampleOutputWrapper\n"); 52 | } 53 | 54 | virtual ~SampleOutputWrapper() 55 | { 56 | printf("OUT: Destroyed SampleOutputWrapper\n"); 57 | } 58 | 59 | virtual void publishGraph(const std::map, Eigen::aligned_allocator>> &connectivity) override 60 | { 61 | printf("OUT: got graph with %d edges\n", (int)connectivity.size()); 62 | 63 | int maxWrite = 5; 64 | 65 | for(const std::pair &p : connectivity) 66 | { 67 | int idHost = p.first>>32; 68 | int idTarget = p.first & ((uint64_t)0xFFFFFFFF); 69 | printf("OUT: Example Edge %d -> %d has %d active and %d marg residuals\n", idHost, idTarget, p.second[0], p.second[1]); 70 | maxWrite--; 71 | if(maxWrite==0) break; 72 | } 73 | } 74 | 75 | 76 | 77 | virtual void publishKeyframes( std::vector &frames, bool final, CalibHessian* HCalib) override 78 | { 79 | for(FrameHessian* f : frames) 80 | { 81 | printf("OUT: KF %d (%s) (id %d, tme %f): %d active, %d marginalized, %d immature points. CameraToWorld:\n", 82 | f->frameID, 83 | final ? "final" : "non-final", 84 | f->shell->incoming_id, 85 | f->shell->timestamp, 86 | (int)f->pointHessians.size(), (int)f->pointHessiansMarginalized.size(), (int)f->immaturePoints.size()); 87 | std::cout << f->shell->camToWorld.matrix3x4() << "\n"; 88 | 89 | 90 | int maxWrite = 5; 91 | for(PointHessian* p : f->pointHessians) 92 | { 93 | printf("OUT: Example Point x=%.1f, y=%.1f, idepth=%f, idepth std.dev. %f, %d inlier-residuals\n", 94 | p->u, p->v, p->idepth_scaled, sqrt(1.0f / p->idepth_hessian), p->numGoodResiduals ); 95 | maxWrite--; 96 | if(maxWrite==0) break; 97 | } 98 | } 99 | } 100 | 101 | virtual void publishCamPose(FrameShell* frame, CalibHessian* HCalib) override 102 | { 103 | printf("OUT: Current Frame %d (time %f, internal ID %d). CameraToWorld:\n", 104 | frame->incoming_id, 105 | frame->timestamp, 106 | frame->id); 107 | std::cout << frame->camToWorld.matrix3x4() << "\n"; 108 | } 109 | 110 | 111 | virtual void pushLiveFrame(FrameHessian* image) override 112 | { 113 | // can be used to get the raw image / intensity pyramid. 114 | } 115 | 116 | virtual void pushDepthImage(MinimalImageB3* image) override 117 | { 118 | // can be used to get the raw image with depth overlay. 119 | } 120 | virtual bool needPushDepthImage() override 121 | { 122 | return false; 123 | } 124 | 125 | virtual void pushDepthImageFloat(MinimalImageF* image, FrameHessian* KF ) override 126 | { 127 | printf("OUT: Predicted depth for KF %d (id %d, time %f, internal frame-ID %d). CameraToWorld:\n", 128 | KF->frameID, 129 | KF->shell->incoming_id, 130 | KF->shell->timestamp, 131 | KF->shell->id); 132 | std::cout << KF->shell->camToWorld.matrix3x4() << "\n"; 133 | 134 | int maxWrite = 5; 135 | for(int y=0;yh;y++) 136 | { 137 | for(int x=0;xw;x++) 138 | { 139 | if(image->at(x,y) <= 0) continue; 140 | 141 | printf("OUT: Example Idepth at pixel (%d,%d): %f.\n", x,y,image->at(x,y)); 142 | maxWrite--; 143 | if(maxWrite==0) break; 144 | } 145 | if(maxWrite==0) break; 146 | } 147 | } 148 | 149 | 150 | }; 151 | 152 | 153 | 154 | } 155 | 156 | 157 | 158 | } 159 | -------------------------------------------------------------------------------- /src/IOWrapper/Pangolin/KeyFrameDisplay.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 | #undef Success 28 | #include 29 | #include "util/NumType.h" 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | namespace dso 36 | { 37 | class CalibHessian; 38 | class FrameHessian; 39 | class FrameShell; 40 | 41 | namespace IOWrap 42 | { 43 | 44 | template 45 | struct InputPointSparse 46 | { 47 | float u; 48 | float v; 49 | float idpeth; 50 | float idepth_hessian; 51 | float relObsBaseline; 52 | int numGoodRes; 53 | unsigned char color[ppp]; 54 | unsigned char status; 55 | }; 56 | 57 | struct MyVertex 58 | { 59 | float point[3]; 60 | unsigned char color[4]; 61 | }; 62 | 63 | // stores a pointcloud associated to a Keyframe. 64 | class KeyFrameDisplay 65 | { 66 | 67 | public: 68 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 69 | KeyFrameDisplay(); 70 | ~KeyFrameDisplay(); 71 | 72 | // copies points from KF over to internal buffer, 73 | // keeping some additional information so we can render it differently. 74 | void setFromKF(FrameHessian* fh, CalibHessian* HCalib); 75 | 76 | // copies points from KF over to internal buffer, 77 | // keeping some additional information so we can render it differently. 78 | void setFromF(FrameShell* fs, CalibHessian* HCalib); 79 | 80 | // copies & filters internal data to GL buffer for rendering. if nothing to do: does nothing. 81 | bool refreshPC(bool canRefresh, float scaledTH, float absTH, int mode, float minBS, int sparsity); 82 | 83 | // renders cam & pointcloud. 84 | void drawCam(float lineWidth = 1, float* color = 0, float sizeFactor=1); 85 | void drawPC(float pointSize); 86 | 87 | int id; 88 | bool active; 89 | SE3 camToWorld; 90 | 91 | inline bool operator < (const KeyFrameDisplay& other) const 92 | { 93 | return (id < other.id); 94 | } 95 | 96 | 97 | private: 98 | float fx,fy,cx,cy; 99 | float fxi,fyi,cxi,cyi; 100 | int width, height; 101 | 102 | float my_scaledTH, my_absTH, my_scale; 103 | int my_sparsifyFactor; 104 | int my_displayMode; 105 | float my_minRelBS; 106 | bool needRefresh; 107 | 108 | 109 | int numSparsePoints; 110 | int numSparseBufferSize; 111 | InputPointSparse* originalInputSparse; 112 | 113 | 114 | bool bufferValid; 115 | int numGLBufferPoints; 116 | int numGLBufferGoodPoints; 117 | pangolin::GlBuffer vertexBuffer; 118 | pangolin::GlBuffer colorBuffer; 119 | }; 120 | 121 | } 122 | } 123 | 124 | -------------------------------------------------------------------------------- /src/IOWrapper/Pangolin/PangolinDSOViewer.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 "boost/thread.hpp" 28 | #include "util/MinimalImage.h" 29 | #include "IOWrapper/Output3DWrapper.h" 30 | #include 31 | #include 32 | 33 | 34 | namespace dso 35 | { 36 | 37 | class FrameHessian; 38 | class CalibHessian; 39 | class FrameShell; 40 | 41 | 42 | namespace IOWrap 43 | { 44 | 45 | class KeyFrameDisplay; 46 | 47 | struct GraphConnection 48 | { 49 | KeyFrameDisplay* from; 50 | KeyFrameDisplay* to; 51 | int fwdMarg, bwdMarg, fwdAct, bwdAct; 52 | }; 53 | 54 | 55 | class PangolinDSOViewer : public Output3DWrapper 56 | { 57 | public: 58 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 59 | PangolinDSOViewer(int w, int h, bool startRunThread=true); 60 | virtual ~PangolinDSOViewer(); 61 | 62 | void run(); 63 | void close(); 64 | 65 | void addImageToDisplay(std::string name, MinimalImageB3* image); 66 | void clearAllImagesToDisplay(); 67 | 68 | 69 | // ==================== Output3DWrapper Functionality ====================== 70 | virtual void publishGraph(const std::map, Eigen::aligned_allocator>> &connectivity) override; 71 | virtual void publishKeyframes( std::vector &frames, bool final, CalibHessian* HCalib) override; 72 | virtual void publishCamPose(FrameShell* frame, CalibHessian* HCalib) override; 73 | 74 | 75 | virtual void pushLiveFrame(FrameHessian* image) override; 76 | virtual void pushDepthImage(MinimalImageB3* image) override; 77 | virtual bool needPushDepthImage() override; 78 | 79 | virtual void join() override; 80 | 81 | virtual void reset() override; 82 | private: 83 | 84 | bool needReset; 85 | void reset_internal(); 86 | void drawConstraints(); 87 | 88 | boost::thread runThread; 89 | bool running; 90 | int w,h; 91 | 92 | 93 | 94 | // images rendering 95 | boost::mutex openImagesMutex; 96 | MinimalImageB3* internalVideoImg; 97 | MinimalImageB3* internalKFImg; 98 | MinimalImageB3* internalResImg; 99 | bool videoImgChanged, kfImgChanged, resImgChanged; 100 | 101 | 102 | 103 | // 3D model rendering 104 | boost::mutex model3DMutex; 105 | KeyFrameDisplay* currentCam; 106 | std::vector keyframes; 107 | std::vector> allFramePoses; 108 | std::map keyframesByKFID; 109 | std::vector> connections; 110 | 111 | 112 | 113 | // render settings 114 | bool settings_showKFCameras; 115 | bool settings_showCurrentCamera; 116 | bool settings_showTrajectory; 117 | bool settings_showFullTrajectory; 118 | bool settings_showActiveConstraints; 119 | bool settings_showAllConstraints; 120 | bool settings_showGroundTruth; 121 | 122 | float settings_scaledVarTH; 123 | float settings_absVarTH; 124 | int settings_pointCloudMode; 125 | float settings_minRelBS; 126 | int settings_sparsity; 127 | 128 | 129 | // timings 130 | struct timeval last_track; 131 | struct timeval last_map; 132 | 133 | 134 | std::deque lastNTrackingMs; 135 | std::deque lastNMappingMs; 136 | }; 137 | 138 | 139 | 140 | } 141 | 142 | 143 | 144 | } 145 | -------------------------------------------------------------------------------- /src/OptimizationBackend/AccumulatedSCHessian.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/AccumulatedSCHessian.h" 26 | #include "OptimizationBackend/EnergyFunctional.h" 27 | #include "OptimizationBackend/EnergyFunctionalStructs.h" 28 | 29 | #include "FullSystem/HessianBlocks.h" 30 | 31 | namespace dso 32 | { 33 | 34 | void AccumulatedSCHessianSSE::addPoint(EFPoint* p, bool shiftPriorToZero, int tid) 35 | { 36 | int ngoodres = 0; 37 | for(EFResidual* r : p->residualsAll) if(r->isActive()) ngoodres++; 38 | if(ngoodres==0) 39 | { 40 | p->HdiF=0; 41 | p->bdSumF=0; 42 | p->data->idepth_hessian=0; 43 | p->data->maxRelBaseline=0; 44 | return; 45 | } 46 | 47 | float H = p->Hdd_accAF+p->Hdd_accLF+p->priorF; 48 | if(H < 1e-10) H = 1e-10; 49 | 50 | p->data->idepth_hessian=H; 51 | 52 | p->HdiF = 1.0 / H; 53 | p->bdSumF = p->bd_accAF + p->bd_accLF; 54 | if(shiftPriorToZero) p->bdSumF += p->priorF*p->deltaF; 55 | VecCf Hcd = p->Hcd_accAF + p->Hcd_accLF; 56 | accHcc[tid].update(Hcd,Hcd,p->HdiF); 57 | accbc[tid].update(Hcd, p->bdSumF * p->HdiF); 58 | 59 | assert(std::isfinite((float)(p->HdiF))); 60 | 61 | int nFrames2 = nframes[tid]*nframes[tid]; 62 | for(EFResidual* r1 : p->residualsAll) 63 | { 64 | if(!r1->isActive()) continue; 65 | int r1ht = r1->hostIDX + r1->targetIDX*nframes[tid]; 66 | 67 | for(EFResidual* r2 : p->residualsAll) 68 | { 69 | if(!r2->isActive()) continue; 70 | 71 | accD[tid][r1ht+r2->targetIDX*nFrames2].update(r1->JpJdF, r2->JpJdF, p->HdiF); 72 | } 73 | 74 | accE[tid][r1ht].update(r1->JpJdF, Hcd, p->HdiF); 75 | accEB[tid][r1ht].update(r1->JpJdF,p->HdiF*p->bdSumF); 76 | } 77 | } 78 | void AccumulatedSCHessianSSE::stitchDoubleInternal( 79 | MatXX* H, VecX* b, EnergyFunctional const * const EF, 80 | int min, int max, Vec10* stats, int tid) 81 | { 82 | int toAggregate = NUM_THREADS; 83 | if(tid == -1) { toAggregate = 1; tid = 0; } // special case: if we dont do multithreading, dont aggregate. 84 | if(min==max) return; 85 | 86 | 87 | int nf = nframes[0]; 88 | int nframes2 = nf*nf; 89 | 90 | for(int k=min;k(); 107 | bp += accEB[tid2][ijIdx].A1m.cast(); 108 | } 109 | 110 | H[tid].block<8,CPARS>(iIdx,0) += EF->adHost[ijIdx] * Hpc; 111 | H[tid].block<8,CPARS>(jIdx,0) += EF->adTarget[ijIdx] * Hpc; 112 | b[tid].segment<8>(iIdx) += EF->adHost[ijIdx] * bp; 113 | b[tid].segment<8>(jIdx) += EF->adTarget[ijIdx] * bp; 114 | 115 | for(int k=0;k(); 128 | } 129 | 130 | H[tid].block<8,8>(iIdx, iIdx) += EF->adHost[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); 131 | H[tid].block<8,8>(jIdx, kIdx) += EF->adTarget[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); 132 | H[tid].block<8,8>(jIdx, iIdx) += EF->adTarget[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); 133 | H[tid].block<8,8>(iIdx, kIdx) += EF->adHost[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); 134 | } 135 | } 136 | 137 | if(min==0) 138 | { 139 | for(int tid2=0;tid2 < toAggregate;tid2++) 140 | { 141 | accHcc[tid2].finish(); 142 | accbc[tid2].finish(); 143 | H[tid].topLeftCorner() += accHcc[tid2].A1m.cast(); 144 | b[tid].head() += accbc[tid2].A1m.cast(); 145 | } 146 | } 147 | 148 | 149 | // // ----- new: copy transposed parts for calibration only. 150 | // for(int h=0;h(0,hIdx).noalias() = H.block<8,4>(hIdx,0).transpose(); 154 | // } 155 | } 156 | 157 | void AccumulatedSCHessianSSE::stitchDouble(MatXX &H, VecX &b, EnergyFunctional const * const EF, int tid) 158 | { 159 | 160 | int nf = nframes[0]; 161 | int nframes2 = nf*nf; 162 | 163 | H = MatXX::Zero(nf*8+CPARS, nf*8+CPARS); 164 | b = VecX::Zero(nf*8+CPARS); 165 | 166 | 167 | for(int i=0;i(); 178 | Vec8 accEBV = accEB[tid][ijIdx].A1m.cast(); 179 | 180 | H.block<8,CPARS>(iIdx,0) += EF->adHost[ijIdx] * accEM; 181 | H.block<8,CPARS>(jIdx,0) += EF->adTarget[ijIdx] * accEM; 182 | 183 | b.segment<8>(iIdx) += EF->adHost[ijIdx] * accEBV; 184 | b.segment<8>(jIdx) += EF->adTarget[ijIdx] * accEBV; 185 | 186 | for(int k=0;k(); 195 | 196 | H.block<8,8>(iIdx, iIdx) += EF->adHost[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); 197 | 198 | H.block<8,8>(jIdx, kIdx) += EF->adTarget[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); 199 | 200 | H.block<8,8>(jIdx, iIdx) += EF->adTarget[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); 201 | 202 | H.block<8,8>(iIdx, kIdx) += EF->adHost[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); 203 | } 204 | } 205 | 206 | accHcc[tid].finish(); 207 | accbc[tid].finish(); 208 | H.topLeftCorner() = accHcc[tid].A1m.cast(); 209 | b.head() = accbc[tid].A1m.cast(); 210 | 211 | // ----- new: copy transposed parts for calibration only. 212 | for(int h=0;h(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose(); 216 | } 217 | } 218 | 219 | } 220 | -------------------------------------------------------------------------------- /src/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/OptimizationBackend/EnergyFunctional.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 "vector" 31 | #include 32 | #include "map" 33 | #include "FullSystem/IMUPreintegrator.h" 34 | 35 | 36 | namespace dso 37 | { 38 | 39 | class PointFrameResidual; 40 | class CalibHessian; 41 | class FrameHessian; 42 | class PointHessian; 43 | 44 | 45 | class EFResidual; 46 | class EFPoint; 47 | class EFFrame; 48 | class EnergyFunctional; 49 | class AccumulatedTopHessian; 50 | class AccumulatedTopHessianSSE; 51 | class AccumulatedSCHessian; 52 | class AccumulatedSCHessianSSE; 53 | 54 | 55 | extern bool EFAdjointsValid; 56 | extern bool EFIndicesValid; 57 | extern bool EFDeltaValid; 58 | 59 | 60 | 61 | class EnergyFunctional { 62 | public: 63 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 64 | friend class EFFrame; 65 | friend class EFPoint; 66 | friend class EFResidual; 67 | friend class AccumulatedTopHessian; 68 | friend class AccumulatedTopHessianSSE; 69 | friend class AccumulatedSCHessian; 70 | friend class AccumulatedSCHessianSSE; 71 | 72 | EnergyFunctional(); 73 | ~EnergyFunctional(); 74 | 75 | 76 | EFResidual* insertResidual(PointFrameResidual* r); 77 | EFFrame* insertFrame(FrameHessian* fh, CalibHessian* Hcalib); 78 | EFPoint* insertPoint(PointHessian* ph); 79 | 80 | void dropResidual(EFResidual* r); 81 | void marginalizeFrame(EFFrame* fh); 82 | void marginalizeFrame_imu(EFFrame* fh); 83 | void removePoint(EFPoint* ph); 84 | 85 | 86 | 87 | void marginalizePointsF(); 88 | void dropPointsF(); 89 | void solveSystemF(int iteration, double lambda, CalibHessian* HCalib); 90 | double calcMEnergyF(); 91 | double calcLEnergyF_MT(); 92 | 93 | 94 | void makeIDX(); 95 | 96 | void setDeltaF(CalibHessian* HCalib); 97 | 98 | void setAdjointsF(CalibHessian* Hcalib); 99 | 100 | std::vector frames; 101 | int nPoints, nFrames, nResiduals; 102 | 103 | MatXX HM; 104 | VecX bM; 105 | 106 | MatXX HM_imu; 107 | VecX bM_imu; 108 | 109 | MatXX HM_bias; 110 | VecX bM_bias; 111 | 112 | MatXX HM_imu_half; 113 | VecX bM_imu_half; 114 | 115 | double s_middle = 1; 116 | double s_last = 1; 117 | double d_now = sqrt(1.1); 118 | double d_half = sqrt(1.1); 119 | bool side_last = true;//for d margin: true: upper s_middle false: below s_middle 120 | 121 | int resInA, resInL, resInM; 122 | MatXX lastHS; 123 | VecX lastbS; 124 | VecX lastX; 125 | std::vector lastNullspaces_forLogging; 126 | std::vector lastNullspaces_pose; 127 | std::vector lastNullspaces_scale; 128 | std::vector lastNullspaces_affA; 129 | std::vector lastNullspaces_affB; 130 | 131 | IndexThreadReduce* red; 132 | 133 | 134 | std::map, 137 | Eigen::aligned_allocator> 138 | > connectivityMap; 139 | 140 | private: 141 | 142 | VecX getStitchedDeltaF() const; 143 | 144 | void resubstituteF_MT(VecX x, CalibHessian* HCalib, bool MT); 145 | void resubstituteFPt(const VecCf &xc, Mat18f* xAd, int min, int max, Vec10* stats, int tid); 146 | 147 | void accumulateAF_MT(MatXX &H, VecX &b, bool MT); 148 | void accumulateLF_MT(MatXX &H, VecX &b, bool MT); 149 | void accumulateSCF_MT(MatXX &H, VecX &b, bool MT); 150 | 151 | void calcLEnergyPt(int min, int max, Vec10* stats, int tid); 152 | 153 | void getIMUHessian(MatXX &H, VecX &b); 154 | 155 | void orthogonalize(VecX* b, MatXX* H); 156 | Mat18f* adHTdeltaF; 157 | 158 | Mat88* adHost; 159 | Mat88* adTarget; 160 | 161 | Mat88f* adHostF; 162 | Mat88f* adTargetF; 163 | 164 | 165 | VecC cPrior; 166 | VecCf cDeltaF; 167 | VecCf cPriorF; 168 | 169 | AccumulatedTopHessianSSE* accSSE_top_L; 170 | AccumulatedTopHessianSSE* accSSE_top_A; 171 | 172 | 173 | AccumulatedSCHessianSSE* accSSE_bot; 174 | 175 | std::vector allPoints; 176 | std::vector allPointsToMarg; 177 | 178 | float currentLambda; 179 | }; 180 | } 181 | 182 | -------------------------------------------------------------------------------- /src/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/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=Vec8f::Zero(); 89 | EIGEN_ALIGN16 VecNRf res_toZeroF; 90 | EIGEN_ALIGN16 Vec8f JpJdF=Vec8f::Zero(); 91 | 92 | 93 | // status. 94 | bool isLinearized; 95 | 96 | // if residual is not OOB & not OUTLIER & should be used during accumulations 97 | bool isActiveAndIsGoodNEW; 98 | inline const bool &isActive() const {return isActiveAndIsGoodNEW;} 99 | }; 100 | 101 | 102 | enum EFPointStatus {PS_GOOD=0, PS_MARGINALIZE, PS_DROP}; 103 | 104 | class EFPoint 105 | { 106 | public: 107 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 108 | EFPoint(PointHessian* d, EFFrame* host_) : data(d),host(host_) 109 | { 110 | takeData(); 111 | stateFlag=EFPointStatus::PS_GOOD; 112 | } 113 | void takeData(); 114 | 115 | PointHessian* data; 116 | 117 | 118 | 119 | float priorF; 120 | float deltaF; 121 | 122 | 123 | // constant info (never changes in-between). 124 | int idxInPoints; 125 | EFFrame* host; 126 | 127 | // contains all residuals. 128 | std::vector residualsAll; 129 | 130 | float bdSumF; 131 | float HdiF=1e-3; 132 | float Hdd_accLF; 133 | VecCf Hcd_accLF; 134 | float bd_accLF; 135 | float Hdd_accAF; 136 | VecCf Hcd_accAF; 137 | float bd_accAF; 138 | 139 | 140 | EFPointStatus stateFlag; 141 | }; 142 | 143 | 144 | 145 | class EFFrame 146 | { 147 | public: 148 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 149 | EFFrame(FrameHessian* d) : data(d) 150 | { 151 | takeData(); 152 | } 153 | void takeData(); 154 | 155 | 156 | Vec8 prior; // prior hessian (diagonal) 157 | Vec8 delta_prior; // = state-state_prior (E_prior = (delta_prior)' * diag(prior) * (delta_prior) 158 | Vec8 delta; // state - state_zero. 159 | 160 | 161 | 162 | std::vector points; 163 | FrameHessian* data; 164 | int idx; // idx in frames. 165 | 166 | int frameID; 167 | bool m_flag = false; 168 | }; 169 | 170 | } 171 | 172 | -------------------------------------------------------------------------------- /src/OptimizationBackend/RawResidualJacobian.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 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 | EIGEN_ALIGN16 VecNRf resF; 37 | 38 | // the two rows of d[x,y]/d[xi]. 39 | EIGEN_ALIGN16 Vec6f Jpdxi[2]; // 2x6 40 | 41 | // the two rows of d[x,y]/d[C]. 42 | EIGEN_ALIGN16 VecCf Jpdc[2]; // 2x4 43 | 44 | // the two rows of d[x,y]/d[idepth]. 45 | EIGEN_ALIGN16 Vec2f Jpdd; // 2x1 46 | 47 | // the two columns of d[r]/d[x,y]. 48 | EIGEN_ALIGN16 VecNRf JIdx[2]; // 9x2 49 | 50 | // = the two columns of d[r] / d[ab] 51 | EIGEN_ALIGN16 VecNRf JabF[2]; // 9x2 52 | 53 | 54 | // = JIdx^T * JIdx (inner product). Only as a shorthand. 55 | EIGEN_ALIGN16 Mat22f JIdx2; // 2x2 56 | // = Jab^T * JIdx (inner product). Only as a shorthand. 57 | EIGEN_ALIGN16 Mat22f JabJIdx; // 2x2 58 | // = Jab^T * Jab (inner product). Only as a shorthand. 59 | EIGEN_ALIGN16 Mat22f Jab2; // 2x2 60 | 61 | }; 62 | } 63 | 64 | -------------------------------------------------------------------------------- /src/util/FrameShell.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | #include "util/NumType.h" 28 | #include "algorithm" 29 | 30 | namespace dso 31 | { 32 | 33 | 34 | class FrameShell 35 | { 36 | public: 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 38 | int id; // INTERNAL ID, starting at zero. 39 | int incoming_id; // ID passed into DSO 40 | double timestamp; // timestamp passed into DSO. 41 | 42 | // set once after tracking 43 | SE3 camToTrackingRef; 44 | FrameShell* trackingRef; 45 | 46 | // constantly adapted. 47 | SE3 camToWorld; // Write: TRACKING, while frame is still fresh; MAPPING: only when locked [shellPoseMutex]. 48 | AffLight aff_g2l; 49 | bool poseValid; 50 | 51 | // statisitcs 52 | int statistics_outlierResOnThis; 53 | int statistics_goodResOnThis; 54 | int marginalizedAt; 55 | double movedByOpt; 56 | 57 | Vec3 velocity = Vec3::Zero(); 58 | Vec3 bias_g = Vec3::Zero(); 59 | Vec3 bias_a = Vec3::Zero(); 60 | Vec3 delta_bias_g = Vec3::Zero(); 61 | Vec3 delta_bias_a = Vec3::Zero(); 62 | 63 | 64 | inline FrameShell() 65 | { 66 | id=0; 67 | poseValid=true; 68 | camToWorld = SE3(); 69 | timestamp=0; 70 | marginalizedAt=-1; 71 | movedByOpt=0; 72 | statistics_outlierResOnThis=statistics_goodResOnThis=0; 73 | trackingRef=0; 74 | camToTrackingRef = SE3(); 75 | } 76 | }; 77 | 78 | 79 | } 80 | 81 | -------------------------------------------------------------------------------- /src/util/ImageAndExposure.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 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/util/IndexThreadReduce.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/settings.h" 28 | #include "boost/thread.hpp" 29 | #include 30 | #include 31 | 32 | 33 | 34 | namespace dso 35 | { 36 | 37 | template 38 | class IndexThreadReduce 39 | { 40 | 41 | public: 42 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 43 | 44 | inline IndexThreadReduce() 45 | { 46 | nextIndex = 0; 47 | maxIndex = 0; 48 | stepSize = 1; 49 | callPerIndex = boost::bind(&IndexThreadReduce::callPerIndexDefault, this, _1, _2, _3, _4); 50 | 51 | running = true; 52 | for(int i=0;i callPerIndex, int first, int end, int stepSize = 0) 77 | { 78 | 79 | memset(&stats, 0, sizeof(Running)); 80 | 81 | // if(!multiThreading) 82 | // { 83 | // callPerIndex(first, end, &stats, 0); 84 | // return; 85 | // } 86 | 87 | 88 | 89 | if(stepSize == 0) 90 | stepSize = ((end-first)+NUM_THREADS-1)/NUM_THREADS; 91 | 92 | 93 | //printf("reduce called\n"); 94 | 95 | boost::unique_lock lock(exMutex); 96 | 97 | // save 98 | this->callPerIndex = callPerIndex; 99 | nextIndex = first; 100 | maxIndex = end; 101 | this->stepSize = stepSize; 102 | 103 | // go worker threads! 104 | for(int i=0;icallPerIndex = boost::bind(&IndexThreadReduce::callPerIndexDefault, this, _1, _2, _3, _4); 135 | 136 | //printf("reduce done (all threads finished)\n"); 137 | } 138 | 139 | Running stats; 140 | 141 | private: 142 | boost::thread workerThreads[NUM_THREADS]; 143 | bool isDone[NUM_THREADS]; 144 | bool gotOne[NUM_THREADS]; 145 | 146 | boost::mutex exMutex; 147 | boost::condition_variable todo_signal; 148 | boost::condition_variable done_signal; 149 | 150 | int nextIndex; 151 | int maxIndex; 152 | int stepSize; 153 | 154 | bool running; 155 | 156 | boost::function callPerIndex; 157 | 158 | void callPerIndexDefault(int i, int j,Running* k, int tid) 159 | { 160 | printf("ERROR: should never be called....\n"); 161 | assert(false); 162 | } 163 | 164 | void workerLoop(int idx) 165 | { 166 | boost::unique_lock lock(exMutex); 167 | 168 | while(running) 169 | { 170 | // try to get something to do. 171 | int todo = 0; 172 | bool gotSomething = false; 173 | if(nextIndex < maxIndex) 174 | { 175 | // got something! 176 | todo = nextIndex; 177 | nextIndex+=stepSize; 178 | gotSomething = true; 179 | } 180 | 181 | // if got something: do it (unlock in the meantime) 182 | if(gotSomething) 183 | { 184 | lock.unlock(); 185 | 186 | assert(callPerIndex != 0); 187 | 188 | Running s; memset(&s, 0, sizeof(Running)); 189 | callPerIndex(todo, std::min(todo+stepSize, maxIndex), &s, idx); 190 | gotOne[idx] = true; 191 | lock.lock(); 192 | stats += s; 193 | } 194 | 195 | // otherwise wait on signal, releasing lock in the meantime. 196 | else 197 | { 198 | if(!gotOne[idx]) 199 | { 200 | lock.unlock(); 201 | assert(callPerIndex != 0); 202 | Running s; memset(&s, 0, sizeof(Running)); 203 | callPerIndex(0, 0, &s, idx); 204 | gotOne[idx] = true; 205 | lock.lock(); 206 | stats += s; 207 | } 208 | isDone[idx] = true; 209 | //printf("worker %d waiting..\n", idx); 210 | done_signal.notify_all(); 211 | todo_signal.wait(lock); 212 | } 213 | } 214 | } 215 | }; 216 | } 217 | -------------------------------------------------------------------------------- /src/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/util/NumType.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 "Eigen/Core" 28 | #include "sophus/sim3.hpp" 29 | #include "sophus/se3.hpp" 30 | #include 31 | 32 | 33 | namespace dso 34 | { 35 | 36 | // CAMERA MODEL TO USE 37 | 38 | 39 | #define SSEE(val,idx) (*(((float*)&val)+idx)) 40 | 41 | 42 | #define MAX_RES_PER_POINT 8 43 | #define NUM_THREADS 6 44 | 45 | 46 | #define todouble(x) (x).cast() 47 | 48 | 49 | typedef Sophus::SE3d SE3; 50 | typedef Sophus::Sim3d Sim3; 51 | typedef Sophus::SO3d SO3; 52 | typedef Sophus::SO3f SO3f; 53 | typedef Sophus::RxSO3d RxSO3; 54 | 55 | 56 | 57 | #define CPARS 4 58 | 59 | 60 | typedef Eigen::Matrix MatXX; 61 | typedef Eigen::Matrix MatCC; 62 | #define MatToDynamic(x) MatXX(x) 63 | 64 | 65 | 66 | typedef Eigen::Matrix MatC10; 67 | typedef Eigen::Matrix Mat1010; 68 | typedef Eigen::Matrix Mat1313; 69 | 70 | typedef Eigen::Matrix Mat810; 71 | typedef Eigen::Matrix Mat83; 72 | typedef Eigen::Matrix Mat66; 73 | typedef Eigen::Matrix Mat96; 74 | typedef Eigen::Matrix Mat53; 75 | typedef Eigen::Matrix Mat43; 76 | typedef Eigen::Matrix Mat42; 77 | typedef Eigen::Matrix Mat33; 78 | typedef Eigen::Matrix Mat36; 79 | typedef Eigen::Matrix Mat22; 80 | typedef Eigen::Matrix Mat8C; 81 | typedef Eigen::Matrix MatC8; 82 | typedef Eigen::Matrix Mat8Cf; 83 | typedef Eigen::Matrix MatC8f; 84 | 85 | typedef Eigen::Matrix Mat88; 86 | typedef Eigen::Matrix Mat77; 87 | typedef Eigen::Matrix Mat73; 88 | 89 | typedef Eigen::Matrix VecC; 90 | typedef Eigen::Matrix VecCf; 91 | typedef Eigen::Matrix Vec15; 92 | typedef Eigen::Matrix Vec17; 93 | typedef Eigen::Matrix Vec16; 94 | typedef Eigen::Matrix Vec13; 95 | typedef Eigen::Matrix Vec12; 96 | typedef Eigen::Matrix Vec11; 97 | typedef Eigen::Matrix Vec10; 98 | typedef Eigen::Matrix Vec9; 99 | typedef Eigen::Matrix Vec8; 100 | typedef Eigen::Matrix Vec7; 101 | typedef Eigen::Matrix Vec6; 102 | typedef Eigen::Matrix Vec5; 103 | typedef Eigen::Matrix Vec4; 104 | typedef Eigen::Matrix Vec3; 105 | typedef Eigen::Matrix Vec2; 106 | typedef Eigen::Matrix VecX; 107 | 108 | typedef Eigen::Matrix Mat33f; 109 | typedef Eigen::Matrix Mat103f; 110 | typedef Eigen::Matrix Mat22f; 111 | typedef Eigen::Matrix Vec3f; 112 | typedef Eigen::Matrix Vec2f; 113 | typedef Eigen::Matrix Vec6f; 114 | 115 | 116 | 117 | typedef Eigen::Matrix Mat49; 118 | typedef Eigen::Matrix Mat89; 119 | 120 | typedef Eigen::Matrix Mat94; 121 | typedef Eigen::Matrix Mat98; 122 | 123 | typedef Eigen::Matrix Mat81; 124 | typedef Eigen::Matrix Mat18; 125 | typedef Eigen::Matrix Mat91; 126 | typedef Eigen::Matrix Mat19; 127 | 128 | 129 | typedef Eigen::Matrix Mat84; 130 | typedef Eigen::Matrix Mat48; 131 | typedef Eigen::Matrix Mat44; 132 | 133 | typedef Eigen::Matrix Mat99; 134 | typedef Eigen::Matrix Mat97; 135 | typedef Eigen::Matrix Mat93; 136 | typedef Eigen::Matrix Mat1212; 137 | typedef Eigen::Matrix Mat1111; 138 | typedef Eigen::Matrix Mat1515; 139 | typedef Eigen::Matrix Mat1717; 140 | typedef Eigen::Matrix Mat1616; 141 | typedef Eigen::Matrix Mat1015; 142 | typedef Eigen::Matrix Mat915; 143 | typedef Eigen::Matrix Mat916; 144 | 145 | 146 | typedef Eigen::Matrix VecNRf; 147 | typedef Eigen::Matrix Vec12f; 148 | typedef Eigen::Matrix Mat18f; 149 | typedef Eigen::Matrix Mat66f; 150 | typedef Eigen::Matrix Mat88f; 151 | typedef Eigen::Matrix Mat84f; 152 | typedef Eigen::Matrix Vec8f; 153 | typedef Eigen::Matrix Vec10f; 154 | typedef Eigen::Matrix Mat66f; 155 | typedef Eigen::Matrix Vec4f; 156 | typedef Eigen::Matrix Mat44f; 157 | typedef Eigen::Matrix Mat1212f; 158 | typedef Eigen::Matrix Vec12f; 159 | typedef Eigen::Matrix Mat1313f; 160 | typedef Eigen::Matrix Mat1010f; 161 | typedef Eigen::Matrix Vec13f; 162 | typedef Eigen::Matrix Mat99f; 163 | typedef Eigen::Matrix Vec9f; 164 | 165 | typedef Eigen::Matrix Mat42f; 166 | typedef Eigen::Matrix Mat62f; 167 | typedef Eigen::Matrix Mat12f; 168 | typedef Eigen::Matrix Mat32f; 169 | typedef Eigen::Matrix Mat36f; 170 | 171 | typedef Eigen::Matrix VecXf; 172 | typedef Eigen::Matrix MatXXf; 173 | 174 | 175 | typedef Eigen::Matrix MatPCPC; 176 | typedef Eigen::Matrix MatPCPCf; 177 | typedef Eigen::Matrix VecPC; 178 | typedef Eigen::Matrix VecPCf; 179 | 180 | typedef Eigen::Matrix Mat1414f; 181 | typedef Eigen::Matrix Vec14f; 182 | typedef Eigen::Matrix Mat1414; 183 | typedef Eigen::Matrix Vec14; 184 | 185 | 186 | 187 | 188 | 189 | 190 | // transforms points from one frame to another. 191 | struct AffLight 192 | { 193 | AffLight(double a_, double b_) : a(a_), b(b_) {}; 194 | AffLight() : a(0), b(0) {}; 195 | 196 | // Affine Parameters: 197 | double a,b; // I_frame = exp(a)*I_global + b. // I_global = exp(-a)*(I_frame - b). 198 | 199 | static Vec2 fromToVecExposure(float exposureF, float exposureT, AffLight g2F, AffLight g2T) 200 | { 201 | if(exposureF==0 || exposureT==0) 202 | { 203 | exposureT = exposureF = 1; 204 | //printf("got exposure value of 0! please choose the correct model.\n"); 205 | //assert(setting_brightnessTransferFunc < 2); 206 | } 207 | 208 | double a = exp(g2T.a-g2F.a) * exposureT / exposureF; 209 | double b = g2T.b - a*g2F.b; 210 | return Vec2(a,b); 211 | } 212 | 213 | Vec2 vec() 214 | { 215 | return Vec2(a,b); 216 | } 217 | }; 218 | 219 | } 220 | 221 | -------------------------------------------------------------------------------- /src/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/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 | -------------------------------------------------------------------------------- /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/Sophus/sophus/tests.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPUHS_TESTS_HPP 2 | #define SOPUHS_TESTS_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include "sophus.hpp" 8 | 9 | namespace Sophus { 10 | 11 | using namespace std; 12 | using namespace Eigen; 13 | 14 | template 15 | class Tests { 16 | 17 | public: 18 | typedef typename LieGroup::Scalar Scalar; 19 | typedef typename LieGroup::Transformation Transformation; 20 | typedef typename LieGroup::Tangent Tangent; 21 | typedef typename LieGroup::Point Point; 22 | typedef typename LieGroup::Adjoint Adjoint; 23 | static const int N = LieGroup::N; 24 | static const int DoF = LieGroup::DoF; 25 | 26 | const Scalar SMALL_EPS; 27 | 28 | Tests() : SMALL_EPS(SophusConstants::epsilon()) { 29 | } 30 | 31 | void setGroupElements(const vector & group_vec) { 32 | group_vec_ = group_vec; 33 | } 34 | 35 | void setTangentVectors(const vector & tangent_vec) { 36 | tangent_vec_ = tangent_vec; 37 | } 38 | 39 | void setPoints(const vector & point_vec) { 40 | point_vec_ = point_vec; 41 | } 42 | 43 | bool adjointTest() { 44 | bool passed = true; 45 | for (size_t i=0; i20.*SMALL_EPS) { 59 | cerr << "Adjoint" << endl; 60 | cerr << "Test case: " << i << "," << j <SMALL_EPS) { 80 | cerr << "G - exp(log(G))" << endl; 81 | cerr << "Test case: " << i << endl; 82 | cerr << DiffT <10.*SMALL_EPS) { 101 | cerr << "expmap(hat(x)) - exp(x)" << endl; 102 | cerr << "Test case: " << i << endl; 103 | cerr << exp_x <SMALL_EPS) { 124 | cerr << "Transform vector" << endl; 125 | cerr << "Test case: " << i << endl; 126 | cerr << (res1-res2) <SMALL_EPS) { 147 | cerr << "Lie Bracket Test" << endl; 148 | cerr << "Test case: " << i << ", " < fastmul_res(fastmul_res_raw); 165 | Eigen::Map group_j_constmap(group_vec_[j].data()); 166 | fastmul_res = group_vec_[i]; 167 | fastmul_res.fastMultiply(group_j_constmap); 168 | Transformation diff = mul_resmat-fastmul_res.matrix(); 169 | Scalar nrm = diff.norm(); 170 | if (isnan(nrm) || nrm>SMALL_EPS) { 171 | cerr << "Map & Multiply" << endl; 172 | cerr << "Test case: " << i << "," << j << endl; 173 | cerr << diff <SMALL_EPS) { 188 | cerr << "Hat-vee Test" << endl; 189 | cerr << "Test case: " << i << endl; 190 | cerr << resDiff << endl; 191 | cerr << endl; 192 | passed = false; 193 | } 194 | } 195 | return passed; 196 | } 197 | 198 | 199 | 200 | void runAllTests() { 201 | bool passed = adjointTest(); 202 | if (!passed) { 203 | cerr << "failed!" << endl << endl; 204 | exit(-1); 205 | } 206 | passed = expLogTest(); 207 | if (!passed) { 208 | cerr << "failed!" << endl << endl; 209 | exit(-1); 210 | } 211 | passed = expMapTest(); 212 | if (!passed) { 213 | cerr << "failed!" << endl << endl; 214 | exit(-1); 215 | } 216 | passed = groupActionTest(); 217 | if (!passed) { 218 | cerr << "failed!" << endl << endl; 219 | exit(-1); 220 | } 221 | passed = lieBracketTest(); 222 | if (!passed) { 223 | cerr << "failed!" << endl << endl; 224 | exit(-1); 225 | } 226 | passed = mapAndMultTest(); 227 | if (!passed) { 228 | cerr << "failed!" << endl << endl; 229 | exit(-1); 230 | } 231 | passed = veeHatTest(); 232 | if (!passed) { 233 | cerr << "failed!" << endl << endl; 234 | exit(-1); 235 | } 236 | cerr << "passed." << endl << endl; 237 | } 238 | 239 | private: 240 | Matrix map(const Matrix & T, 241 | const Matrix & p) { 242 | return T.template topLeftCorner()*p 243 | + T.template topRightCorner(); 244 | } 245 | 246 | Matrix map(const Matrix & T, 247 | const Matrix & p) { 248 | return T*p; 249 | } 250 | 251 | Scalar norm(const Scalar & v) { 252 | return std::abs(v); 253 | } 254 | 255 | Scalar norm(const Matrix & T) { 256 | return T.norm(); 257 | } 258 | 259 | std::vector group_vec_; 260 | std::vector tangent_vec_; 261 | std::vector point_vec_; 262 | }; 263 | } 264 | #endif // TESTS_HPP 265 | -------------------------------------------------------------------------------- /thirdparty/libzip-1.1.1.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RonaldSun/VI-Stereo-DSO/158cc08034a87e3dc4387674976d7cbcdc97034d/thirdparty/libzip-1.1.1.tar.gz --------------------------------------------------------------------------------