├── CHANGELOG ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake ├── FindEigen3.cmake ├── FindLibZip.cmake └── FindSuiteParse.cmake ├── doc ├── final_trajectory.png └── run_outdoor.gif ├── output └── pose.txt ├── sensor ├── ijrr │ ├── x_b_lb3.txt │ ├── x_lb3_c1.csv │ ├── x_lb3_c2.csv │ ├── x_lb3_c3.csv │ ├── x_lb3_c4.csv │ └── x_lb3_c5.csv └── nclt │ ├── x_lb3_c1.csv │ ├── x_lb3_c2.csv │ ├── x_lb3_c3.csv │ ├── x_lb3_c4.csv │ └── x_lb3_c5.csv ├── src ├── FullSystem │ ├── CoarseInitializer.cpp │ ├── CoarseInitializer.h │ ├── CoarseTracker.cpp │ ├── CoarseTracker.h │ ├── CoarseTracker_xiang.h │ ├── FullSystem.cpp │ ├── FullSystem.h │ ├── FullSystemDebugStuff.cpp │ ├── FullSystemMarginalize.cpp │ ├── FullSystemOptPoint.cpp │ ├── FullSystemOptimize.cpp │ ├── HessianBlocks.cpp │ ├── HessianBlocks.h │ ├── ImmaturePoint.cpp │ ├── ImmaturePoint.h │ ├── PixelSelector.h │ ├── PixelSelector2.cpp │ ├── PixelSelector2.h │ ├── ResidualProjections.h │ ├── Residuals.cpp │ └── Residuals.h ├── IOWrapper │ ├── ImageDisplay.h │ ├── ImageDisplay_dummy.cpp │ ├── ImageRW.h │ ├── ImageRW_dummy.cpp │ ├── OpenCV │ │ ├── ImageDisplay_OpenCV.cpp │ │ └── ImageRW_OpenCV.cpp │ ├── Output3DWrapper.h │ ├── OutputWrapper │ │ └── SampleOutputWrapper.h │ └── Pangolin │ │ ├── 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 │ ├── lsd.cpp │ ├── lsd.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 /CHANGELOG: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ========= 16.11.2016 ========== 5 | * Added Sample output wrapper IOWrapper/OutputWrapper/SampleOutputWrapper.h. 6 | * Added extensive comments to IOWrapper/Output3DWrapper.h. 7 | * Added support for multiple 3DOutputWrapper simulataneously. 8 | * Added options "quiet=1" and "sampleoutput=1". 9 | * Did some minor code cleaning. 10 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dso) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -O3") 5 | 6 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 7 | 8 | find_package(SuiteParse REQUIRED) 9 | find_package(Eigen3 REQUIRED) 10 | find_package(Boost COMPONENTS system thread) 11 | find_package(OpenMP REQUIRED) 12 | 13 | find_package(LibZip QUIET) 14 | find_package(Pangolin 0.2 QUIET) 15 | find_package(OpenCV REQUIRED) 16 | find_package(PCL REQUIRED QUIET) 17 | 18 | # flags 19 | add_definitions("-DENABLE_SSE") 20 | #set(CMAKE_CXX_FLAGS 21 | # "${SSE_FLAGS} -O3 -g -std=c++0x -march=native" 22 | # "${SSE_FLAGS} -O3 -g -std=c++0x -fno-omit-frame-pointer" 23 | #) 24 | 25 | if (MSVC) 26 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /EHsc") 27 | endif (MSVC) 28 | 29 | set(dso_SOURCE_FILES 30 | ${PROJECT_SOURCE_DIR}/src/FullSystem/FullSystem.cpp 31 | ${PROJECT_SOURCE_DIR}/src/FullSystem/FullSystemOptimize.cpp 32 | ${PROJECT_SOURCE_DIR}/src/FullSystem/FullSystemOptPoint.cpp 33 | ${PROJECT_SOURCE_DIR}/src/FullSystem/FullSystemDebugStuff.cpp 34 | ${PROJECT_SOURCE_DIR}/src/FullSystem/FullSystemMarginalize.cpp 35 | ${PROJECT_SOURCE_DIR}/src/FullSystem/Residuals.cpp 36 | ${PROJECT_SOURCE_DIR}/src/FullSystem/CoarseTracker.cpp 37 | ${PROJECT_SOURCE_DIR}/src/FullSystem/CoarseInitializer.cpp 38 | ${PROJECT_SOURCE_DIR}/src/FullSystem/ImmaturePoint.cpp 39 | ${PROJECT_SOURCE_DIR}/src/FullSystem/HessianBlocks.cpp 40 | ${PROJECT_SOURCE_DIR}/src/FullSystem/PixelSelector2.cpp 41 | ${PROJECT_SOURCE_DIR}/src/OptimizationBackend/EnergyFunctional.cpp 42 | ${PROJECT_SOURCE_DIR}/src/OptimizationBackend/AccumulatedTopHessian.cpp 43 | ${PROJECT_SOURCE_DIR}/src/OptimizationBackend/AccumulatedSCHessian.cpp 44 | ${PROJECT_SOURCE_DIR}/src/OptimizationBackend/EnergyFunctionalStructs.cpp 45 | ${PROJECT_SOURCE_DIR}/src/util/settings.cpp 46 | ${PROJECT_SOURCE_DIR}/src/util/Undistort.cpp 47 | ${PROJECT_SOURCE_DIR}/src/util/globalCalib.cpp 48 | ${PROJECT_SOURCE_DIR}/src/util/lsd.cpp 49 | ) 50 | 51 | 52 | include_directories( 53 | ${catkin_INCLUDE_DIRS} 54 | ${PROJECT_SOURCE_DIR}/src 55 | ${PROJECT_SOURCE_DIR}/thirdparty/Sophus 56 | ${PROJECT_SOURCE_DIR}/thirdparty/sse2neon 57 | ${EIGEN3_INCLUDE_DIR} 58 | ${PCL_INCLUDE_DIRS} 59 | ) 60 | 61 | if(OPENMP_FOUND) 62 | message("OPENMP FOUND") 63 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 64 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 65 | endif() 66 | 67 | if (Pangolin_FOUND) 68 | message("--- found PANGOLIN, compiling dso_pangolin library.") 69 | include_directories( ${catkin_INCLUDE_DIRS} ${Pangolin_INCLUDE_DIRS} ) 70 | set(dso_pangolin_SOURCE_FILES 71 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/Pangolin/KeyFrameDisplay.cpp 72 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/Pangolin/PangolinDSOViewer.cpp) 73 | set(HAS_PANGOLIN 1) 74 | else () 75 | message("--- could not find PANGOLIN, not compiling dso_pangolin library.") 76 | message(" this means there will be no 3D display / GUI available for dso_dataset.") 77 | set(dso_pangolin_SOURCE_FILES ) 78 | set(HAS_PANGOLIN 0) 79 | endif () 80 | 81 | if (OpenCV_FOUND) 82 | message("--- found OpenCV, compiling dso_opencv library.") 83 | message("--- OpenCV_VERSION: ${OpenCV_VERSION}") 84 | message("--- OpenCV_INCLUDE_PATH: ${OpenCV_INCLUDE_DIRS}") 85 | message("--- OpenCV_LIB_PATH: ${OpenCV_LIBRARIES}") 86 | include_directories( ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ) 87 | set(dso_opencv_SOURCE_FILES 88 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/OpenCV/ImageDisplay_OpenCV.cpp 89 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/OpenCV/ImageRW_OpenCV.cpp) 90 | set(HAS_OPENCV 1) 91 | else () 92 | message("--- could not find OpenCV, not compiling dso_opencv library.") 93 | message(" this means there will be no image display, and image read / load functionality.") 94 | set(dso_opencv_SOURCE_FILES 95 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/ImageDisplay_dummy.cpp 96 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/ImageRW_dummy.cpp) 97 | set(HAS_OPENCV 0) 98 | endif () 99 | 100 | if (LIBZIP_LIBRARY) 101 | message("--- found ziplib (${LIBZIP_VERSION}), compiling with zip capability.") 102 | add_definitions(-DHAS_ZIPLIB=1) 103 | include_directories( ${catkin_INCLUDE_DIRS} ${LIBZIP_INCLUDE_DIR_ZIP} ${LIBZIP_INCLUDE_DIR_ZIPCONF} ) 104 | else() 105 | message("--- not found ziplib (${LIBZIP_LIBRARY}), compiling without zip capability.") 106 | set(LIBZIP_LIBRARY "") 107 | endif() 108 | 109 | # compile main library. 110 | include_directories( ${catkin_INCLUDE_DIRS} ${CSPARSE_INCLUDE_DIR} ${CHOLMOD_INCLUDE_DIR}) 111 | add_library(dso ${dso_SOURCE_FILES} ${dso_opencv_SOURCE_FILES} ${dso_pangolin_SOURCE_FILES}) 112 | target_link_libraries(dso ${OpenCV_LIBRARIES} ${PCL_LIBRARIES}) 113 | 114 | #set_property( TARGET dso APPEND_STRING PROPERTY COMPILE_FLAGS -Wall ) 115 | 116 | 117 | if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") # OSX 118 | set(BOOST_THREAD_LIBRARY boost_thread-mt) 119 | else() 120 | set(BOOST_THREAD_LIBRARY boost_thread) 121 | endif() 122 | 123 | # build main executable (only if we have both OpenCV and Pangolin) 124 | if (OpenCV_FOUND AND Pangolin_FOUND) 125 | message("--- compiling dso_dataset.") 126 | add_executable(dso_dataset ${PROJECT_SOURCE_DIR}/src/main_dso_pangolin.cpp ) 127 | target_link_libraries(dso_dataset dso boost_system cxsparse ${catkin_LIBRARIES} ${BOOST_THREAD_LIBRARY} ${LIBZIP_LIBRARY} ${Pangolin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES}) 128 | else() 129 | message("--- not building dso_dataset, since either don't have openCV or Pangolin.") 130 | endif() 131 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Panoramic-LDSO 2 | 3 | **Panoramic-LDSO** (Panoramic Direct LiDAR-assisted Visual Odometry) is designed for fully associating the 360-degree field-of-view (FOV) LiDAR points with the 360-degree FOV panoramic image datas. 360-degree FOV panoramic images can provide more available information, which can compensate inaccurate pose estimation caused by insufficient texture or motion blur from a single view. 4 | 5 | ## Related Work 6 | 7 | [Panoramic Direct LiDAR-assisted Visual Odometry](https://arxiv.org/abs/2409.09287) 8 | 9 | Authors: [*Zikang Yuan*](https://scholar.google.com/citations?hl=zh-CN&user=acxdM9gAAAAJ), *Tianle Xu*, *Xiaoxiang Wang*, *Jinni Geng* and [*Xin Yang*](https://scholar.google.com/citations?user=lsz8OOYAAAAJ&hl=zh-CN) 10 | 11 | ## Demo Video (2024-09-14 Update) 12 | 13 | The **x16 Real-Time Performance (Up)** and **Final Trjaectory and Sparse Map (Down)** on the segment of sequence *2012-01-08* from [*NCLT*](http://robots.engin.umich.edu/nclt/) dataset. 14 | 15 |
16 | 17 | 18 |
19 | 20 | ## Installation 21 | 22 | ### 1. Requirements 23 | 24 | > GCC >= 7.5.0 25 | > 26 | > Cmake >= 3.16.0 27 | > 28 | > [Eigen3](http://eigen.tuxfamily.org/index.php?title=Main_Page) >= 3.3.4 29 | > 30 | > [OpenCV](https://github.com/opencv/opencv) >= 3.3 31 | > 32 | > [PCL](https://pointclouds.org/downloads/) == 1.8 for Ubuntu 18.04, and == 1.10 for Ubuntu 20.04 33 | > 34 | > [Pangolin](https://github.com/stevenlovegrove/Pangolin) == 0.5 or 0.6 for Ubuntu 20.04 35 | 36 | ##### Have Tested On: 37 | 38 | | OS | GCC | Cmake | Eigen3 | OpenCV | PCL | Pangolin | 39 | |:-:|:-:|:-:|:-:|:-:|:-:|:-:| 40 | | Ubuntu 20.04 | 9.4.0 | 3.16.3 | 3.3.7 | 4.2.0 | 1.10.0 | 0.5 | 41 | 42 | ### 2. Clone the directory and build 43 | 44 | ```bash 45 | git clone https://github.com/ZikangYuan/panoramic_lidar_dso.git 46 | mkdir build 47 | cd build 48 | cmake .. 49 | make 50 | ``` 51 | 52 | ## Run on Public Datasets 53 | 54 | Noted: 55 | 56 | 1. Before running, please down load the **calib** folder from **[Google drive](https://drive.google.com/drive/folders/1WnvzUzP_s70p4myPf5fsP1Jtr_62PnL1)**, and put it under the . 57 | 58 | 2. Currently the package only supports interfaces to *NCLT* and *IJRR* datasets. If you want to run on other datasets, you'll need to modify the code yourself. 59 | 60 | ### 1. Run on [*NCLT*](http://robots.engin.umich.edu/nclt/) 61 | 62 | Before running, please ensure the dataset format is as follow: 63 | 64 | ```bash 65 | 66 | |____________2012-01-08 67 | |____________lb3 68 | |____________velodyne_sync 69 | |____________2012-09-28 70 | |____________lb3 71 | |____________velodyne_sync 72 | |____________2012-11-04 73 | |____________lb3 74 | |____________velodyne_sync 75 | |____________2012-12-01 76 | |____________lb3 77 | |____________velodyne_sync 78 | |____________2013-02-23 79 | |____________lb3 80 | |____________velodyne_sync 81 | |____________2013-04-05 82 | |____________lb3 83 | |____________velodyne_sync 84 | ``` 85 | 86 | Then open the terminal in the path of the /build, and type: 87 | 88 | ```bash 89 | ./dso_dataset dataset= sequence= seg= calib=/calib/nclt/calib undistort=/calib/nclt/U2D_Cam pathSensorPrameter/sensor/nclt/x_lb3_c resultPath=/output/pose.txt mode=1 quiet=0 IJRR=0 90 | ``` 91 | 92 | ### 2. Run on [*IJRR*](https://robots.engin.umich.edu/SoftwareData/InfoFord) 93 | 94 | Before running, please ensure the dataset format is as follow: 95 | 96 | ```bash 97 | 98 | |____________ford_1 99 | |____________Timestamp.log 100 | |____________IMAGES 101 | |____________pcd 102 | |____________ford_2 103 | |____________Timestamp.log 104 | |____________IMAGES 105 | |____________pcd 106 | ``` 107 | 108 | Then open the terminal in the path of the /build, and type: 109 | 110 | ```bash 111 | ./dso_dataset dataset= sequence= seg= calib=/calib/ijrr/calib undistort=/calib/ijrr/U2D_Cam pathSensorPrameter=/sensor/ijrr/x_lb3_c resultPath=/output/pose.txt mode=1 quiet=0 IJRR=1 112 | ``` 113 | 114 | ## Citation 115 | 116 | If you use our work in your research project, please consider citing: 117 | 118 | ``` 119 | @article{yuan2024panoramic, 120 | title={Panoramic Direct LiDAR-assisted Visual Odometry}, 121 | author={Yuan, Zikang and Xu, Tianle and Wang, Xiaoxiang and Geng, Jinni and Yang, Xin}, 122 | journal={arXiv preprint arXiv:2409.09287}, 123 | year={2024} 124 | } 125 | ``` 126 | 127 | ## Acknowledgments 128 | 129 | Thanks for [DSO](https://github.com/JakobEngel/dso). 130 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /doc/final_trajectory.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZikangYuan/panoramic_lidar_dso/67da5dfc5d35e8c42a7a7b431137feb211cfa7ee/doc/final_trajectory.png -------------------------------------------------------------------------------- /doc/run_outdoor.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZikangYuan/panoramic_lidar_dso/67da5dfc5d35e8c42a7a7b431137feb211cfa7ee/doc/run_outdoor.gif -------------------------------------------------------------------------------- /output/pose.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZikangYuan/panoramic_lidar_dso/67da5dfc5d35e8c42a7a7b431137feb211cfa7ee/output/pose.txt -------------------------------------------------------------------------------- /sensor/ijrr/x_b_lb3.txt: -------------------------------------------------------------------------------- 1 | 0.34 -0.01 -0.42 0.7056 -0.0001 -0.0003 -0.7086 -------------------------------------------------------------------------------- /sensor/ijrr/x_lb3_c1.csv: -------------------------------------------------------------------------------- 1 | -0.0010 -0.0038 -1.0000 0.4150 2 | -1.0000 -0.0019 0.0010 0.0129 3 | -0.0019 1.0000 -0.0038 0.2998 4 | -------------------------------------------------------------------------------- /sensor/ijrr/x_lb3_c2.csv: -------------------------------------------------------------------------------- 1 | 0.0026 0.0031 -1.0000 0.4173 2 | -0.3100 0.9507 0.0022 0.3282 3 | 0.9507 0.3100 0.0034 0.0508 4 | -------------------------------------------------------------------------------- /sensor/ijrr/x_lb3_c3.csv: -------------------------------------------------------------------------------- 1 | 0.0017 -0.0032 -1.0000 0.4154 2 | 0.8056 0.5924 -5.4270e-04 0.1935 3 | 0.5924 -0.8056 0.0035 -0.3247 4 | -------------------------------------------------------------------------------- /sensor/ijrr/x_lb3_c4.csv: -------------------------------------------------------------------------------- 1 | 0.0070 0.0035 -1.0000 0.4177 2 | 0.8108 -0.5853 0.0036 -0.2092 3 | -0.5852 -0.8108 -0.0069 -0.3080 4 | 5 | -------------------------------------------------------------------------------- /sensor/ijrr/x_lb3_c5.csv: -------------------------------------------------------------------------------- 1 | 0.0079 0.0013 -1.0000 0.4171 2 | -0.3048 -0.9524 -0.0037 -0.3173 3 | -0.9524 0.3048 -0.0071 0.0764 4 | -------------------------------------------------------------------------------- /sensor/nclt/x_lb3_c1.csv: -------------------------------------------------------------------------------- 1 | 0.014543, 0.039337, 0.000398, -138.449751, 89.703877, -66.518051 2 | -------------------------------------------------------------------------------- /sensor/nclt/x_lb3_c2.csv: -------------------------------------------------------------------------------- 1 | -0.032674, 0.025928, 0.000176, 160.101024, 89.836345, -56.101163 2 | -------------------------------------------------------------------------------- /sensor/nclt/x_lb3_c3.csv: -------------------------------------------------------------------------------- 1 | -0.034969, -0.022993, 0.000030, 95.603967, 89.724274, -48.640335 2 | -------------------------------------------------------------------------------- /sensor/nclt/x_lb3_c4.csv: -------------------------------------------------------------------------------- 1 | 0.011238, -0.040367, -0.000393, -160.239278, 89.812338, 127.472911 2 | -------------------------------------------------------------------------------- /sensor/nclt/x_lb3_c5.csv: -------------------------------------------------------------------------------- 1 | 0.041862, -0.001905, -0.000212, 160.868615, 89.914152, 160.619894 2 | -------------------------------------------------------------------------------- /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 | struct frame_hessian; 42 | class FullSystem; 43 | 44 | 45 | struct Pnt 46 | { 47 | public: 48 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 49 | // index in jacobian. never changes (actually, there is no reason why). 50 | float u,v; 51 | 52 | // idepth / isgood / energy during optimization. 53 | float idepth; //!< 该点对应参考帧的逆深度 54 | bool isGood; //!< 点在新图像内, 相机前, 像素值有穷则好 55 | Vec2f energy; //!< [0]残差的平方, [1]正则化项(逆深度减一的平方) // (UenergyPhotometric, energyRegularizer) 56 | bool isGood_new; 57 | float idepth_new; //!< 该点在新的一帧(当前帧)上的逆深度 58 | Vec2f energy_new; //!< 迭代计算的新的能量 59 | 60 | float iR; //!< 逆深度的期望值 61 | float iRSumNum; //!< 子点逆深度信息矩阵之和 62 | 63 | float lastHessian; //!< 逆深度的Hessian, 即协方差, dd*dd 64 | float lastHessian_new; //!< 新一次迭代的协方差 65 | 66 | // max stepsize for idepth (corresponding to max. movement in pixel-space). 67 | float maxstep; //!< 逆深度增加的最大步长 68 | 69 | // idx (x+y*w) of closest point one pyramid level above. 70 | int parent; //!< 上一层中该点的父节点 (距离最近的)的id 71 | float parentDist; //!< 上一层中与父节点的距离 72 | 73 | // idx (x+y*w) of up to 10 nearest points in pixel space. 74 | int neighbours[10]; //!< 图像中离该点最近的10个点 75 | float neighboursDist[10]; //!< 最近10个点的距离 76 | 77 | float my_type; //!< 第0层提取是1, 2, 4, 对应d, 2d, 4d, 其它层是1 78 | float outlierTH; //!< 外点阈值 79 | 80 | // 2019.11.07 yzk 81 | float mdepth; 82 | float midepth; 83 | bool isFromSensor; 84 | // 2019.11.07 yzk 85 | }; 86 | 87 | class CoarseInitializer { 88 | public: 89 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 90 | CoarseInitializer(int w, int h); 91 | ~CoarseInitializer(); 92 | 93 | // 2019.11.15 yzk 94 | void setFirstFromLidar(CalibHessian* HCalib, FrameHessian* newFrameHessian, FullSystem* fullSystem, 95 | std::vector> cloudPixel[CAM_NUM]); 96 | // 2019.11.15 yzk 97 | void calcTGrads(FrameHessian* newFrameHessian); 98 | 99 | int frameID; //!< 当前加入的帧数 100 | bool fixAffine; //!< 是否优化光度参数 101 | bool printDebug; 102 | 103 | /* 104 | Pnt* points[PYR_LEVELS]; //!< 每一层上的点类, 是第一帧提取出来的 105 | int numPoints[PYR_LEVELS]; //!< 每一层的点数目 106 | */ 107 | Pnt* points[CAM_NUM][PYR_LEVELS]; //!< 每一层上的点类, 是第一帧提取出来的 108 | int numPoints[CAM_NUM][PYR_LEVELS]; //!< 每一层的点数目 109 | 110 | AffLight thisToNext_aff; //!< 参考帧与当前帧之间光度系数 111 | SE3 thisToNext; //!< 参考帧与当前帧之间位姿 112 | 113 | 114 | FrameHessian* firstFrame; //!< 第一帧 115 | FrameHessian* newFrame; //!< track中新加入的帧 116 | private: 117 | /* 118 | Mat33 K[PYR_LEVELS]; //!< camera参数 119 | Mat33 Ki[PYR_LEVELS]; 120 | double fx[PYR_LEVELS]; 121 | double fy[PYR_LEVELS]; 122 | double fxi[PYR_LEVELS]; 123 | double fyi[PYR_LEVELS]; 124 | double cx[PYR_LEVELS]; 125 | double cy[PYR_LEVELS]; 126 | double cxi[PYR_LEVELS]; 127 | double cyi[PYR_LEVELS]; 128 | */ 129 | Mat33 K[CAM_NUM][PYR_LEVELS]; //!< camera参数 130 | Mat33 Ki[CAM_NUM][PYR_LEVELS]; 131 | double fx[CAM_NUM][PYR_LEVELS]; 132 | double fy[CAM_NUM][PYR_LEVELS]; 133 | double fxi[CAM_NUM][PYR_LEVELS]; 134 | double fyi[CAM_NUM][PYR_LEVELS]; 135 | double cx[CAM_NUM][PYR_LEVELS]; 136 | double cy[CAM_NUM][PYR_LEVELS]; 137 | double cxi[CAM_NUM][PYR_LEVELS]; 138 | double cyi[CAM_NUM][PYR_LEVELS]; 139 | 140 | int w[PYR_LEVELS]; 141 | int h[PYR_LEVELS]; 142 | void makeK(CalibHessian* HCalib); 143 | 144 | bool snapped; //!< 是否尺度收敛 (暂定) 145 | int snappedAt; //!< 尺度收敛在第几帧 146 | 147 | // pyramid images & levels on all levels 148 | Eigen::Vector3f* dINew[PYR_LEVELS]; 149 | Eigen::Vector3f* dIFist[PYR_LEVELS]; 150 | 151 | Eigen::DiagonalMatrix wM; 152 | 153 | // temporary buffers for H and b. 154 | Vec10f* JbBuffer; //!< 用来计算Schur的 0-7: sum(dd * dp). 8: sum(res*dd). 9: 1/(1+sum(dd*dd))=inverse hessian entry. 155 | Vec10f* JbBuffer_new; //!< 跌待更新后新的值 156 | 157 | //* 9维向量, 乘积获得9*9矩阵, 并做的累加器 158 | Accumulator9 acc9; //!< Hessian 矩阵 159 | Accumulator9 acc9SC; //!< Schur部分Hessian 160 | 161 | 162 | Vec3f dGrads[PYR_LEVELS]; //!< 163 | 164 | //? 这几个参数很迷 165 | float alphaK; //!< 2.5*2.5 166 | float alphaW; //!< 150*150 167 | float regWeight; //!< 对逆深度的加权值, 0.8 168 | float couplingWeight; //!< 1 169 | 170 | Vec3f calcEC(int lvl); // returns OLD NERGY, NEW ENERGY, NUM TERMS. 171 | void optReg(int lvl); 172 | 173 | void propagateUp(int srcLvl); 174 | void propagateDown(int srcLvl); 175 | float rescale(); 176 | 177 | void resetPoints(int lvl); 178 | void doStep(int lvl, float lambda, Vec8f inc); 179 | void applyStep(int lvl); 180 | 181 | void makeGradients(Eigen::Vector3f** data); 182 | 183 | void debugPlot(int lvl, std::vector &wraps); 184 | // 2021.11.14 185 | void makeNN(int cam_idx); 186 | // 2021.11.14 187 | }; 188 | 189 | 190 | 191 | //* 作为 KDTreeSingleIndexAdaptor 类的第二个模板参数必须给出, 包括下面的接口 192 | struct FLANNPointcloud 193 | { 194 | inline FLANNPointcloud() {num=0; points=0;} 195 | inline FLANNPointcloud(int n, Pnt* p) : num(n), points(p) {} 196 | 197 | int num; 198 | Pnt* points; 199 | 200 | // 返回数据点的数目 201 | inline size_t kdtree_get_point_count() const { return num; } 202 | // 使用L2度量时使用, 返回向量p1, 到第idx_p2个数据点的欧氏距离 203 | inline float kdtree_distance(const float *p1, const size_t idx_p2,size_t /*size*/) const 204 | { 205 | const float d0=p1[0]-points[idx_p2].u; 206 | const float d1=p1[1]-points[idx_p2].v; 207 | return d0*d0+d1*d1; 208 | } 209 | // 返回第idx个点的第dim维数据 210 | inline float kdtree_get_pt(const size_t idx, int dim) const 211 | { 212 | if (dim==0) return points[idx].u; 213 | else return points[idx].v; 214 | } 215 | // 可选计算bounding box 216 | // false 表示默认 217 | // true 本函数应该返回bb 218 | template 219 | bool kdtree_get_bbox(BBOX& /* bb */) const { return false; } 220 | }; 221 | 222 | } 223 | 224 | 225 | -------------------------------------------------------------------------------- /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 35 | #include "util/lsd.h" 36 | #include 37 | #include 38 | #include 39 | 40 | 41 | 42 | 43 | namespace dso 44 | { 45 | struct CalibHessian; 46 | struct FrameHessian; 47 | struct PointFrameResidual; 48 | 49 | class CoarseTracker { 50 | public: 51 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 52 | 53 | CoarseTracker(int w, int h); 54 | ~CoarseTracker(); 55 | 56 | bool trackNewestCoarse( 57 | FrameHessian* newFrameHessian, 58 | SE3 &lastToNew_out, int coarsestLvl, 59 | Vec5 minResForAbort, SE3 slastToLast, int tries, 60 | IOWrap::Output3DWrapper* wrap=0); 61 | 62 | bool trackNewestCoarse_inv( 63 | FrameHessian* newFrameHessian, 64 | SE3 &lastToNew_out, int coarsestLvl, 65 | Vec5 minResForAbort, 66 | IOWrap::Output3DWrapper* wrap=0); 67 | int nThreads; 68 | std::vector m_RandEngines; // Mersenne twister high quality RNG that support *OpenMP* multi-threading 69 | bool trackNewestCoarse_ransac( 70 | FrameHessian* newFrameHessian, 71 | SE3 &lastToNew_out, int coarsestLvl, 72 | Vec5 minResForAbort, SE3 slastToLast, 73 | IOWrap::Output3DWrapper* wrap=0); 74 | 75 | bool TrackNewestToSelectCam( 76 | SE3 &lastToNew_out, int coarsestLvl, 77 | Vec5 minResForAbort, Vec6& lastRes, bool first = false); 78 | void recurrent( 79 | int num,int numUse, 80 | SE3 &lastToNew_out, int coarsestLvl, 81 | Vec5 &minResForAbort); 82 | 83 | void setCoarseTrackingRef( 84 | std::vector frameHessians, 85 | std::vector idxUse); 86 | 87 | // 2019.11.15 yzk 88 | void makeCoarseDepthForFirstFrame(FrameHessian* fh); 89 | void setCTRefForFirstFrame(std::vector frameHessians); 90 | // 2019.11.15 yzk 91 | 92 | void makeK(CalibHessian* HCalib); 93 | 94 | bool debugPrint, debugPlot; 95 | 96 | 97 | Mat33f K[CAM_NUM][PYR_LEVELS]; 98 | Mat33f Ki[CAM_NUM][PYR_LEVELS]; 99 | float fx[CAM_NUM][PYR_LEVELS]; 100 | float fy[CAM_NUM][PYR_LEVELS]; 101 | float fxi[CAM_NUM][PYR_LEVELS]; 102 | float fyi[CAM_NUM][PYR_LEVELS]; 103 | float cx[CAM_NUM][PYR_LEVELS]; 104 | float cy[CAM_NUM][PYR_LEVELS]; 105 | float cxi[CAM_NUM][PYR_LEVELS]; 106 | float cyi[CAM_NUM][PYR_LEVELS]; 107 | 108 | 109 | int w[PYR_LEVELS]; 110 | int h[PYR_LEVELS]; 111 | 112 | std::vector idxUse; 113 | bool weight[CAM_NUM]; 114 | 115 | void debugPlotIDepthMap(float* minID, float* maxID, std::vector &wraps); 116 | void debugPlotIDepthMapFloat(std::vector &wraps); 117 | 118 | FrameHessian* lastRef; //!< 参考帧 119 | // AffLight lastRef_aff_g2l; 120 | AffLight lastRef_aff_g2l[CAM_NUM]; 121 | FrameHessian* newFrame; //!< 新来的一帧 122 | int refFrameID; //!< 参考帧id 123 | 124 | bool hasChanged; 125 | 126 | // act as pure ouptut 127 | Vec6 lastResiduals; 128 | Vec3 lastFlowIndicators; //!< 光流指示用, 只有平移和, 旋转+平移的像素移动 129 | double firstCoarseRMSE; 130 | private: 131 | 132 | 133 | void makeCoarseDepthL0(std::vector frameHessians); 134 | 135 | float* idepth[CAM_NUM][PYR_LEVELS]; 136 | float* weightSums[PYR_LEVELS]; 137 | float* weightSums_bak[PYR_LEVELS]; 138 | 139 | 140 | Vec6 calcResAndGS(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH); 141 | float onlyCalcRes(const SE3 &refToNew, float cutoffTH); 142 | Vec6 calcRes(int lvl, const SE3 &refToNew, SE3 slastToLast, float cutoffTH); 143 | Vec6 calcRes_ransac(int lvl, const SE3 &refToNew, SE3 slastToLast, float cutoffTH, std::vector& idx_to_reproject); 144 | Vec6 calcRes_cross(int lvl, const SE3 &refToNew, SE3 slastToLast, float cutoffTH, std::queue idx[][CAM_NUM]); 145 | void preCompute(int lvl); 146 | Vec6 calcResInv(int lvl, const SE3 &refToNew, float cutoffTH, std::queue& resInliers, Mat66f &H, Vec6f &J_res); 147 | Vec3 calcExtrinRes( 148 | std::vector>& cld, 149 | const SE3 &T_lb3_lidar, float cutoffTH, Mat66f& H, Vec6f& J_res, int itr); 150 | Vec3 calcExtrinRes( 151 | std::vector>& cld, 152 | const SE3 &T_lb3_lidar, float cutoffTH); 153 | void calcGSSSE(int lvl, Mat66 &H_out, Vec6 &b_out, SE3 lastToNew, SE3 slastToLast); 154 | void calcGSSSE_inv(int lvl, Mat66f &H_out, Vec6f &b_out, std::queue& idx); 155 | void calcGSSSE_MutualPSimple(int lvl, Mat66 &H_out, Vec6 &b_out, SE3 lastToNew, SE3 slastToLast); 156 | void calcGS(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l); 157 | 158 | // pc buffers 159 | float* pc_u[CAM_NUM][PYR_LEVELS]; //!< 每层上的有逆深度点的坐标x 160 | float* pc_v[CAM_NUM][PYR_LEVELS]; //!< 每层上的有逆深度点的坐标y 161 | float* pc_idepth[CAM_NUM][PYR_LEVELS]; //!< 每层上点的逆深度 162 | float* pc_color[CAM_NUM][PYR_LEVELS]; //!< 每层上点的颜色值 163 | float* pc_Idu[CAM_NUM][PYR_LEVELS]; //!< 每层上点的颜色值在u方向上的梯度 164 | float* pc_Idv[CAM_NUM][PYR_LEVELS]; //!< 每层上点的颜色值在v方向上的梯度 165 | int pc_n[CAM_NUM][PYR_LEVELS]; //!< 每层上点的个数 166 | 167 | 168 | // warped buffers 169 | // float* buf_warped_X; //!< 投影得到的点在主相机坐标系下的X 170 | // float* buf_warped_Y; //!< 投影得到的点在主相机坐标系下的Y 171 | // float* buf_warped_Z; //!< 投影得到的点在主相机坐标系下的Z 172 | // float* buf_warped_idepth; //!< 投影得到的点的逆深度 173 | // float* buf_warped_u; //!< 投影得到的归一化坐标 174 | // float* buf_warped_v; //!< 同上 175 | // float* buf_warped_dx; //!< 投影点的图像梯度 176 | // float* buf_warped_dy; //!< 同上 177 | // float* buf_warped_residual; //!< 投影得到的残差 178 | // float* buf_warped_weight; //!< 投影的huber函数权重 179 | float* buf_warped_Xs[12]; //!< 投影得到的点在主相机坐标系下的X 180 | float* buf_warped_Ys[12]; //!< 投影得到的点在主相机坐标系下的Y 181 | float* buf_warped_Zs[12]; //!< 投影得到的点在主相机坐标系下的Z 182 | float* buf_warped_idepths[12]; //!< 投影得到的点的逆深度 183 | float* buf_warped_us[12]; //!< 投影得到的归一化坐标 184 | float* buf_warped_vs[12]; //!< 同上 185 | float* buf_warped_dxs[12]; //!< 投影点的图像梯度 186 | float* buf_warped_dys[12]; //!< 同上 187 | float* buf_warped_residuals[12]; //!< 投影得到的残差 188 | float* buf_warped_weights[12]; //!< 投影的huber函数权重 189 | int buf_warpeds[12][CAM_NUM*CAM_NUM]; 190 | int buf_warped_n; //!< 投影点的个数 191 | Vec6f* buf_dI_dT[PYR_LEVELS]; 192 | Mat66f* buf_H[PYR_LEVELS]; 193 | float* buf_res[PYR_LEVELS]; 194 | float* buf_hw[PYR_LEVELS]; 195 | 196 | 197 | std::vector ptrToDelete; //!< 所有的申请的内存指针, 用于析构删除 198 | 199 | Accumulator acc; 200 | Accumulator7 acc7; 201 | }; 202 | 203 | 204 | class CoarseDistanceMap { 205 | public: 206 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 207 | 208 | CoarseDistanceMap(int w, int h); 209 | ~CoarseDistanceMap(); 210 | 211 | void makeDistanceMap( 212 | std::vector frameHessians, 213 | FrameHessian* frame, int cam_idx); 214 | 215 | void makeInlierVotes( 216 | std::vector frameHessians); 217 | 218 | void makeK( CalibHessian* HCalib, int cam_idx); 219 | 220 | 221 | float* fwdWarpedIDDistFinal; //!< 距离场的数值 222 | 223 | Mat33f K[PYR_LEVELS]; 224 | Mat33f Ki[PYR_LEVELS]; 225 | float fx[PYR_LEVELS]; 226 | float fy[PYR_LEVELS]; 227 | float fxi[PYR_LEVELS]; 228 | float fyi[PYR_LEVELS]; 229 | float cx[PYR_LEVELS]; 230 | float cy[PYR_LEVELS]; 231 | float cxi[PYR_LEVELS]; 232 | float cyi[PYR_LEVELS]; 233 | int w[PYR_LEVELS]; 234 | int h[PYR_LEVELS]; 235 | 236 | void addIntoDistFinal(int u, int v); 237 | 238 | 239 | private: 240 | 241 | PointFrameResidual** coarseProjectionGrid; 242 | int* coarseProjectionGridNum; 243 | Eigen::Vector2i* bfsList1; //!< 投影到frame的坐标 244 | Eigen::Vector2i* bfsList2; //!< 和1轮换使用 245 | 246 | void growDistBFS(int bfsNum); 247 | }; 248 | 249 | } 250 | 251 | -------------------------------------------------------------------------------- /src/FullSystem/CoarseTracker_xiang.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 35 | #include "util/lsd.h" 36 | #include 37 | 38 | 39 | 40 | 41 | namespace dso 42 | { 43 | struct CalibHessian; 44 | struct FrameHessian; 45 | struct PointFrameResidual; 46 | 47 | class CoarseTracker { 48 | public: 49 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 50 | 51 | CoarseTracker(int w, int h); 52 | ~CoarseTracker(); 53 | 54 | bool trackNewestCoarse( 55 | FrameHessian* newFrameHessian, 56 | SE3 &lastToNew_out, int coarsestLvl, 57 | Vec5 minResForAbort, SE3 slastToLast, int tries, 58 | IOWrap::Output3DWrapper* wrap=0); 59 | 60 | bool TrackNewestToSelectCam( 61 | SE3 &lastToNew_out, int coarsestLvl, 62 | Vec5 minResForAbort, Vec6& lastRes, bool first = false); 63 | void recurrent( 64 | int num,int numUse, 65 | SE3 &lastToNew_out, int coarsestLvl, 66 | Vec5 &minResForAbort); 67 | 68 | void setCoarseTrackingRef( 69 | std::vector frameHessians, 70 | std::vector idxUse); 71 | 72 | // 2019.11.15 yzk 73 | void makeCoarseDepthForFirstFrame(FrameHessian* fh); 74 | void setCTRefForFirstFrame(std::vector frameHessians); 75 | // 2019.11.15 yzk 76 | 77 | void makeK(CalibHessian* HCalib); 78 | 79 | bool debugPrint, debugPlot; 80 | 81 | 82 | Mat33f K[CAM_NUM][PYR_LEVELS]; 83 | Mat33f Ki[CAM_NUM][PYR_LEVELS]; 84 | float fx[CAM_NUM][PYR_LEVELS]; 85 | float fy[CAM_NUM][PYR_LEVELS]; 86 | float fxi[CAM_NUM][PYR_LEVELS]; 87 | float fyi[CAM_NUM][PYR_LEVELS]; 88 | float cx[CAM_NUM][PYR_LEVELS]; 89 | float cy[CAM_NUM][PYR_LEVELS]; 90 | float cxi[CAM_NUM][PYR_LEVELS]; 91 | float cyi[CAM_NUM][PYR_LEVELS]; 92 | 93 | 94 | int w[PYR_LEVELS]; 95 | int h[PYR_LEVELS]; 96 | 97 | std::vector idxUse; 98 | bool weight[CAM_NUM]; 99 | 100 | void debugPlotIDepthMap(float* minID, float* maxID, std::vector &wraps); 101 | void debugPlotIDepthMapFloat(std::vector &wraps); 102 | 103 | FrameHessian* lastRef; //!< 参考帧 104 | // AffLight lastRef_aff_g2l; 105 | AffLight lastRef_aff_g2l[CAM_NUM]; 106 | FrameHessian* newFrame; //!< 新来的一帧 107 | int refFrameID; //!< 参考帧id 108 | 109 | bool hasChanged; 110 | 111 | // act as pure ouptut 112 | Vec6 lastResiduals; 113 | Vec3 lastFlowIndicators; //!< 光流指示用, 只有平移和, 旋转+平移的像素移动 114 | double firstCoarseRMSE; 115 | private: 116 | 117 | 118 | void makeCoarseDepthL0(std::vector frameHessians); 119 | 120 | float* idepth[CAM_NUM][PYR_LEVELS]; 121 | float* weightSums[PYR_LEVELS]; 122 | float* weightSums_bak[PYR_LEVELS]; 123 | 124 | 125 | Vec6 calcResAndGS(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH); 126 | void choose_point(int lvl,std::vector>& point_choose,std::vector>& index); 127 | Vec6 calcRes(int lvl, const SE3 &refToNew, const SE3 refToNews[], SE3 slastToLast, float cutoffTH); 128 | Vec6 calcRes_ransac(int lvl, const SE3 &refToNew, const SE3 refToNews[], SE3 slastToLast, float cutoffTH,std::vector> point_choose); 129 | void cal_InitPoint(int lvl, const SE3 &refToNew, const SE3 refToNews[],std::vector>& index); 130 | int cal_InPoint(const SE3 &refToNew, const SE3 refToNews[],int coarsestLvl); 131 | Vec6 calcResAndInpointnum(int lvl, const SE3 &refToNew, const SE3 refToNews[], SE3 slastToLast, float cutoffTH,int& In_num); 132 | void preCompute(int lvl); 133 | Vec6 calcResInv(int lvl, const SE3 &refToNew, float cutoffTH, Mat66f& H, Vec6f& res); 134 | void ProjectForVisualization( 135 | std::vector>& cld, 136 | SE3 T_lb3_lidar, float res, bool before); 137 | bool getCloseViewObs(const float& x, const float& y, const float& id, const SE3 refToNews[], int idx_ori, int lvl, int& min_idx); 138 | Vec3 calcExtrinRes( 139 | std::vector>& cld, 140 | const SE3 &T_lb3_lidar, float cutoffTH, Mat66f& H, Vec6f& J_res, int itr); 141 | Vec3 calcExtrinRes( 142 | std::vector>& cld, 143 | const SE3 &T_lb3_lidar, float cutoffTH); 144 | void calcGSSSE(int lvl, Mat66 &H_out, Vec6 &b_out, SE3 lastToNew, SE3 slastToLast); 145 | void calcGSSSE_MutualPSimple(int lvl, Mat66 &H_out, Vec6 &b_out, SE3 lastToNew, SE3 slastToLast); 146 | void calcGS(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l); 147 | 148 | // pc buffers 149 | float* pc_u[CAM_NUM][PYR_LEVELS]; //!< 每层上的有逆深度点的坐标x 150 | float* pc_v[CAM_NUM][PYR_LEVELS]; //!< 每层上的有逆深度点的坐标y 151 | float* pc_idepth[CAM_NUM][PYR_LEVELS]; //!< 每层上点的逆深度 152 | float* pc_color[CAM_NUM][PYR_LEVELS]; //!< 每层上点的颜色值 153 | float* pc_Idu[CAM_NUM][PYR_LEVELS]; //!< 每层上点的颜色值在u方向上的梯度 154 | float* pc_Idv[CAM_NUM][PYR_LEVELS]; //!< 每层上点的颜色值在v方向上的梯度 155 | int pc_n[CAM_NUM][PYR_LEVELS]; //!< 每层上点的个数 156 | 157 | 158 | // warped buffers 159 | float* buf_warped_X; //!< 投影得到的点在主相机坐标系下的X 160 | float* buf_warped_Y; //!< 投影得到的点在主相机坐标系下的Y 161 | float* buf_warped_Z; //!< 投影得到的点在主相机坐标系下的Z 162 | float* buf_warped_idepth; //!< 投影得到的点的逆深度 163 | float* buf_warped_u; //!< 投影得到的归一化坐标 164 | float* buf_warped_v; //!< 同上 165 | float* buf_warped_dx; //!< 投影点的图像梯度 166 | float* buf_warped_dy; //!< 同上 167 | float* buf_warped_residual; //!< 投影得到的残差 168 | float* buf_warped_weight; //!< 投影的huber函数权重 169 | float* buf_warped_refColor; //!< 投影点参考帧上的灰度值 170 | int buf_warped_n; //!< 投影点的个数 171 | int buf_warped[CAM_NUM*CAM_NUM]; 172 | Vec6f* buf_dI_dT[PYR_LEVELS]; 173 | Mat66f* buf_H[PYR_LEVELS]; 174 | 175 | 176 | std::vector ptrToDelete; //!< 所有的申请的内存指针, 用于析构删除 177 | 178 | Accumulator acc; 179 | Accumulator7 acc7; 180 | }; 181 | 182 | 183 | class CoarseDistanceMap { 184 | public: 185 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 186 | 187 | CoarseDistanceMap(int w, int h); 188 | ~CoarseDistanceMap(); 189 | 190 | void makeDistanceMap( 191 | std::vector frameHessians, 192 | FrameHessian* frame, int cam_idx); 193 | 194 | void makeInlierVotes( 195 | std::vector frameHessians); 196 | 197 | void makeK( CalibHessian* HCalib, int cam_idx); 198 | 199 | 200 | float* fwdWarpedIDDistFinal; //!< 距离场的数值 201 | 202 | Mat33f K[PYR_LEVELS]; 203 | Mat33f Ki[PYR_LEVELS]; 204 | float fx[PYR_LEVELS]; 205 | float fy[PYR_LEVELS]; 206 | float fxi[PYR_LEVELS]; 207 | float fyi[PYR_LEVELS]; 208 | float cx[PYR_LEVELS]; 209 | float cy[PYR_LEVELS]; 210 | float cxi[PYR_LEVELS]; 211 | float cyi[PYR_LEVELS]; 212 | int w[PYR_LEVELS]; 213 | int h[PYR_LEVELS]; 214 | 215 | void addIntoDistFinal(int u, int v); 216 | 217 | 218 | private: 219 | 220 | PointFrameResidual** coarseProjectionGrid; 221 | int* coarseProjectionGridNum; 222 | Eigen::Vector2i* bfsList1; //!< 投影到frame的坐标 223 | Eigen::Vector2i* bfsList2; //!< 和1轮换使用 224 | 225 | void growDistBFS(int bfsNum); 226 | }; 227 | 228 | } 229 | 230 | -------------------------------------------------------------------------------- /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 | // ImmaturePoint的状态, 残差和目标帧 35 | struct ImmaturePointTemporaryResidual 36 | { 37 | public: 38 | ResState state_state; //!< 逆深度残差的状态 39 | double state_energy; //!< 残差值 40 | ResState state_NewState; //!< 新计算的逆深度残差的状态 41 | double state_NewEnergy; //!< 新计算的残差值 42 | frame_hessian* target; 43 | }; 44 | 45 | 46 | enum ImmaturePointStatus { 47 | IPS_GOOD=0, // traced well and good 48 | // 搜索区间超出图像, 尺度变化太大, 两次残差都大于阈值, 不再搜索 49 | IPS_OOB, // OOB: end tracking & marginalize! 50 | // 第一次残差大于阈值, 外点 51 | IPS_OUTLIER, // energy too high: if happens again: outlier! 52 | // 搜索区间太短，但是没激活 53 | IPS_SKIPPED, // traced well and good (but not actually traced). 54 | // 梯度和极线夹角太大 55 | IPS_BADCONDITION, // not traced because of bad condition. 56 | IPS_UNINITIALIZED}; // not even traced once. 57 | 58 | 59 | class ImmaturePoint 60 | { 61 | public: 62 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 63 | // static values 64 | float color[MAX_RES_PER_POINT]; //!< 原图上pattern上对应的像素值 65 | float weights[MAX_RES_PER_POINT]; //!< 原图上pattern对应的权重(与梯度成反比) 66 | 67 | // 2019.12.04 yzk 68 | float idepth_fromSensor; 69 | // 2019.12.04 yzk 70 | 71 | static const int halfpatch_size_ = 4; 72 | static const int patch_size_ = 8; 73 | 74 | Mat22f gradH; //!< 图像梯度hessian矩阵 75 | Vec2f gradH_ev; 76 | Mat22f gradH_eig; 77 | float energyTH; 78 | float u,v; //!< host里的像素坐标 79 | frame_hessian* host; // 2022.1.11 80 | int idxInImmaturePoints; 81 | 82 | float quality; //!< 第二误差/第一误差 作为搜索质量, 越大越好 83 | 84 | float my_type; 85 | 86 | float idepth_min; //!< 逆深度范围 87 | float idepth_max; 88 | 89 | // 2019.12.02 yzk 90 | bool isFromSensor; 91 | // 2019.12.02 yzk 92 | 93 | ImmaturePoint(int u_, int v_, frame_hessian* host_, float type, CalibHessian* HCalib); 94 | ~ImmaturePoint(); 95 | 96 | ImmaturePointStatus traceOn(frame_hessian* frame, const Mat33f &hostToFrame_KRKi, const Vec3f &hostToFrame_Kt, const Vec2f &hostToFrame_affine, CalibHessian* HCalib, bool debugPrint=false); 97 | 98 | ImmaturePointStatus lastTraceStatus; //!< 上一次跟踪状态 99 | Vec2f lastTraceUV; //!< 上一次搜索得到的位置 100 | float lastTracePixelInterval; //!< 上一次的搜索范围长度 101 | 102 | float idepth_GT; 103 | 104 | double linearizeResidual( 105 | CalibHessian * HCalib, const float outlierTHSlack, 106 | ImmaturePointTemporaryResidual* tmpRes, 107 | float &Hdd, float &bd, 108 | float idepth); 109 | void resetPosition(float u_, float v_); 110 | double linearizeResPixel( 111 | CalibHessian * HCalib, const float outlierTHSlack, 112 | ImmaturePointTemporaryResidual* tmpRes, 113 | cv::Mat &Hdd, cv::Mat &bd, int level_ref, 114 | float idepth); 115 | double calcResPixel( 116 | CalibHessian * HCalib, const float outlierTHSlack, 117 | ImmaturePointTemporaryResidual* tmpRes, 118 | float idepth, float res); 119 | double ResPixel( 120 | const float outlierTHSlack, 121 | FrameHessian* target, float u, float v, 122 | cv::Mat &Hdd, cv::Mat &bd, int level, 123 | float mean_diff, float idepth); 124 | void getWarpMatrixAffine( 125 | const double idepth, 126 | const Mat33f &KRKi, const Vec3f &Kt, 127 | Mat22f& A_cur_ref); 128 | int getBestSearchLevel( 129 | const Mat22f& A_cur_ref, 130 | const int max_level); 131 | float getdPixdd( 132 | CalibHessian * HCalib, 133 | ImmaturePointTemporaryResidual* tmpRes, 134 | float idepth); 135 | 136 | float calcResidual( 137 | CalibHessian * HCalib, const float outlierTHSlack, 138 | ImmaturePointTemporaryResidual* tmpRes, 139 | float idepth); 140 | 141 | private: 142 | }; 143 | } 144 | 145 | -------------------------------------------------------------------------------- /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 | // 2020.07.02 yzk 29 | #include 30 | #include "FullSystem/HessianBlocks.h" 31 | // 2020.07.02 yzk 32 | 33 | namespace dso 34 | { 35 | 36 | enum PixelSelectorStatus {PIXSEL_VOID=0, PIXSEL_1, PIXSEL_2, PIXSEL_3}; 37 | 38 | 39 | class FrameHessian; 40 | // 2022.1.10 41 | class frame_hessian; 42 | // 43 | 44 | class PixelSelector 45 | { 46 | public: 47 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 48 | // 2020.07.02 49 | int makeMaps( 50 | const frame_hessian* const fh, 51 | float* map_out, float density, int recursionsLeft=1, bool plot=false, float thFactor=1); 52 | // 2020.07.02 53 | 54 | int makeMapsFromLidar(const frame_hessian* const fh, float* map_out, float density, int recursionsLeft, 55 | bool plot, float thFactor, std::vector> &vCloudPixel, int id=0); 56 | Eigen::Matrix simpleMakeMapsFromLidar(const FrameHessian* const Fh, float* map_out, float density, int recursionsLeft, 57 | bool plot, std::vector> vCloudPixel[CAM_NUM], int id=0); 58 | 59 | PixelSelector(int w, int h); 60 | ~PixelSelector(); 61 | int currentPotential; //!< 当前选择像素点的潜力, 就是网格大小, 越大选点越少 62 | 63 | float currentTH; 64 | 65 | bool allowFast; 66 | void makeHists(const frame_hessian* const fh); 67 | 68 | private: 69 | Eigen::Vector3i selectFromLidar(const frame_hessian* const fh, float* map_out, int pot, float thFactor, 70 | std::vector> &vCloudPixel, int idx=0); 71 | Eigen::Matrix simpleSelectFromLidar(const FrameHessian* const fh, float* map_out, float numWant, 72 | std::vector> vCloudPixel[CAM_NUM], int _idx=0); 73 | 74 | 75 | Eigen::Vector3i select(const frame_hessian* const fh, 76 | float* map_out, int pot, float thFactor=1); 77 | 78 | 79 | unsigned char* randomPattern; 80 | 81 | 82 | int* gradHist; //!< 根号梯度平方和分布直方图, 0是所有像素个数 83 | float* ths; //!< 平滑之前的阈值 84 | float* thsSmoothed; //!< 平滑后的阈值 85 | int thsStep; 86 | const frame_hessian* gradHistFrame; 87 | }; 88 | 89 | 90 | 91 | 92 | } 93 | 94 | -------------------------------------------------------------------------------- /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 | //@ 返回u,v的导数值 46 | EIGEN_STRONG_INLINE Vec2f derive_uv( 47 | const Mat33f &R, const float &u, const float &v, 48 | const float &fx, const float &fy, 49 | const float &Ix, const float &Iy, 50 | const float &Ix1, const float &Iy1, 51 | const float &drescale) 52 | { 53 | return Vec2f(drescale* (Ix*(R(0,0)-R(2,0)*u) + Iy*fy/fx*(R(1,0)-R(2,0)*v)) - Ix1, 54 | drescale* (Ix*fx/fy*(R(0,1)-R(2,1)*u) + Iy*(R(1,1)-v*R(2,1))) - Iy1); 55 | } 56 | 57 | //@ 把host上的点变换到target上 58 | EIGEN_STRONG_INLINE bool projectPoint( 59 | const float &u_pt,const float &v_pt, 60 | const float &idepth, 61 | const Mat33f &KRKi, const Vec3f &Kt, 62 | float &Ku, float &Kv, int cam_idx) 63 | { 64 | Vec3f ptp = KRKi * Vec3f(u_pt,v_pt, 1) + Kt*idepth; // host上点除深度 65 | Ku = ptp[0] / ptp[2]; 66 | Kv = ptp[1] / ptp[2]; 67 | // 2021.12.28 68 | return Ku>1.1f && Kv>1.1f && Kucxl(cam_idx_i))*HCalib->fxli(cam_idx_i), 89 | (v_pt+dy-HCalib->cyl(cam_idx_i))*HCalib->fyli(cam_idx_i), 90 | 1); 91 | 92 | Vec3f ptp = R * KliP + t*idepth; 93 | drescale = 1.0f/ptp[2]; // target帧逆深度 比 host帧逆深度 94 | new_idepth = idepth*drescale; // 新的帧上逆深度 95 | 96 | if(!(drescale>0)) return false; 97 | 98 | // 归一化平面 99 | u = ptp[0] * drescale; 100 | v = ptp[1] * drescale; 101 | // 像素平面 102 | Ku = u*HCalib->fxl(cam_idx_j) + HCalib->cxl(cam_idx_j); 103 | Kv = v*HCalib->fyl(cam_idx_j) + HCalib->cyl(cam_idx_j); 104 | 105 | // 2021.11.25 106 | return Ku>1.1f && Kv>1.1f && Ku0)) return false; 134 | 135 | // 归一化平面 136 | u = ptp[0] * drescale; 137 | v = ptp[1] * drescale; 138 | // 像素平面 139 | Ku = u*fxG[cam_idx][lvl] + cxG[cam_idx][lvl]; 140 | Kv = v*fyG[cam_idx][lvl] + cyG[cam_idx][lvl]; 141 | 142 | // 2021.11.25 143 | return Ku>1.1f && Kv>1.1f && Ku<(wG[lvl]-2) && Kv<(hG[lvl]-2) && maskG[cam_idx][lvl][(int)Ku+(int)Kv*wG[lvl]]; 144 | // 2021.11.25 145 | } 146 | 147 | EIGEN_STRONG_INLINE bool projectPoint( 148 | const Eigen::Vector3f pt, int lvl, 149 | Vec3f &pt_lb3, 150 | CalibHessian* const &HCalib, 151 | const Mat33f &R_lb3l, const Vec3f &t_lb3l, 152 | const Mat33f &R_cl, const Vec3f &t_cl, 153 | const Mat33f &R, const Vec3f &t, 154 | float &u_src, float &v_src, 155 | float &u, float &v, 156 | float &Ku_src, float &Kv_src, 157 | float &Ku, float &Kv, 158 | float &idepth, float &new_idepth, 159 | int cam_idx)// 2021.11.18 160 | { 161 | pt_lb3 = R_lb3l * pt + t_lb3l; 162 | Vec3f p_c = R_cl * pt + t_cl; 163 | // host上归一化平面点 164 | u_src = p_c[0]/p_c[2]; 165 | v_src = p_c[1]/p_c[2]; 166 | idepth = 1/p_c[2]; 167 | 168 | // 像素平面 169 | Ku_src = u_src*fxG[cam_idx][lvl] + cxG[cam_idx][lvl]; 170 | Kv_src = v_src*fyG[cam_idx][lvl] + cyG[cam_idx][lvl]; 171 | 172 | if(!(Ku_src>1.1f && Kv_src>1.1f && Ku_src0)) return false; 183 | 184 | // 像素平面 185 | Ku = u*fxG[cam_idx][lvl] + cxG[cam_idx][lvl]; 186 | Kv = v*fyG[cam_idx][lvl] + cyG[cam_idx][lvl]; 187 | 188 | 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 frame_hessian; 41 | class CalibHessian; 42 | 43 | class EFResidual; 44 | 45 | enum ResLocation {ACTIVE=0, LINEARIZED, MARGINALIZED, NONE}; 46 | enum ResState {IN=0, OOB, OUTLIER}; // IN在内部, OOB 点超出图像, OUTLIER有外点 47 | 48 | struct FullJacRowT 49 | { 50 | Eigen::Vector2f projectedTo[MAX_RES_PER_POINT]; 51 | }; 52 | 53 | class PointFrameResidual 54 | { 55 | public: 56 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 57 | 58 | EFResidual* efResidual; 59 | 60 | static int instanceCounter; 61 | 62 | 63 | ResState state_state; //!< 上一次的残差状态 64 | double state_energy; //!< 上一次的能量值 65 | ResState state_NewState; //!< 新的一次计算的状态 66 | double state_NewEnergy; //!< 新的能量, 如果大于阈值则把等于阈值 67 | double state_NewEnergyWithOutlier; //!< 可能具有外点的能量, 可能大于阈值 68 | 69 | 70 | void setState(ResState s) {state_state = s;} 71 | 72 | 73 | PointHessian* point; //!< 点 74 | // 2022.1.11 75 | frame_hessian* host; //!< 主帧 76 | frame_hessian* target; //!< 目标帧// 77 | RawResidualJacobian* J; //!< 残差对变量的各种雅克比 78 | 79 | 80 | bool isNew; 81 | 82 | 83 | Eigen::Vector2f projectedTo[MAX_RES_PER_POINT]; //!< 各个patch的投影坐标 84 | Vec3f centerProjectedTo; //!< patch的中心点投影 [像素x, 像素y, 新帧逆深度] 85 | 86 | ~PointFrameResidual(); 87 | PointFrameResidual(); 88 | PointFrameResidual(PointHessian* point_, frame_hessian* host_, frame_hessian* target_); 89 | double linearize(CalibHessian* HCalib); 90 | 91 | 92 | void resetOOB() 93 | { 94 | state_NewEnergy = state_energy = 0; 95 | state_NewState = ResState::OUTLIER; 96 | 97 | setState(ResState::IN); 98 | }; 99 | void applyRes( bool copyJacobians); 100 | 101 | void debugPlot(); 102 | 103 | void printRows(std::vector &v, VecX &r, int nFrames, int nPoints, int M, int res); 104 | }; 105 | } 106 | 107 | -------------------------------------------------------------------------------- /src/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 | // 2020.06.22 yzk 31 | #include 32 | // 2020.06.22 yzk 33 | 34 | namespace dso 35 | { 36 | namespace IOWrap 37 | { 38 | 39 | MinimalImageB* readImageBW_8U(std::string filename); 40 | MinimalImageB3* readImageRGB_8U(std::string filename); 41 | MinimalImage* readImageBW_16U(std::string filename); 42 | // 2019.11.07 yzk 43 | MinimalImage* readDepthImageBW_16U(std::string filename); 44 | // 2019.11.07 yzk 45 | // 2020.06.22 yzk 46 | MinimalImageB* readRosImageBW_8U(cv::Mat &image); 47 | // 2020.06.22 yzk 48 | 49 | MinimalImageB* readStreamBW_8U(char* data, int numBytes); 50 | 51 | void writeImage(std::string filename, MinimalImageB* img); 52 | void writeImage(std::string filename, MinimalImageB3* img); 53 | void writeImage(std::string filename, MinimalImageF* img); 54 | void writeImage(std::string filename, MinimalImageF3* img); 55 | 56 | } 57 | } 58 | -------------------------------------------------------------------------------- /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 | #include 29 | #include 30 | 31 | 32 | namespace dso 33 | { 34 | 35 | namespace IOWrap 36 | { 37 | MinimalImageB* readImageBW_8U(std::string filename) 38 | { 39 | cv::Mat m = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE); 40 | if(m.rows*m.cols==0) 41 | { 42 | printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str()); 43 | return 0; 44 | } 45 | if(m.type() != CV_8U) 46 | { 47 | printf("cv::imread did something strange! this may segfault. \n"); 48 | return 0; 49 | } 50 | MinimalImageB* img = new MinimalImageB(m.cols, m.rows); 51 | memcpy(img->data, m.data, m.rows*m.cols); 52 | return img; 53 | } 54 | 55 | // 2020.06.22 yzk 56 | MinimalImageB* readRosImageBW_8U(cv::Mat &image) 57 | { 58 | cv::Mat m = image.clone(); 59 | //cv::imshow("raw image", m); 60 | //cv::waitKey(0); 61 | if(m.rows*m.cols==0) 62 | { 63 | printf("cv::imread could not read image. \n"); 64 | return 0; 65 | } 66 | if(m.type() != CV_8U) 67 | { 68 | printf("cv::imread did something strange! this may segfault. \n"); 69 | return 0; 70 | } 71 | MinimalImageB* img = new MinimalImageB(m.cols, m.rows); 72 | memcpy(img->data, m.data, m.rows*m.cols); 73 | 74 | return img; 75 | } 76 | // 2020.06.22 yzk 77 | 78 | MinimalImageB3* readImageRGB_8U(std::string filename) 79 | { 80 | cv::Mat m = cv::imread(filename, CV_LOAD_IMAGE_COLOR); 81 | if(m.rows*m.cols==0) 82 | { 83 | printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str()); 84 | return 0; 85 | } 86 | if(m.type() != CV_8UC3) 87 | { 88 | printf("cv::imread did something strange! this may segfault. \n"); 89 | return 0; 90 | } 91 | MinimalImageB3* img = new MinimalImageB3(m.cols, m.rows); 92 | memcpy(img->data, m.data, 3*m.rows*m.cols); 93 | return img; 94 | } 95 | 96 | MinimalImage* readImageBW_16U(std::string filename) 97 | { 98 | cv::Mat m = cv::imread(filename, CV_LOAD_IMAGE_UNCHANGED); 99 | if(m.rows*m.cols==0) 100 | { 101 | printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str()); 102 | return 0; 103 | } 104 | if(m.type() != CV_16U) 105 | { 106 | printf("readImageBW_16U called on image that is not a 16bit grayscale image. this may segfault. \n"); 107 | return 0; 108 | } 109 | MinimalImage* img = new MinimalImage(m.cols, m.rows); 110 | memcpy(img->data, m.data, 2*m.rows*m.cols); 111 | return img; 112 | } 113 | 114 | // 2019.11.07 yzk 115 | MinimalImage* readDepthImageBW_16U(std::string filename) 116 | { 117 | cv::Mat m = cv::imread(filename, CV_LOAD_IMAGE_UNCHANGED); 118 | //cv::imshow("original depthimage", m); 119 | if(m.rows*m.cols==0) 120 | { 121 | printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str()); 122 | return 0; 123 | } 124 | if(m.type() != CV_16U) 125 | { 126 | printf("readImageBW_16U called on image that is not a 16bit grayscale image. this may segfault. \n"); 127 | return 0; 128 | } 129 | MinimalImage* img = new MinimalImage(m.cols, m.rows); 130 | memcpy(img->data, m.data, 2 * m.rows*m.cols); 131 | //memcpy(m.data, img->data, m.rows*m.cols); 132 | //cv::imshow("second depthimage", m); 133 | return img; 134 | } 135 | // 2019.11.07 yzk 136 | 137 | MinimalImageB* readStreamBW_8U(char* data, int numBytes) 138 | { 139 | cv::Mat m = cv::imdecode(cv::Mat(numBytes,1,CV_8U, data), CV_LOAD_IMAGE_GRAYSCALE); 140 | if(m.rows*m.cols==0) 141 | { 142 | printf("cv::imdecode could not read stream (%d bytes)! this may segfault. \n", numBytes); 143 | return 0; 144 | } 145 | if(m.type() != CV_8U) 146 | { 147 | printf("cv::imdecode did something strange! this may segfault. \n"); 148 | return 0; 149 | } 150 | MinimalImageB* img = new MinimalImageB(m.cols, m.rows); 151 | memcpy(img->data, m.data, m.rows*m.cols); 152 | return img; 153 | } 154 | 155 | 156 | 157 | void writeImage(std::string filename, MinimalImageB* img) 158 | { 159 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_8U, img->data)); 160 | } 161 | void writeImage(std::string filename, MinimalImageB3* img) 162 | { 163 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_8UC3, img->data)); 164 | } 165 | void writeImage(std::string filename, MinimalImageF* img) 166 | { 167 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_32F, img->data)); 168 | } 169 | void writeImage(std::string filename, MinimalImageF3* img) 170 | { 171 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_32FC3, img->data)); 172 | } 173 | 174 | } 175 | 176 | } 177 | -------------------------------------------------------------------------------- /src/IOWrapper/Output3DWrapper.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 29 | 30 | #include "util/NumType.h" 31 | #include "util/MinimalImage.h" 32 | #include "map" 33 | 34 | namespace cv { 35 | class Mat; 36 | } 37 | 38 | namespace dso 39 | { 40 | 41 | class FrameHessian; 42 | class CalibHessian; 43 | class FrameShell; 44 | 45 | namespace IOWrap 46 | { 47 | 48 | /* ======================= Some typical usecases: =============== 49 | * 50 | * (1) always get the pose of the most recent frame: 51 | * -> Implement [publishCamPose]. 52 | * 53 | * (2) always get the depthmap of the most recent keyframe 54 | * -> Implement [pushDepthImageFloat] (use inverse depth in [image], and pose / frame information from [KF]). 55 | * 56 | * (3) accumulate final model 57 | * -> Implement [publishKeyframes] (skip for final!=false), and accumulate frames. 58 | * 59 | * (4) get evolving model in real-time 60 | * -> Implement [publishKeyframes] (update all frames for final==false). 61 | * 62 | * 63 | * 64 | * 65 | * ==================== How to use the structs: =================== 66 | * [FrameShell]: minimal struct kept for each frame ever tracked. 67 | * ->camToWorld = camera to world transformation 68 | * ->poseValid = false if [camToWorld] is invalid (only happens for frames during initialization). 69 | * ->trackingRef = Shell of the frame this frame was tracked on. 70 | * ->id = ID of that frame, starting with 0 for the very first frame. 71 | * 72 | * ->incoming_id = ID passed into [addActiveFrame( ImageAndExposure* image, int id )]. 73 | * ->timestamp = timestamp passed into [addActiveFrame( ImageAndExposure* image, int id )] as image.timestamp. 74 | * 75 | * [FrameHessian] 76 | * ->immaturePoints: contains points that have not been "activated" (they do however have a depth initialization). 77 | * ->pointHessians: contains active points. 78 | * ->pointHessiansMarginalized: contains marginalized points. 79 | * ->pointHessiansOut: contains outlier points. 80 | * 81 | * ->frameID: incremental ID for keyframes only. 82 | * ->shell: corresponding [FrameShell] struct. 83 | * 84 | * 85 | * [CalibHessian] 86 | * ->fxl(), fyl(), cxl(), cyl(): get optimized, most recent (pinhole) camera intrinsics. 87 | * 88 | * 89 | * [PointHessian] 90 | * ->u,v: pixel-coordinates of point. 91 | * ->idepth_scaled: inverse depth of point. 92 | * DO NOT USE [idepth], since it may be scaled with [SCALE_IDEPTH] ... however that is currently set to 1 so never mind. 93 | * ->host: pointer to host-frame of point. 94 | * ->status: current status of point (ACTIVE=0, INACTIVE, OUTLIER, OOB, MARGINALIZED) 95 | * ->numGoodResiduals: number of non-outlier residuals supporting this point (approximate). 96 | * ->maxRelBaseline: value roughly proportional to the relative baseline this point was observed with (0 = no baseline). 97 | * points for which this value is low are badly contrained. 98 | * ->idepth_hessian: hessian value (inverse variance) of inverse depth. 99 | * 100 | * [ImmaturePoint] 101 | * ->u,v: pixel-coordinates of point. 102 | * ->idepth_min, idepth_max: the initialization sais that the inverse depth of this point is very likely 103 | * between these two thresholds (their mean being the best guess) 104 | * ->host: pointer to host-frame of point. 105 | */ 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | class Output3DWrapper 114 | { 115 | public: 116 | Output3DWrapper() {} 117 | virtual ~Output3DWrapper() {} 118 | 119 | 120 | /* Usage: 121 | * Called once after each new Keyframe is inserted & optimized. 122 | * [connectivity] contains for each frame-frame pair the number of [0] active residuals in between them, 123 | * and [1] the number of marginalized reisduals between them. 124 | * frame-frame pairs are encoded as HASH_IDX = [(int)hostFrameKFID << 32 + (int)targetFrameKFID]. 125 | * the [***frameKFID] used for hashing correspond to the [FrameHessian]->frameID. 126 | * 127 | * Calling: 128 | * Always called, no overhead if not used. 129 | */ 130 | virtual void publishGraph(const std::map, Eigen::aligned_allocator > > &connectivity) {} 131 | 132 | 133 | 134 | 135 | 136 | /* Usage: 137 | * Called after each new Keyframe is inserted & optimized, with all keyframes that were part of the active window during 138 | * that optimization in [frames] (with final=false). Use to access the new frame pose and points. 139 | * Also called just before a frame is marginalized (with final=true) with only that frame in [frames]; at that point, 140 | * no further updates will ever occur to it's optimized values (pose & idepth values of it's points). 141 | * 142 | * If you want always all most recent values for all frames, just use the [final=false] calls. 143 | * If you only want to get the final model, but don't care about it being delay-free, only use the 144 | * [final=true] calls, to save compute. 145 | * 146 | * Calling: 147 | * Always called, negligible overhead if not used. 148 | */ 149 | virtual void publishKeyframes(std::vector &frames, bool final, CalibHessian* HCalib) {} 150 | 151 | 152 | 153 | 154 | 155 | /* Usage: 156 | * Called once for each tracked frame, with the real-time, low-delay frame pose. 157 | * 158 | * Calling: 159 | * Always called, no overhead if not used. 160 | */ 161 | virtual void publishCamPose(FrameShell* frame, CalibHessian* HCalib) {} 162 | 163 | 164 | 165 | 166 | 167 | /* Usage: 168 | * Called once for each new frame, before it is tracked (i.e., it doesn't have a pose yet). 169 | * 170 | * Calling: 171 | * Always called, no overhead if not used. 172 | */ 173 | virtual void pushLiveFrame(FrameHessian* image) {} 174 | 175 | 176 | 177 | 178 | /* called once after a new keyframe is created, with the color-coded, forward-warped inverse depthmap for that keyframe, 179 | * which is used for initial alignment of future frames. Meant for visualization. 180 | * 181 | * Calling: 182 | * Needs to prepare the depth image, so it is only called if [needPushDepthImage()] returned true. 183 | */ 184 | // 2021.11.23 185 | virtual void pushDepthImage(std::vector& image) {} 186 | // 2021.11.23 187 | virtual bool needPushDepthImage() {return false;} 188 | 189 | 190 | 191 | /* Usage: 192 | * called once after a new keyframe is created, with the forward-warped inverse depthmap for that keyframe. 193 | * (<= 0 for pixels without inv. depth value, >0 for pixels with inv. depth value) 194 | * 195 | * Calling: 196 | * Always called, almost no overhead if not used. 197 | */ 198 | virtual void pushDepthImageFloat(MinimalImageF* image, FrameHessian* KF ) {} 199 | 200 | 201 | 202 | /* call on finish */ 203 | virtual void join() {} 204 | 205 | /* call on reset */ 206 | virtual void reset() {} 207 | 208 | }; 209 | 210 | } 211 | } 212 | -------------------------------------------------------------------------------- /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->frame[0]->pointHessians.size(), (int)f->frame[0]->pointHessiansMarginalized.size(), (int)f->frame[0]->immaturePoints.size());// 2022.1.11 87 | std::cout << f->shell->camToWorld.matrix3x4() << "\n"; 88 | 89 | 90 | int maxWrite = 5; 91 | for(PointHessian* p : f->frame[0]->pointHessians)// 2022.1.11 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(std::vector& 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 | // 2021.12.14 35 | #include "util/settings.h" 36 | // 2021.12.14 37 | 38 | namespace dso 39 | { 40 | class CalibHessian; 41 | class FrameHessian; 42 | class FrameShell; 43 | 44 | namespace IOWrap 45 | { 46 | 47 | template 48 | struct InputPointSparse 49 | { 50 | float u; 51 | float v; 52 | float idpeth; 53 | float idepth_hessian; 54 | float relObsBaseline; 55 | int numGoodRes; 56 | unsigned char color[ppp]; 57 | unsigned char status; 58 | }; 59 | 60 | struct MyVertex 61 | { 62 | float point[3]; 63 | unsigned char color[4]; 64 | }; 65 | 66 | // stores a pointcloud associated to a Keyframe. 67 | class KeyFrameDisplay 68 | { 69 | 70 | public: 71 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 72 | KeyFrameDisplay(); 73 | ~KeyFrameDisplay(); 74 | 75 | // copies points from KF over to internal buffer, 76 | // keeping some additional information so we can render it differently. 77 | void setFromKF(FrameHessian* fh, CalibHessian* HCalib); 78 | 79 | // copies points from KF over to internal buffer, 80 | // keeping some additional information so we can render it differently. 81 | void setFromF(FrameShell* fs, CalibHessian* HCalib); 82 | 83 | // copies & filters internal data to GL buffer for rendering. if nothing to do: does nothing. 84 | bool refreshPC(bool canRefresh, float scaledTH, float absTH, int mode, float minBS, int sparsity); 85 | 86 | // renders cam & pointcloud. 87 | void drawCam(float lineWidth = 1, float* color = 0, float sizeFactor=1); 88 | void drawPC(float pointSize); 89 | 90 | int id; 91 | bool active; 92 | SE3 camToWorld; 93 | // 2021.12.20 94 | SE3 camToWorld_gt; 95 | // 2021.12.20 96 | 97 | inline bool operator < (const KeyFrameDisplay& other) const 98 | { 99 | return (id < other.id); 100 | } 101 | 102 | 103 | private: 104 | // 2021.12.13 105 | /* 106 | float fx,fy,cx,cy; 107 | float fxi,fyi,cxi,cyi; 108 | */ 109 | float fx[CAM_NUM],fy[CAM_NUM],cx[CAM_NUM],cy[CAM_NUM]; 110 | float fxi[CAM_NUM],fyi[CAM_NUM],cxi[CAM_NUM],cyi[CAM_NUM]; 111 | // 2021.12.13 112 | int width, height; 113 | 114 | float my_scaledTH, my_absTH, my_scale; 115 | int my_sparsifyFactor; 116 | int my_displayMode; 117 | float my_minRelBS; 118 | bool needRefresh; 119 | 120 | 121 | // 2021.12.14 122 | // int numSparsePoints; 123 | // int numSparseBufferSize; 124 | // InputPointSparse* originalInputSparse; 125 | int numSparsePoints[1]; 126 | int numSparseBufferSize[1]; 127 | InputPointSparse* originalInputSparse[1]; 128 | // 2021.12.14 129 | 130 | 131 | bool bufferValid; 132 | int numGLBufferPoints; 133 | int numGLBufferGoodPoints; 134 | // 2021.12.14 135 | pangolin::GlBuffer vertexBuffer[1/*CAM_NUM*/]; 136 | pangolin::GlBuffer colorBuffer[1]; 137 | // 2021.12.14 138 | }; 139 | 140 | } 141 | } 142 | -------------------------------------------------------------------------------- /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(std::vector& 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 | // 2021.11.23 97 | MinimalImageB3* internalVideoImg[5]; 98 | MinimalImageB3* internalKFImg[5]; 99 | // 2021.11.23 100 | MinimalImageB3* internalResImg; 101 | bool videoImgChanged, kfImgChanged, resImgChanged; 102 | 103 | 104 | 105 | // 3D model rendering 106 | boost::mutex model3DMutex; 107 | KeyFrameDisplay* currentCam; 108 | std::vector keyframes; 109 | std::vector> allFramePoses; 110 | std::map keyframesByKFID; 111 | std::vector> connections; 112 | 113 | 114 | 115 | // render settings 116 | bool settings_showKFCameras; 117 | bool settings_showCurrentCamera; 118 | bool settings_showTrajectory; 119 | // xiang.2022.1.13 120 | bool settings_showgt; 121 | // xiang.2022.1.13 122 | bool settings_showFullTrajectory; 123 | bool settings_showActiveConstraints; 124 | bool settings_showAllConstraints; 125 | 126 | float settings_scaledVarTH; 127 | float settings_absVarTH; 128 | int settings_pointCloudMode; 129 | float settings_minRelBS; 130 | int settings_sparsity; 131 | 132 | 133 | // timings 134 | struct timeval last_track; 135 | struct timeval last_map; 136 | 137 | 138 | std::deque lastNTrackingMs; 139 | std::deque lastNMappingMs; 140 | }; 141 | 142 | 143 | 144 | } 145 | 146 | 147 | 148 | } 149 | -------------------------------------------------------------------------------- /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 | //* hessian + 边缘化得到hessian + 先验hessian 47 | //点的先验为0 AF 和 LF 其中一个有数值 48 | float H = p->Hdd_accAF+p->Hdd_accLF+p->priorF; 49 | 50 | if(H < 1e-10) H = 1e-10; 51 | 52 | p->data->idepth_hessian=H; //设置到ph去了 53 | p->HdiF = 1.0 / H; 54 | //* 逆深度残差 55 | p->bdSumF = p->bd_accAF + p->bd_accLF; 56 | 57 | if(shiftPriorToZero) p->bdSumF += p->priorF*p->deltaF; 58 | 59 | //* 逆深度和内参的交叉项 60 | VecCf Hcd = p->Hcd_accAF + p->Hcd_accLF; 61 | 62 | 63 | if(p->data->isFromSensor == true) 64 | return; 65 | 66 | 67 | //* schur complement 68 | //! Hcd * Hdd_inv * Hcd^`T 69 | accHcc[tid].update(Hcd,Hcd,p->HdiF); 70 | //! Hcd * Hdd_inv * bd 71 | accbc[tid].update(Hcd, p->bdSumF * p->HdiF); 72 | 73 | //assert(std::isfinite((float)(p->HdiF))); 74 | 75 | int nFrames2 = nframes[tid]*nframes[tid]; 76 | for(EFResidual* r1 : p->residualsAll) 77 | { 78 | if(!r1->isActive()) continue; 79 | int r1ht = r1->hostIDX + r1->targetIDX*nframes[tid]; 80 | 81 | for(EFResidual* r2 : p->residualsAll) 82 | { 83 | if(!r2->isActive()) continue; 84 | //! Hfd_1 * Hdd_inv * Hfd_2^T, f = [xi, a b]位姿 光度 accD是8*8的数组 accE: 8*4 accEB: 8 * 1 85 | accD[tid][r1ht+r2->targetIDX*nFrames2].update(r1->JpJdF, r2->JpJdF, p->HdiF); 86 | } 87 | accE[tid][r1ht].update(r1->JpJdF, Hcd, p->HdiF); 88 | accEB[tid][r1ht].update(r1->JpJdF,p->HdiF*p->bdSumF); 89 | } 90 | } 91 | 92 | //@ 从累加器里面得到 hessian矩阵Schur complement 93 | // XTL 优化时使用 94 | void AccumulatedSCHessianSSE::stitchDoubleInternal( 95 | MatXX* H, VecX* b, EnergyFunctional const * const EF, 96 | int min, int max, Vec10* stats, int tid) 97 | { 98 | int toAggregate = NUM_THREADS; 99 | if(tid == -1) { toAggregate = 1; tid = 0; } // special case: if we dont do multithreading, dont aggregate. 100 | if(min==max) return; 101 | 102 | 103 | int nf = nframes[0]; 104 | int nframes2 = nf*nf; 105 | 106 | for(int k=min;k(); 124 | bp += accEB[tid2][ijIdx].A1m.cast(); 125 | } 126 | //! Hfc部分Schur 127 | H[tid].block<8,CPARS>(iIdx,0) += EF->adHost[ijIdx] * Hpc; 128 | H[tid].block<8,CPARS>(jIdx,0) += EF->adTarget[ijIdx] * Hpc; 129 | //! 位姿,光度部分的残差Schur 130 | b[tid].segment<8>(iIdx) += EF->adHost[ijIdx] * bp; 131 | b[tid].segment<8>(jIdx) += EF->adTarget[ijIdx] * bp; 132 | 133 | 134 | 135 | for(int k=0;k(); 148 | } 149 | //! Hff部分Schur 150 | H[tid].block<8,8>(iIdx, iIdx) += EF->adHost[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); 151 | H[tid].block<8,8>(jIdx, kIdx) += EF->adTarget[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); 152 | H[tid].block<8,8>(jIdx, iIdx) += EF->adTarget[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); 153 | H[tid].block<8,8>(iIdx, kIdx) += EF->adHost[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); 154 | } 155 | } 156 | 157 | if(min==0) 158 | { 159 | for(int tid2=0;tid2 < toAggregate;tid2++) 160 | { 161 | accHcc[tid2].finish(); 162 | accbc[tid2].finish(); 163 | //! Hcc 部分Schur 164 | H[tid].topLeftCorner() += accHcc[tid2].A1m.cast(); 165 | //! 内参部分的残差Schur 166 | b[tid].head() += accbc[tid2].A1m.cast(); 167 | } 168 | } 169 | //最终H 为68 * 68 b 为68 * 1; 170 | 171 | // // ----- new: copy transposed parts for calibration only. 172 | // for(int h=0;h(0,hIdx).noalias() = H.block<8,4>(hIdx,0).transpose(); 176 | // } 177 | } 178 | // XTL 边缘化时使用 179 | void AccumulatedSCHessianSSE::stitchDouble(MatXX &H, VecX &b, EnergyFunctional const * const EF, int tid) 180 | { 181 | 182 | int nf = nframes[0]; 183 | int nframes2 = nf*nf; 184 | 185 | H = MatXX::Zero(nf*8+CPARS, nf*8+CPARS); 186 | b = VecX::Zero(nf*8+CPARS); 187 | 188 | 189 | for(int i=0;i(); 200 | Vec8 accEBV = accEB[tid][ijIdx].A1m.cast(); 201 | 202 | H.block<8,CPARS>(iIdx,0) += EF->adHost[ijIdx] * accEM; 203 | H.block<8,CPARS>(jIdx,0) += EF->adTarget[ijIdx] * accEM; 204 | 205 | b.segment<8>(iIdx) += EF->adHost[ijIdx] * accEBV; 206 | b.segment<8>(jIdx) += EF->adTarget[ijIdx] * accEBV; 207 | 208 | for(int k=0;k(); 217 | 218 | H.block<8,8>(iIdx, iIdx) += EF->adHost[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); 219 | 220 | H.block<8,8>(jIdx, kIdx) += EF->adTarget[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); 221 | 222 | H.block<8,8>(jIdx, iIdx) += EF->adTarget[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); 223 | 224 | H.block<8,8>(iIdx, kIdx) += EF->adHost[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); 225 | } 226 | } 227 | 228 | accHcc[tid].finish(); 229 | accbc[tid].finish(); 230 | H.topLeftCorner() = accHcc[tid].A1m.cast(); 231 | b.head() = accbc[tid].A1m.cast(); 232 | 233 | // ----- new: copy transposed parts for calibration only. 234 | for(int h=0;h(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose(); 238 | } 239 | } 240 | 241 | } 242 | -------------------------------------------------------------------------------- /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]; 74 | accEB[tid] = new AccumulatorX<8>[n*n]; 75 | accD[tid] = new AccumulatorXX<8,8>[n*n*n]; 76 | } 77 | accbc[tid].initialize(); 78 | accHcc[tid].initialize(); 79 | 80 | for(int i=0;i* red, MatXX &H, VecX &b, EnergyFunctional const * const EF, bool MT) 95 | { 96 | // sum up, splitting by bock in square. 97 | if(MT) 98 | { 99 | MatXX Hs[NUM_THREADS]; 100 | VecX bs[NUM_THREADS]; 101 | // 分配空间大小 102 | for(int i=0;ireduce(boost::bind(&AccumulatedSCHessianSSE::stitchDoubleInternal, 110 | this,Hs, bs, EF, _1, _2, _3, _4), 0, nframes[0]*nframes[0], 0); 111 | 112 | // sum up results 113 | H = Hs[0]; 114 | b = bs[0]; 115 | 116 | for(int i=1;i(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose(); 135 | } 136 | } 137 | 138 | // 2021/12/18{ 139 | AccumulatorXX<8,CPARS>* accE[NUM_THREADS]; //!< 位姿和内参关于逆深度的 Schur 140 | AccumulatorX<8>* accEB[NUM_THREADS]; //!< 位姿光度关于逆深度的 b*Schur 141 | AccumulatorXX<8,8>* accD[NUM_THREADS]; //!< 两位姿光度关于逆深度的 Schur 142 | //} 143 | 144 | AccumulatorXX accHcc[NUM_THREADS]; //!< 内参关于逆深度的 Schur 145 | AccumulatorX accbc[NUM_THREADS]; //!< 内参关于逆深度的 b*Schur 146 | int nframes[NUM_THREADS]; 147 | 148 | 149 | void addPointsInternal( 150 | std::vector* points, bool shiftPriorToZero, 151 | int min=0, int max=1, Vec10* stats=0, int tid=0) 152 | { 153 | 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 | // 初始化, 设置初值 79 | for(int i=0;i void addPoint(EFPoint* p, EnergyFunctional const * const ef, int tid=0); 89 | 90 | 91 | // XTL 原版的dso usePrior为false，MT取决于设置 92 | void stitchDoubleMT(IndexThreadReduce* red, MatXX &H, VecX &b, EnergyFunctional const * const EF, bool usePrior, bool MT) 93 | { 94 | // sum up, splitting by bock in square. 95 | if(MT) 96 | { 97 | MatXX Hs[NUM_THREADS]; 98 | VecX bs[NUM_THREADS]; 99 | for(int i=0;ireduce(boost::bind(&AccumulatedTopHessianSSE::stitchDoubleInternal, 108 | this,Hs, bs, EF, usePrior, _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(); //! [内参, 位姿] 对称部分 133 | 134 | for(int t=h+1;t(hIdx, tIdx).noalias() += H.block<8,8>(tIdx, hIdx).transpose(); 139 | H.block<8,8>(tIdx, hIdx).noalias() = H.block<8,8>(hIdx, tIdx).transpose(); 140 | } 141 | } 142 | } 143 | 144 | 145 | 146 | 147 | int nframes[NUM_THREADS]; //!< 每个线程的帧数 148 | 149 | EIGEN_ALIGN16 AccumulatorApprox* acc[NUM_THREADS]; //!< 计算hessian的累乘器 150 | 151 | 152 | int nres[NUM_THREADS]; //!< 残差计数 153 | 154 | 155 | template void addPointsInternal( 156 | std::vector* points, EnergyFunctional const * const ef, 157 | int min=0, int max=1, Vec10* stats=0, int tid=0) 158 | { 159 | for(int i=min;i((*points)[i],ef,tid); 160 | } 161 | 162 | 163 | 164 | private: 165 | 166 | void stitchDoubleInternal( 167 | MatXX* H, VecX* b, EnergyFunctional const * const EF, bool usePrior, 168 | int min, int max, Vec10* stats, int tid); 169 | }; 170 | } 171 | 172 | -------------------------------------------------------------------------------- /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 "time.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 removePoint(EFPoint* ph); 83 | 84 | 85 | 86 | void marginalizePointsF(); 87 | void dropPointsF(); 88 | void solveSystemF(int iteration, double lambda, CalibHessian* HCalib); 89 | double calcMEnergyF(); 90 | double calcLEnergyF_MT(); 91 | 92 | 93 | void makeIDX(); 94 | 95 | void setDeltaF(CalibHessian* HCalib); 96 | 97 | void setAdjointsF(CalibHessian* Hcalib); 98 | 99 | std::vector frames; //!< 能量函数中的帧 100 | int nPoints, nFrames, nResiduals; //!< EFPoint的数目, EFframe关键帧数, 残差数 101 | 102 | MatXX HM; //!< 优化的Hessian矩阵, 边缘化掉逆深度 103 | VecX bM; //!< 优化的Jr项, 边缘化掉逆深度 104 | 105 | int resInA, resInL, resInM; //!< 分别是在计算A, L, 边缘化H和b中残差的数量 106 | MatXX lastHS; 107 | VecX lastbS; 108 | VecX lastX; 109 | 110 | std::vector lastNullspaces_forLogging; 111 | std::vector lastNullspaces_pose; 112 | std::vector lastNullspaces_scale; 113 | std::vector lastNullspaces_affA; 114 | std::vector lastNullspaces_affB; 115 | 116 | IndexThreadReduce* red; 117 | 118 | 119 | std::map, 122 | Eigen::aligned_allocator> // 64位对齐 123 | > connectivityMap; //!< 关键帧之间的连接关系, first: 前32表示host ID, 后32位表示target ID; second:数目 [0] 普通的, [1] 边缘化的 124 | 125 | private: 126 | 127 | VecX getStitchedDeltaF() const; 128 | 129 | void resubstituteF_MT(VecX x, CalibHessian* HCalib, bool MT); 130 | void resubstituteFPt(const VecCf &xc, Mat18f* xAd, int min, int max, Vec10* stats, int tid); 131 | 132 | void accumulateAF_MT(MatXX &H, VecX &b, bool MT); 133 | void accumulateLF_MT(MatXX &H, VecX &b, bool MT); 134 | void accumulateSCF_MT(MatXX &H, VecX &b, bool MT); 135 | 136 | void calcLEnergyPt(int min, int max, Vec10* stats, int tid); 137 | 138 | void orthogonalize(VecX* b, MatXX* H); 139 | 140 | Mat18f* adHTdeltaF; //!< host和target之间位姿的增量, 一共帧数×帧数个 141 | 142 | Mat88* adHost; //!< 伴随矩阵, double 143 | Mat88* adTarget; 144 | 145 | Mat88f* adHostF; //!< 伴随矩阵, float 146 | Mat88f* adTargetF; 147 | 148 | 149 | VecC cPrior; //!< setting_initialCalibHessian 信息矩阵 150 | VecCf cDeltaF; //!< 相机内参增量 151 | VecCf cPriorF; // float型 152 | 153 | AccumulatedTopHessianSSE* accSSE_top_L; //!< 154 | AccumulatedTopHessianSSE* accSSE_top_A; //!< 155 | 156 | 157 | AccumulatedSCHessianSSE* accSSE_bot; 158 | 159 | std::vector allPoints; 160 | std::vector allPointsToMarg; 161 | 162 | float currentLambda; 163 | }; 164 | } 165 | 166 | -------------------------------------------------------------------------------- /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 | // XTL：交换EFResidual的J和PointFrameResidual的J，计算EFResidual的JpJdF 40 | void EFResidual::takeDataF() 41 | { 42 | std::swap(J, data->J); 43 | 44 | Vec2f JI_JI_Jd = J->JIdx2 * J->Jpdd; 45 | 46 | for(int i=0;i<6;i++) 47 | JpJdF[i] = J->Jpdxi[0][i]*JI_JI_Jd[0] + J->Jpdxi[1][i] * JI_JI_Jd[1]; 48 | 49 | JpJdF.segment<2>(6) = J->JabJIdx*J->Jpdd; 50 | } 51 | 52 | 53 | // XTL：创建EFFrame时调用(每次创建关键帧时才会创建)，给位姿（仅第一帧）和光度参数设置prior，同时设置efframe的delta 54 | void EFFrame::takeData() 55 | { 56 | prior = data->getPrior().head<8>(); // 得到先验状态, 主要是光度仿射变换 57 | delta = data->get_state_minus_stateZero().head<8>(); 58 | delta_prior = (data->get_state() - data->getPriorZero()).head<8>(); 59 | 60 | 61 | assert(data->frameID != -1); 62 | 63 | frameID = data->frameID; // 所有帧的ID序号 64 | } 65 | 66 | 67 | 68 | // XTL：创建EFPoint时调用 69 | void EFPoint::takeData() 70 | { 71 | priorF = data->hasDepthPrior ? setting_idepthFixPrior*SCALE_IDEPTH*SCALE_IDEPTH : 0; 72 | if(setting_solverMode & SOLVER_REMOVE_POSEPRIOR) priorF=0; 73 | //in fact 这个priorF 只在初始化时候有， 其他时候统一set to zero 74 | deltaF = data->idepth - data->idepth_zero; // 当前状态逆深度减去线性化处 75 | } 76 | 77 | //@ 计算线性化更新后的残差 78 | // XTL：固定进一步优化时，DeltaX造成的残差项的变化，保留在res_toZeroF当中 res_toZeroF = resF+delta_r 79 | void EFResidual::fixLinearizationF(EnergyFunctional* ef) 80 | { 81 | Mat18f dp = ef->adHTdeltaF[hostIDX+ef->nFrames*targetIDX]; // 得到hostIDX --> targetIDX的状态增量 82 | 83 | // compute Jp*delta 84 | __m128 Jp_delta_x = _mm_set1_ps(J->Jpdxi[0].dot(dp.head<6>()) 85 | +J->Jpdc[0].dot(ef->cDeltaF) 86 | +J->Jpdd[0]*point->deltaF); 87 | __m128 Jp_delta_y = _mm_set1_ps(J->Jpdxi[1].dot(dp.head<6>()) 88 | +J->Jpdc[1].dot(ef->cDeltaF) 89 | +J->Jpdd[1]*point->deltaF); 90 | 91 | __m128 delta_a = _mm_set1_ps((float)(dp[6])); 92 | __m128 delta_b = _mm_set1_ps((float)(dp[7])); 93 | 94 | for(int i=0;iresF)+i); // 光度残差 98 | rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JIdx))+i),Jp_delta_x)); 99 | rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JIdx+1))+i),Jp_delta_y)); 100 | rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JabF))+i),delta_a)); 101 | rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JabF+1))+i),delta_b)); 102 | _mm_store_ps(((float*)&res_toZeroF)+i, rtz); // 存储在res_toZeroF 103 | } 104 | 105 | isLinearized = true; 106 | } 107 | 108 | } 109 | -------------------------------------------------------------------------------- /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 | void fixLinearizationF(EnergyFunctional* ef); 74 | 75 | 76 | // structural pointers 77 | PointFrameResidual* data; 78 | int hostIDX, targetIDX; //!< 残差对应的 host 和 Target ID号 79 | EFPoint* point; //!< 残差点 80 | EFFrame* host; //!< 主 81 | EFFrame* target; //!< 目标 82 | int idxInAll; //!< 所有残差中的id 83 | 84 | RawResidualJacobian* J; //!< 用来计算jacob, res值 85 | 86 | VecNRf res_toZeroF; //!< 更新delta后的线性残差 87 | Vec8f JpJdF; //!< 逆深度Jaco和位姿+光度Jaco的Hessian 2021.12.19 88 | 89 | 90 | // status. 91 | bool isLinearized; //!< 计算完成res_toZeroF 92 | 93 | // if residual is not OOB & not OUTLIER & should be used during accumulations 94 | bool isActiveAndIsGoodNEW; //!< 激活的还可以参与优化 95 | inline const bool &isActive() const {return isActiveAndIsGoodNEW;} //!< 是不是激活的取决于残差状态 96 | }; 97 | 98 | 99 | enum EFPointStatus {PS_GOOD=0, PS_MARGINALIZE, PS_DROP}; 100 | 101 | class EFPoint 102 | { 103 | public: 104 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 105 | EFPoint(PointHessian* d, EFFrame* host_) : data(d),host(host_) 106 | { 107 | takeData(); 108 | stateFlag=EFPointStatus::PS_GOOD; 109 | } 110 | void takeData(); 111 | 112 | PointHessian* data; //!< PointHessian数据 113 | 114 | 115 | 116 | float priorF; //!< 逆深度先验信息矩阵, 初始化之后的有 117 | float deltaF; //!< 当前逆深度和线性化处的差, 没有使用FEJ, 就是0 118 | 119 | 120 | // constant info (never changes in-between). 121 | int idxInPoints; //!< 当前点在EFFrame中id 122 | EFFrame* host; 123 | 124 | // contains all residuals. 125 | std::vector residualsAll; //!< 该点的所有残差 126 | 127 | float bdSumF; //!< 当前残差 + 边缘化先验残差 128 | float HdiF; //!< 逆深度hessian的逆, 协方差 129 | float Hdd_accLF; //!< 边缘化, 逆深度的hessian 130 | VecCf Hcd_accLF; //!< 边缘化, 逆深度和内参的hessian 131 | float bd_accLF; //!< 边缘化, J逆深度*残差 132 | float Hdd_accAF; //!< 正常逆深度的hessian 133 | VecCf Hcd_accAF; //!< 正常逆深度和内参的hessian 134 | float bd_accAF; //!< 正常 J逆深度*残差 135 | 136 | 137 | EFPointStatus stateFlag; //!< 点的状态 138 | }; 139 | 140 | 141 | 142 | class EFFrame 143 | { 144 | public: 145 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 146 | EFFrame(FrameHessian* d) : data(d) 147 | { 148 | takeData(); 149 | } 150 | void takeData(); 151 | 152 | //! 位姿 0-5, 光度ab 6-7 153 | Vec8 prior; //!< 位姿只有第一帧有先验 prior hessian (diagonal) 154 | Vec8 delta_prior; //!< 相对于先验的增量 // = state-state_prior (E_prior = (delta_prior)' * diag(prior) * (delta_prior) 155 | Vec8 delta; //!< 相对于线性化点位姿, 光度的增量 // state - state_zero. 156 | 157 | 158 | std::vector points; //!< 帧上所有点 159 | FrameHessian* data; //!< 对应FrameHessian数据 160 | 161 | int idx; //!< 在能量函数中帧id // idx in frames. 162 | 163 | int frameID; //!< 所有历史帧ID 164 | }; 165 | 166 | } 167 | 168 | -------------------------------------------------------------------------------- /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 | VecNRf resF; 37 | 38 | // XTL：Jpdxi为投影点坐标对host到target位姿的导数 the two rows of d[x,y]/d[xi]. 2x6 39 | Vec6f Jpdxi[2]; 40 | 41 | // the two rows of d[x,y]/d[C]. 42 | VecCf Jpdc[2]; 43 | 44 | // the two rows of d[x,y]/d[E]. 45 | Vec6f Jpde[2]; 46 | 47 | // the two rows of d[x,y]/d[idepth]. 48 | Vec2f Jpdd; 49 | 50 | // XTL：Jpdxi为误差对投影点的导数 the two columns of d[r]/d[x,y].9x2 51 | VecNRf JIdx[2]; 52 | 53 | // XTL：Jpdxi为误差对光度参数的导数 = the two columns of d[r] / d[ab] 54 | VecNRf JabF[2]; 55 | 56 | 57 | // = JIdx^T * JIdx (inner product). Only as a shorthand. 58 | Mat22f JIdx2; // 2x2 59 | // = Jab^T * JIdx (inner product). Only as a shorthand. 60 | Mat22f JabJIdx; // 2x2 61 | // = Jab^T * Jab (inner product). Only as a shorthand. 62 | Mat22f Jab2; // 2x2 63 | 64 | }; 65 | } 66 | 67 | -------------------------------------------------------------------------------- /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 | //? 两个ID有啥不同, 这两个i id 是从0开始的帧的id, 理解为id_true, incoming_id是主函数中处理过的dataset中图像的id,(可能会受倍率的影响而不同) 39 | int id; // INTERNAL ID, starting at zero. 40 | int incoming_id; // ID passed into DSO 41 | double timestamp; // timestamp passed into DSO. 42 | 43 | // set once after tracking 44 | SE3 camToTrackingRef; 45 | FrameShell* trackingRef; 46 | 47 | // constantly adapted. 48 | SE3 camToWorld; // Write: TRACKING, while frame is still fresh; MAPPING: only when locked [shellPoseMutex]. 49 | // 2021.12.17 50 | // AffLight aff_g2l; 51 | AffLight aff_g2l[CAM_NUM]; 52 | // 2021.12.17 53 | bool poseValid; 54 | 55 | // 2021.09.17 yzk 56 | SE3 gtPose; 57 | // 2021.09.17 yzk 58 | 59 | // statisitcs 60 | int statistics_outlierResOnThis; 61 | int statistics_goodResOnThis; 62 | int marginalizedAt; //!< 被边缘化时最新关键帧的id 63 | double movedByOpt; //!< 边缘化时得到的距离线性化点位姿的6维模 64 | 65 | 66 | inline FrameShell() 67 | { 68 | id=0; 69 | poseValid=true; 70 | camToWorld = SE3(); 71 | timestamp=0; 72 | marginalizedAt=-1; 73 | movedByOpt=0; 74 | statistics_outlierResOnThis=statistics_goodResOnThis=0; 75 | trackingRef=0; 76 | camToTrackingRef = SE3(); 77 | } 78 | }; 79 | 80 | 81 | } 82 | 83 | -------------------------------------------------------------------------------- /src/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 | //* 辐照度值B和曝光时间t 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 | //! 论文中曝光时间t * 辐照度B 43 | inline ImageAndExposure(int w_, int h_, double timestamp_=0) : w(w_), h(h_), timestamp(timestamp_) 44 | { 45 | image = new float[w*h]; //这表示图像,666, 标定后的辐照度 46 | exposure_time=1; 47 | } 48 | inline ~ImageAndExposure() 49 | { 50 | delete[] image; 51 | } 52 | // 曝光时间赋值给other 53 | inline void copyMetaTo(ImageAndExposure &other) 54 | { 55 | other.exposure_time = exposure_time; 56 | } 57 | // 深度拷贝 58 | inline ImageAndExposure* getDeepCopy() 59 | { 60 | ImageAndExposure* img = new ImageAndExposure(w,h,timestamp); 61 | img->exposure_time = exposure_time; 62 | memcpy(img->image, image, w*h*sizeof(float)); 63 | return img; 64 | } 65 | }; 66 | 67 | 68 | } 69 | -------------------------------------------------------------------------------- /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 | // 初始化, 未赋值不能使用 50 | callPerIndex = boost::bind(&IndexThreadReduce::callPerIndexDefault, this, _1, _2, _3, _4); 51 | 52 | running = true; 53 | for(int i=0;i callPerIndex, int first, int end, int stepSize = 0) 78 | { 79 | 80 | memset(&stats, 0, sizeof(Running)); 81 | 82 | // if(!multiThreading) 83 | // { 84 | // callPerIndex(first, end, &stats, 0); 85 | // return; 86 | // } 87 | 88 | 89 | 90 | if(stepSize == 0) 91 | stepSize = ((end-first)+NUM_THREADS-1)/NUM_THREADS; 92 | 93 | 94 | //printf("reduce called\n"); 95 | 96 | boost::unique_lock lock(exMutex); 97 | 98 | // save 99 | this->callPerIndex = callPerIndex; 100 | nextIndex = first; 101 | maxIndex = end; 102 | this->stepSize = stepSize; 103 | 104 | // go worker threads! 105 | for(int i=0;icallPerIndex = boost::bind(&IndexThreadReduce::callPerIndexDefault, this, _1, _2, _3, _4); 136 | 137 | //printf("reduce done (all threads finished)\n"); 138 | } 139 | 140 | Running stats; 141 | 142 | private: 143 | boost::thread workerThreads[NUM_THREADS]; 144 | bool isDone[NUM_THREADS]; 145 | bool gotOne[NUM_THREADS]; 146 | 147 | boost::mutex exMutex; 148 | boost::condition_variable todo_signal; 149 | boost::condition_variable done_signal; 150 | 151 | int nextIndex; 152 | int maxIndex; 153 | int stepSize; 154 | 155 | bool running; 156 | 157 | boost::function callPerIndex; 158 | 159 | void callPerIndexDefault(int i, int j,Running* k, int tid) 160 | { 161 | printf("ERROR: should never be called....\n"); 162 | assert(false); 163 | } 164 | 165 | void workerLoop(int idx) 166 | { 167 | boost::unique_lock lock(exMutex); 168 | 169 | while(running) 170 | { 171 | // try to get something to do. 172 | int todo = 0; 173 | bool gotSomething = false; 174 | if(nextIndex < maxIndex) 175 | { 176 | // got something! 177 | todo = nextIndex; 178 | nextIndex+=stepSize; 179 | gotSomething = true; 180 | } 181 | 182 | // if got something: do it (unlock in the meantime) 183 | if(gotSomething) 184 | { 185 | lock.unlock(); 186 | 187 | assert(callPerIndex != 0); 188 | 189 | Running s; memset(&s, 0, sizeof(Running)); 190 | callPerIndex(todo, std::min(todo+stepSize, maxIndex), &s, idx); 191 | gotOne[idx] = true; 192 | lock.lock(); 193 | stats += s; 194 | } 195 | 196 | // otherwise wait on signal, releasing lock in the meantime. 197 | else 198 | { 199 | if(!gotOne[idx]) 200 | { 201 | lock.unlock(); 202 | assert(callPerIndex != 0); 203 | Running s; memset(&s, 0, sizeof(Running)); 204 | callPerIndex(0, 0, &s, idx); 205 | gotOne[idx] = true; 206 | lock.lock(); 207 | stats += s; 208 | } 209 | isDone[idx] = true; 210 | //printf("worker %d waiting..\n", idx); 211 | done_signal.notify_all(); 212 | todo_signal.wait(lock); 213 | } 214 | } 215 | } 216 | }; 217 | } 218 | -------------------------------------------------------------------------------- /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 | // 2021.12.15 32 | #include "settings.h" 33 | 34 | 35 | namespace dso 36 | { 37 | 38 | // CAMERA MODEL TO USE 39 | 40 | 41 | #define SSEE(val,idx) (*(((float*)&val)+idx)) 42 | 43 | 44 | #define MAX_RES_PER_POINT 8 45 | #define NUM_THREADS 6 46 | 47 | 48 | #define todouble(x) (x).cast() 49 | 50 | 51 | typedef Sophus::SE3d SE3; 52 | typedef Sophus::Sim3d Sim3; 53 | typedef Sophus::SO3d SO3; 54 | 55 | 56 | 57 | #define CPARS 4 // 相机内参维数 58 | 59 | typedef Eigen::Matrix MatXX; 60 | typedef Eigen::Matrix MatCC; 61 | #define MatToDynamic(x) MatXX(x) 62 | 63 | 64 | 65 | typedef Eigen::Matrix MatC10; 66 | typedef Eigen::Matrix Mat1010; 67 | typedef Eigen::Matrix Mat1313; 68 | 69 | typedef Eigen::Matrix Mat810; 70 | typedef Eigen::Matrix Mat83; 71 | typedef Eigen::Matrix Mat66; 72 | typedef Eigen::Matrix Mat53; 73 | typedef Eigen::Matrix Mat43; 74 | typedef Eigen::Matrix Mat42; 75 | typedef Eigen::Matrix Mat33; 76 | typedef Eigen::Matrix Mat22; 77 | typedef Eigen::Matrix Mat26; 78 | 79 | typedef Eigen::Matrix Mat8C; 80 | 81 | typedef Eigen::Matrix MatC8; 82 | typedef Eigen::Matrix Mat8Cf; 83 | typedef Eigen::Matrix MatC8f; 84 | 85 | typedef Eigen::Matrix Mat88; 86 | 87 | typedef Eigen::Matrix Mat77; 88 | 89 | typedef Eigen::Matrix VecC; 90 | typedef Eigen::Matrix VecCf; 91 | typedef Eigen::Matrix Vec13; 92 | typedef Eigen::Matrix Vec10; 93 | 94 | typedef Eigen::Matrix Vec9; 95 | typedef Eigen::Matrix Vec8; 96 | typedef Eigen::Matrix Vec7; 97 | 98 | typedef Eigen::Matrix Vec6; 99 | typedef Eigen::Matrix Vec5; 100 | typedef Eigen::Matrix Vec4; 101 | typedef Eigen::Matrix Vec3; 102 | typedef Eigen::Matrix Vec2; 103 | typedef Eigen::Matrix VecX; 104 | 105 | typedef Eigen::Matrix Mat33f; 106 | typedef Eigen::Matrix Mat103f; 107 | typedef Eigen::Matrix Mat22f; 108 | typedef Eigen::Matrix Mat23f; 109 | typedef Eigen::Matrix Mat36f; 110 | typedef Eigen::Matrix Vec3f; 111 | typedef Eigen::Matrix Vec2f; 112 | typedef Eigen::Matrix Vec6f; 113 | 114 | 115 | 116 | typedef Eigen::Matrix Mat49; 117 | typedef Eigen::Matrix Mat89; 118 | 119 | typedef Eigen::Matrix Mat94; 120 | typedef Eigen::Matrix Mat98; 121 | 122 | typedef Eigen::Matrix Mat81; 123 | typedef Eigen::Matrix Mat18; 124 | typedef Eigen::Matrix Mat91; 125 | typedef Eigen::Matrix Mat19; 126 | 127 | 128 | typedef Eigen::Matrix Mat84; 129 | typedef Eigen::Matrix Mat48; 130 | typedef Eigen::Matrix Mat44; 131 | 132 | 133 | typedef Eigen::Matrix VecNRf; 134 | typedef Eigen::Matrix Vec12f; 135 | typedef Eigen::Matrix Mat18f; 136 | 137 | typedef Eigen::Matrix Mat66f; 138 | typedef Eigen::Matrix Mat88f; 139 | 140 | typedef Eigen::Matrix Vec8f; 141 | typedef Eigen::Matrix Mat84f; 142 | typedef Eigen::Matrix Vec8f; 143 | typedef Eigen::Matrix Vec10f; 144 | typedef Eigen::Matrix Mat66f; 145 | typedef Eigen::Matrix Vec4f; 146 | typedef Eigen::Matrix Mat44f; 147 | typedef Eigen::Matrix Mat1212f; 148 | typedef Eigen::Matrix Vec12f; 149 | typedef Eigen::Matrix Mat1313f; 150 | 151 | typedef Eigen::Matrix Mat1010f; 152 | typedef Eigen::Matrix Vec13f; 153 | typedef Eigen::Matrix Mat99f; 154 | typedef Eigen::Matrix Mat77f; 155 | typedef Eigen::Matrix Vec9f; 156 | typedef Eigen::Matrix Vec7f; 157 | 158 | typedef Eigen::Matrix Mat42f; 159 | typedef Eigen::Matrix Mat62f; 160 | typedef Eigen::Matrix Mat26f; 161 | typedef Eigen::Matrix Mat12f; 162 | typedef Eigen::Matrix Mat16f; 163 | 164 | typedef Eigen::Matrix VecXf; 165 | typedef Eigen::Matrix MatXXf; 166 | 167 | 168 | typedef Eigen::Matrix MatPCPC; 169 | typedef Eigen::Matrix MatPCPCf; 170 | typedef Eigen::Matrix VecPC; 171 | typedef Eigen::Matrix VecPCf; 172 | 173 | typedef Eigen::Matrix Mat1414f; 174 | typedef Eigen::Matrix Vec14f; 175 | typedef Eigen::Matrix Mat1414; 176 | typedef Eigen::Matrix Vec14; 177 | 178 | 179 | 180 | 181 | 182 | /******************************** 183 | * @ function: 光度仿射变换, 来建模曝光时间 184 | * 185 | * @ param: 186 | * 187 | * @ note: 曝光时间未知: lambda_a = lambda_b = 0 t = 1 188 | * @ 曝光时间已知: a = b = 0 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 | //! 光度矫正后(tB) I_frame = exp(a)*I_global + b. 198 | //! 辐照度(B) I_global = exp(-a)*(I_frame - b). 199 | double a,b; 200 | 201 | static Vec2 fromToVecExposure(float exposureF, float exposureT, AffLight g2F, AffLight g2T) 202 | { 203 | // 没有曝光时间标定, 置1 204 | if(exposureF==0 || exposureT==0) 205 | { 206 | exposureT = exposureF = 1; 207 | //printf("got exposure value of 0! please choose the correct model.\n"); 208 | //assert(setting_brightnessTransferFunc < 2); 209 | } 210 | // XTL r = IT - exp(g2T.a-g2F.a)*(IF-g2F.b) - g2T.b 211 | // = IT - a*Ia + a*g2F.b - g2T.b 212 | // = IT - a*Ia - b 213 | double a = exp(g2T.a-g2F.a) * exposureT / exposureF; 214 | double b = g2T.b - a*g2F.b; 215 | return Vec2(a,b); 216 | } 217 | 218 | Vec2 vec() 219 | { 220 | return Vec2(a,b); 221 | } 222 | }; 223 | 224 | } 225 | 226 | -------------------------------------------------------------------------------- /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 | // 2021.10.28 72 | bool* mask; 73 | // 2021.10.28 74 | virtual ~Undistort(); 75 | 76 | virtual void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const = 0; 77 | 78 | 79 | inline const Mat33 getK() const {return K;}; 80 | inline const Eigen::Vector2i getSize() const {return Eigen::Vector2i(w,h);}; 81 | inline const VecX getOriginalParameter() const {return parsOrg;}; 82 | inline const Eigen::Vector2i getOriginalSize() {return Eigen::Vector2i(wOrg,hOrg);}; 83 | inline bool isValid() {return valid;}; 84 | 85 | template 86 | ImageAndExposure* undistort(const MinimalImage* image_raw, float exposure=0, double timestamp=0, float factor=1) const; 87 | // 2019.11.07 yzk 88 | template 89 | ImageAndExposure* transformDepthImage(const MinimalImage* image_raw, float exposure=0, double timestamp=0, float factor=1) const; 90 | // 2019.11.07 yzk 91 | static Undistort* getUndistorterForFile(std::string configFilename, std::string gammaFilename, std::string vignetteFilename, std::string undistortFile); 92 | 93 | void loadPhotometricCalibration(std::string file, std::string noiseImage, std::string vignetteImage); 94 | 95 | PhotometricUndistorter* photometricUndist; // 光度矫正类 96 | 97 | protected: 98 | int w, h, wOrg, hOrg, wUp, hUp; //!< 输入图像大小, 相机原像素大小, 99 | int upsampleUndistFactor; 100 | Mat33 K; //!< 矫正后的相机参数(也可能是更改了的标定输出) 101 | VecX parsOrg; //!< 原来相机参数 102 | bool valid; //!< 参数有效 103 | bool passthrough; //!< 通过??? 不知道这个是干嘛的 104 | 105 | float* remapX; //!< 矫正所用的remap, 无畸变与畸变的映射 106 | float* remapY; 107 | 108 | void applyBlurNoise(float* img) const; 109 | 110 | void makeOptimalK_crop(); 111 | void makeOptimalK_full(); 112 | 113 | void readFromFile(const char* configFileName, int nPars, std::string prefix = "", std::string undistortFile = ""); 114 | }; 115 | 116 | class UndistortFOV : public Undistort 117 | { 118 | public: 119 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 120 | 121 | UndistortFOV(const char* configFileName, bool noprefix); 122 | ~UndistortFOV(); 123 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; 124 | 125 | }; 126 | 127 | class UndistortRadTan : public Undistort 128 | { 129 | public: 130 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 131 | UndistortRadTan(const char* configFileName, bool noprefix); 132 | ~UndistortRadTan(); 133 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; 134 | 135 | }; 136 | 137 | class UndistortEquidistant : public Undistort 138 | { 139 | public: 140 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 141 | UndistortEquidistant(const char* configFileName, bool noprefix); 142 | ~UndistortEquidistant(); 143 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; 144 | 145 | }; 146 | 147 | class UndistortPinhole : public Undistort 148 | { 149 | public: 150 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 151 | UndistortPinhole(const char* configFileName, bool noprefix); 152 | ~UndistortPinhole(); 153 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; 154 | 155 | private: 156 | float inputCalibration[8]; 157 | }; 158 | 159 | class UndistortKB : public Undistort 160 | { 161 | public: 162 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 163 | UndistortKB(const char* configFileName, bool noprefix); 164 | ~UndistortKB(); 165 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; 166 | 167 | }; 168 | // 2021.10.28 169 | class UndistortOmni : public Undistort 170 | { 171 | public: 172 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 173 | UndistortOmni(const char* configFileName, bool noprefix, std::string undistortFile = ""); 174 | ~UndistortOmni(); 175 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; 176 | }; 177 | // 2021.10.28 178 | } 179 | 180 | -------------------------------------------------------------------------------- /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 | //! 后面带G的是global变量 31 | namespace dso 32 | { 33 | int wG[PYR_LEVELS], hG[PYR_LEVELS]; 34 | float powG[PYR_LEVELS] = {1,2,4,8,16,32}; 35 | 36 | bool firstG; 37 | float weightC[CAM_NUM]; 38 | float lastRes; 39 | 40 | int _left=-1; 41 | int _right=10000; 42 | int _top=-1; 43 | int _bottom=10000; 44 | 45 | float fxG[CAM_NUM][PYR_LEVELS], fyG[CAM_NUM][PYR_LEVELS], 46 | cxG[CAM_NUM][PYR_LEVELS], cyG[CAM_NUM][PYR_LEVELS]; 47 | 48 | float fxiG[CAM_NUM][PYR_LEVELS], fyiG[CAM_NUM][PYR_LEVELS], 49 | cxiG[CAM_NUM][PYR_LEVELS], cyiG[CAM_NUM][PYR_LEVELS]; 50 | 51 | Eigen::Matrix3f KG[CAM_NUM][PYR_LEVELS], KiG[CAM_NUM][PYR_LEVELS]; 52 | 53 | 54 | std::vector T_lb_c(CAM_NUM); 55 | 56 | std::vector T_c0_c(CAM_NUM); 57 | 58 | std::vector T_c_c0(CAM_NUM); 59 | 60 | bool* maskG[CAM_NUM][PYR_LEVELS]; 61 | 62 | float wM3G; // w-3 global 63 | float hM3G; 64 | 65 | int ids[CAM_NUM+2]; 66 | 67 | Eigen::Matrix LUT[256]; 68 | 69 | 70 | void draw(cv::Mat& m, int Ku, int Kv, cv::Vec3b color, int margin) 71 | { 72 | for(int i = Ku-margin; i <= Ku+margin; i++) 73 | { 74 | for(int j = Kv-margin; j <= Kv+margin; j++) 75 | { 76 | // m.at(j, i) = color[0]; 77 | m.at(j, i) = color; 78 | } 79 | } 80 | /* 81 | for(int i = Ku-2; i <= Ku+2; i++) 82 | { 83 | m.at(Kv-2, i) = cv::Vec3b(0,0,0); 84 | m.at(Kv+2, i) = cv::Vec3b(0,0,0); 85 | } 86 | 87 | for(int j = Kv-1; j <= Kv+1; j++) 88 | { 89 | m.at(j, Ku-2) = cv::Vec3b(0,0,0); 90 | m.at(j, Ku+2) = cv::Vec3b(0,0,0); 91 | } 92 | */ 93 | } 94 | 95 | void setLUT(){ 96 | int s; 97 | for (s = 0; s < 32; s++) { 98 | LUT[s][0] = 128 + 4 * s; 99 | LUT[s][1] = 0; 100 | LUT[s][2] = 0; 101 | } 102 | LUT[32][0] = 255; 103 | LUT[32][1] = 0; 104 | LUT[32][2] = 0; 105 | for (s = 0; s < 63; s++) { 106 | LUT[33+s][0] = 255; 107 | LUT[33+s][1] = 4+4*s; 108 | LUT[33+s][2] = 0; 109 | } 110 | LUT[96][0] = 254; 111 | LUT[96][1] = 255; 112 | LUT[96][2] = 2; 113 | for (s = 0; s < 62; s++) { 114 | LUT[97 + s][0] = 250 - 4 * s; 115 | LUT[97 + s][1] = 255; 116 | LUT[97 + s][2] = 6+4*s; 117 | } 118 | LUT[159][0] = 1; 119 | LUT[159][1] = 255; 120 | LUT[159][2] = 254; 121 | for (s = 0; s < 64; s++) { 122 | LUT[160 + s][0] = 0; 123 | LUT[160 + s][1] = 252 - (s * 4); 124 | LUT[160 + s][2] = 255; 125 | } 126 | for (s = 0; s < 32; s++) { 127 | LUT[224 + s][0] = 0; 128 | LUT[224 + s][1] = 0; 129 | LUT[224 + s][2] = 252-4*s; 130 | } 131 | } 132 | 133 | void setGlobalCalib(int w, int h,const Eigen::Matrix3f &K, int cam_idx, bool* mask) 134 | { 135 | // XTL nclt的cam顺序是0,1,2,3,4 136 | // XTL 1->0,2->1,3->2 137 | for(int i=0;i2,4->0 140 | ids[0] = cam_num-1; 141 | ids[cam_num+1] = 0; 142 | 143 | maskG[cam_idx][0] = new bool[w*h]; 144 | for(int i=0;i=0) 149 | // mask[x+y*w-i] = false; 150 | // if(x+y*w+i=0) 153 | // mask[x+(y-i)*w] = false; 154 | // if(x+(y+i)*w 5000 && pyrLevelsUsed < PYR_LEVELS) 166 | { 167 | wlvl /=2; 168 | hlvl /=2; 169 | pyrLevelsUsed++; 170 | } 171 | printf("using pyramid levels 0 to %d. coarsest resolution: %d x %d!\n", 172 | pyrLevelsUsed-1, wlvl, hlvl); 173 | if(wlvl>100 && hlvl > 100) 174 | { 175 | printf("\n\n===============WARNING!===================\n " 176 | "using not enough pyramid levels.\n" 177 | "Consider scaling to a resolution that is a multiple of a power of 2.\n"); 178 | } 179 | if(pyrLevelsUsed < 3) 180 | { 181 | printf("\n\n===============WARNING!===================\n " 182 | "I need higher resolution.\n" 183 | "I will probably segfault.\n"); 184 | } 185 | 186 | wM3G = w-3; 187 | hM3G = h-3; 188 | 189 | wG[0] = w; 190 | hG[0] = h; 191 | 192 | KG[cam_idx][0] = K; 193 | fxG[cam_idx][0] = K(0,0); 194 | fyG[cam_idx][0] = K(1,1); 195 | cxG[cam_idx][0] = K(0,2); 196 | cyG[cam_idx][0] = K(1,2); 197 | KiG[cam_idx][0] = KG[cam_idx][0].inverse(); 198 | fxiG[cam_idx][0] = KiG[cam_idx][0](0,0); 199 | fyiG[cam_idx][0] = KiG[cam_idx][0](1,1); 200 | cxiG[cam_idx][0] = KiG[cam_idx][0](0,2); 201 | cyiG[cam_idx][0] = KiG[cam_idx][0](1,2); 202 | 203 | for (int level = 1; level < pyrLevelsUsed; ++ level) 204 | { 205 | wG[level] = w >> level; 206 | hG[level] = h >> level; 207 | 208 | fxG[cam_idx][level] = fxG[cam_idx][level-1] * 0.5; 209 | fyG[cam_idx][level] = fyG[cam_idx][level-1] * 0.5; 210 | cxG[cam_idx][level] = (cxG[cam_idx][0] + 0.5) / ((int)1<(0,0) = R; 272 | T.topRightCorner<3, 1>() = t; 273 | 274 | return T; 275 | } 276 | 277 | void setGlobalExtrin(float ssc[6], int cam_idx){ 278 | Eigen::Matrix4d T = xyzrpy2T(ssc); 279 | T_lb_c[cam_idx] = SE3(T.block<3,3>(0,0),T.topRightCorner<3, 1>()); 280 | } 281 | 282 | void setGlobalExtrin_ijrr(Eigen::Matrix4d T_c_l, int cam_idx){ 283 | T_lb_c[cam_idx] = SE3(T_c_l.block<3,3>(0,0),T_c_l.topRightCorner<3, 1>()).inverse(); 284 | } 285 | 286 | void calGlobalExtrin(){ 287 | for(int i=0;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 | 26 | #pragma once 27 | #include "util/settings.h" 28 | #include "util/NumType.h" 29 | #include 30 | 31 | #include 32 | 33 | namespace dso 34 | { 35 | extern int wG[PYR_LEVELS], hG[PYR_LEVELS]; 36 | 37 | extern float powG[PYR_LEVELS]; 38 | extern float weightC[CAM_NUM]; 39 | extern bool firstG; 40 | extern float lastRes; 41 | extern float fxG[CAM_NUM][PYR_LEVELS], fyG[CAM_NUM][PYR_LEVELS], 42 | cxG[CAM_NUM][PYR_LEVELS], cyG[CAM_NUM][PYR_LEVELS]; 43 | 44 | extern float fxiG[CAM_NUM][PYR_LEVELS], fyiG[CAM_NUM][PYR_LEVELS], 45 | cxiG[CAM_NUM][PYR_LEVELS], cyiG[CAM_NUM][PYR_LEVELS]; 46 | 47 | extern Eigen::Matrix3f KG[CAM_NUM][PYR_LEVELS],KiG[CAM_NUM][PYR_LEVELS]; 48 | // 2021.11.13 49 | 50 | extern float wM3G; 51 | extern float hM3G; 52 | 53 | extern Eigen::Matrix LUT[256]; 54 | 55 | extern std::vector T_lb_c; 56 | extern std::vector T_c0_c; 57 | extern std::vector T_c_c0; 58 | extern bool* maskG[CAM_NUM][PYR_LEVELS]; 59 | 60 | extern int _left, _right, _top, _bottom; 61 | 62 | extern int ids[CAM_NUM+2]; 63 | 64 | void setGlobalCalib(int w, int h, const Eigen::Matrix3f &K, int cam_idx, bool* mask); 65 | void draw(cv::Mat& m, int Ku, int Kv, cv::Vec3b color, int margin); 66 | 67 | Eigen::Matrix4d xyzrpy2T(float ssc[6]); 68 | void setGlobalExtrin(float ssc[6], int cam_idx); 69 | void setGlobalExtrin_ijrr(Eigen::Matrix4d T, int cam_idx); 70 | 71 | void calGlobalExtrin(); 72 | void setLUT(); 73 | } 74 | -------------------------------------------------------------------------------- /src/util/settings.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 | 28 | #include 29 | #include 30 | #include 31 | 32 | namespace dso 33 | { 34 | #define SOLVER_SVD (int)1 35 | #define SOLVER_ORTHOGONALIZE_SYSTEM (int)2 36 | #define SOLVER_ORTHOGONALIZE_POINTMARG (int)4 37 | #define SOLVER_ORTHOGONALIZE_FULL (int)8 38 | #define SOLVER_SVD_CUT7 (int)16 39 | #define SOLVER_REMOVE_POSEPRIOR (int)32 40 | #define SOLVER_USE_GN (int)64 41 | #define SOLVER_FIX_LAMBDA (int)128 42 | #define SOLVER_ORTHOGONALIZE_X (int)256 43 | #define SOLVER_MOMENTUM (int)512 44 | #define SOLVER_STEPMOMENTUM (int)1024 45 | #define SOLVER_ORTHOGONALIZE_X_LATER (int)2048 46 | 47 | 48 | // ============== PARAMETERS TO BE DECIDED ON COMPILE TIME ================= 49 | #define PYR_LEVELS 6 50 | #define DTOR (M_PI / 180.0) 51 | #define RTOD (1 / DTOR) 52 | // #define CAM_NUM 1 53 | #define CAM_NUM 5 54 | #define USE_GT 0 55 | // #define CHI_SQUARE_TEST 56 | 57 | extern int pyrLevelsUsed; 58 | extern bool setting_useNCLT; 59 | extern bool setting_optDepth; 60 | extern bool setting_useKFselection; 61 | extern int estimateType; 62 | extern float setting_mulPweight; 63 | extern float setting_weightMotion; 64 | 65 | extern const std::string pointCloudTopic; 66 | extern const std::string pointCloudTopic1; 67 | extern const std::string pointCloudTopic2; 68 | extern const std::string pointCloudTopic3; 69 | extern const std::string pointCloudTopic4; 70 | extern const std::string pointCloudTopic5; 71 | extern const std::string ImgTopic1; 72 | extern const std::string ImgTopic2; 73 | extern const std::string ImgTopic3; 74 | extern const std::string ImgTopic4; 75 | extern const std::string ImgTopic5; 76 | extern int setting_margin; 77 | 78 | extern const int N_SCAN; 79 | extern const int Horizon_SCAN; 80 | extern const float ang_res_x; 81 | extern const float ang_res_y; 82 | extern const float ang_bottom; 83 | 84 | extern int cam_num; 85 | extern int m_binFraction; 86 | extern int m_numBins; 87 | extern int cam_idx_array[5]; 88 | extern int idx_use[5]; 89 | // extern int idx_use[1]; 90 | extern bool setting_useMultualP; 91 | extern bool setting_useMultualPBack; 92 | extern bool setting_resManyBlock; 93 | 94 | extern float setting_keyframesPerSecond; 95 | extern bool setting_realTimeMaxKF; 96 | extern float setting_maxShiftWeightT; 97 | extern float setting_maxShiftWeightR; 98 | extern float setting_maxShiftWeightRT; 99 | extern float setting_maxAffineWeight; 100 | extern float setting_kfGlobalWeight; 101 | 102 | 103 | 104 | extern float setting_idepthFixPrior; 105 | extern float setting_idepthFixPriorMargFac; 106 | extern float setting_initialRotPrior; 107 | extern float setting_initialTransPrior; 108 | extern float setting_initialAffBPrior; 109 | extern float setting_initialAffAPrior; 110 | extern float setting_initialCalibHessian; 111 | 112 | extern int setting_solverMode; 113 | extern double setting_solverModeDelta; 114 | 115 | 116 | extern float setting_minIdepthH_act; 117 | extern float setting_minIdepthH_marg; 118 | 119 | 120 | 121 | extern float setting_maxIdepth; 122 | extern float setting_maxPixSearch; 123 | extern float setting_desiredImmatureDensity; // done 124 | extern float setting_desiredPointDensity; // done 125 | extern float setting_minPointsRemaining; 126 | extern float setting_maxLogAffFacInWindow; 127 | extern int setting_minFrames; 128 | extern int setting_maxFrames; 129 | extern int setting_minFrameAge; 130 | extern int setting_maxOptIterations; 131 | extern int setting_minOptIterations; 132 | extern float setting_thOptIterations; 133 | extern float setting_outlierTH; 134 | extern float setting_outlierTHSumComponent; 135 | 136 | 137 | 138 | extern int setting_pattern; 139 | extern float setting_margWeightFac; 140 | extern int setting_GNItsOnPointActivation; 141 | 142 | 143 | extern float setting_minTraceQuality; 144 | extern int setting_minTraceTestRadius; 145 | extern float setting_reTrackThreshold; 146 | 147 | 148 | extern int setting_minGoodActiveResForMarg; 149 | extern int setting_minGoodResForMarg; 150 | extern int setting_minInlierVotesForMarg; 151 | 152 | 153 | 154 | 155 | extern int setting_photometricCalibration; 156 | extern bool setting_useExposure; 157 | extern float setting_affineOptModeA; 158 | extern float setting_affineOptModeB; 159 | extern int setting_gammaWeightsPixelSelect; 160 | 161 | 162 | 163 | extern bool setting_forceAceptStep; 164 | 165 | 166 | 167 | extern float setting_huberTH; 168 | 169 | 170 | extern bool setting_logStuff; 171 | extern float benchmarkSetting_fxfyfac; 172 | extern int benchmarkSetting_width; 173 | extern int benchmarkSetting_height; 174 | extern float benchmark_varNoise; 175 | extern float benchmark_varBlurNoise; 176 | extern int benchmark_noiseGridsize; 177 | extern float benchmark_initializerSlackFactor; 178 | 179 | extern float setting_frameEnergyTHConstWeight; 180 | extern float setting_frameEnergyTHN; 181 | 182 | extern float setting_frameEnergyTHFacMedian; 183 | extern float setting_overallEnergyTHWeight; 184 | extern float setting_coarseCutoffTH; 185 | 186 | extern float setting_minGradHistCut; 187 | extern float setting_minGradHistAdd; 188 | 189 | extern float setting_gradDownweightPerLevel; 190 | extern bool setting_selectDirectionDistribution; 191 | 192 | 193 | 194 | extern float setting_trace_stepsize; 195 | extern int setting_trace_GNIterations; 196 | extern float setting_trace_GNThreshold; 197 | extern float setting_trace_extraSlackOnTH; 198 | extern float setting_trace_slackInterval; 199 | extern float setting_trace_minImprovementFactor; 200 | 201 | 202 | extern bool setting_render_displayCoarseTrackingFull; 203 | extern bool setting_render_renderWindowFrames; 204 | extern bool setting_render_plotTrackingFull; 205 | extern bool setting_render_display3D; 206 | extern bool setting_render_displayResidual; 207 | extern bool setting_render_displayVideo; 208 | extern bool setting_render_displayDepth; 209 | 210 | extern bool setting_fullResetRequested; 211 | 212 | extern bool setting_debugout_runquiet; 213 | 214 | extern bool disableAllDisplay; 215 | extern bool disableReconfigure; 216 | 217 | 218 | extern bool setting_onlyLogKFPoses; 219 | 220 | 221 | 222 | 223 | extern bool debugSaveImages; 224 | 225 | 226 | extern int sparsityFactor; 227 | extern bool goStepByStep; 228 | extern bool plotStereoImages; 229 | extern bool multiThreading; 230 | 231 | extern float freeDebugParam1; 232 | extern float freeDebugParam2; 233 | extern float freeDebugParam3; 234 | extern float freeDebugParam4; 235 | extern float freeDebugParam5; 236 | 237 | void handleKey(char k); 238 | 239 | 240 | 241 | 242 | extern int staticPattern[10][40][2]; 243 | extern int staticPatternNum[10]; 244 | extern int staticPatternPadding[10]; 245 | 246 | 247 | 248 | 249 | //#define patternNum staticPatternNum[setting_pattern] 250 | //#define patternP staticPattern[setting_pattern] 251 | //#define patternPadding staticPatternPadding[setting_pattern] 252 | 253 | // 254 | #define patternNum 8 255 | #define patternP staticPattern[8] // 取第8种pattern 256 | #define patternPadding 2 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | } 271 | -------------------------------------------------------------------------------- /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/ZikangYuan/panoramic_lidar_dso/67da5dfc5d35e8c42a7a7b431137feb211cfa7ee/thirdparty/libzip-1.1.1.tar.gz --------------------------------------------------------------------------------