├── Jacobians.pdf ├── calibs ├── TUM │ ├── vignette0.png │ ├── vignette1.png │ ├── camera0.txt │ ├── camera1.txt │ ├── pcalib0.txt │ ├── pcalib1.txt │ └── calib.yaml └── EuRoC │ ├── camera0.txt │ ├── camera1.txt │ └── calib.yaml ├── package.xml ├── thirdparty └── Sophus │ ├── README │ ├── SophusConfig.cmake.in │ ├── cmake_modules │ └── FindEigen3.cmake │ ├── sophus │ ├── sophus.hpp │ ├── test_so2.cpp │ ├── test_so3.cpp │ ├── test_se2.cpp │ ├── test_rxso3.cpp │ ├── test_se3.cpp │ ├── test_sim3.cpp │ └── tests.hpp │ ├── CMakeLists.txt │ └── FindEigen3.cmake ├── launch ├── euroc.launch └── tum.launch ├── cmake ├── FindLibZip.cmake ├── FindEigen3.cmake └── FindSuiteParse.cmake ├── src ├── util │ ├── globalCalib.h │ ├── ImageAndExposure.h │ ├── FrameShell.h │ ├── globalCalib.cpp │ ├── MinimalImage.h │ ├── Undistort.h │ ├── IndexThreadReduce.h │ ├── settings.h │ └── NumType.h ├── IOWrapper │ ├── ImageRW.h │ ├── ImageRW_dummy.cpp │ ├── ImageDisplay_dummy.cpp │ ├── ImageDisplay.h │ ├── Pangolin │ │ ├── KeyFrameDisplay.h │ │ └── PangolinDSOViewer.h │ ├── OpenCV │ │ ├── ImageRW_OpenCV.cpp │ │ └── ImageDisplay_OpenCV.cpp │ ├── OutputWrapper │ │ └── SampleOutputWrapper.h │ └── Output3DWrapper.h ├── OptimizationBackend │ ├── RawResidualJacobian.h │ ├── EnergyFunctionalStructs.h │ ├── EnergyFunctionalStructs.cpp │ ├── EnergyFunctional.h │ ├── AccumulatedSCHessian.h │ ├── AccumulatedTopHessian.h │ └── AccumulatedSCHessian.cpp └── FullSystem │ ├── PixelSelector2.h │ ├── Residuals.h │ ├── ResidualProjections.h │ ├── ImmaturePoint.h │ ├── CoarseTracker.h │ ├── CoarseInitializer.h │ ├── FullSystemOptPoint.cpp │ ├── FullSystemMarginalize.cpp │ └── PixelSelector.h ├── CMakeLists.txt └── README.md /Jacobians.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IRVLab/spline_vio/HEAD/Jacobians.pdf -------------------------------------------------------------------------------- /calibs/TUM/vignette0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IRVLab/spline_vio/HEAD/calibs/TUM/vignette0.png -------------------------------------------------------------------------------- /calibs/TUM/vignette1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IRVLab/spline_vio/HEAD/calibs/TUM/vignette1.png -------------------------------------------------------------------------------- /calibs/EuRoC/camera0.txt: -------------------------------------------------------------------------------- 1 | RadTan 0.609912234 0.9527 0.488982713 0.518489583 -0.28340811 0.07395907 0.00019359 1.76187114e-05 2 | 752 480 3 | crop 4 | 752 480 5 | -------------------------------------------------------------------------------- /calibs/EuRoC/camera1.txt: -------------------------------------------------------------------------------- 1 | RadTan 0.608493351 0.950279167 0.505982713 0.5327875 -0.28368365 0.07451284 -0.00010473 -3.55590700e-05 2 | 752 480 3 | crop 4 | 752 480 5 | -------------------------------------------------------------------------------- /calibs/TUM/camera0.txt: -------------------------------------------------------------------------------- 1 | EquiDistant 0.373004838186 0.372994740336 0.498890050897 0.502729380663 0.00348238940225 0.000715034845216 -0.00205323614187 0.000202936735918 2 | 512 512 3 | crop 4 | 512 512 5 | -------------------------------------------------------------------------------- /calibs/TUM/camera1.txt: -------------------------------------------------------------------------------- 1 | EquiDistant 0.371957753309 0.371942262641 0.494334955407 0.498861778606 0.00340031707904 0.00176627815347 -0.00266312569782 0.000329951742393 2 | 512 512 3 | crop 4 | 512 512 5 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | spline_vio 4 | 0.0.0 5 | The spline_vio package 6 | 7 | Jiawei Mo 8 | 9 | 10 | GPLv3 11 | 12 | catkin 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /calibs/EuRoC/calib.yaml: -------------------------------------------------------------------------------- 1 | # transformation of the main camera in IMU frame 2 | T_imu: 3 | cols: 4 4 | rows: 4 5 | data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, 6 | 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, 7 | -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, 8 | 0.0, 0.0, 0.0, 1.0] 9 | 10 | rate_hz: 200 11 | accelerometer_noise_density: 2.0000e-3 # [ m / s^2 / sqrt(Hz) ] 12 | accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) 13 | gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] 14 | gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) 15 | 16 | # transformation of the main camera in the other camera frame 17 | T_stereo: 18 | cols: 4 19 | rows: 4 20 | data: [1.0000, 0.0023, 0.0004, -0.1101, 21 | -0.0023, 0.9999, 0.0141, 0.0004, 22 | -0.0003, -0.0141, 0.9999, -0.0009, 23 | 0.0, 0.0, 0.0, 1.0] 24 | -------------------------------------------------------------------------------- /launch/euroc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /launch/tum.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /calibs/TUM/pcalib0.txt: -------------------------------------------------------------------------------- 1 | 0.0 1.0 2.0 3.0 4.0 5.0 6.0 7.0 8.0 9.0 10.0 11.0 12.0 13.0 14.0 15.0 16.0 17.0 18.0 19.0 20.0 21.0 22.0 23.0 24.0 25.0 26.0 27.0 28.0 29.0 30.0 31.0 32.0 33.0 34.0 35.0 36.0 37.0 38.0 39.0 40.0 41.0 42.0 43.0 44.0 45.0 46.0 47.0 48.0 49.0 50.0 51.0 52.0 53.0 54.0 55.0 56.0 57.0 58.0 59.0 60.0 61.0 62.0 63.0 64.0 65.0 66.0 67.0 68.0 69.0 70.0 71.0 72.0 73.0 74.0 75.0 76.0 77.0 78.0 79.0 80.0 81.0 82.0 83.0 84.0 85.0 86.0 87.0 88.0 89.0 90.0 91.0 92.0 93.0 94.0 95.0 96.0 97.0 98.0 99.0 100.0 101.0 102.0 103.0 104.0 105.0 106.0 107.0 108.0 109.0 110.0 111.0 112.0 113.0 114.0 115.0 116.0 117.0 118.0 119.0 120.0 121.0 122.0 123.0 124.0 125.0 126.0 127.0 128.0 129.0 130.0 131.0 132.0 133.0 134.0 135.0 136.0 137.0 138.0 139.0 140.0 141.0 142.0 143.0 144.0 145.0 146.0 147.0 148.0 149.0 150.0 151.0 152.0 153.0 154.0 155.0 156.0 157.0 158.0 159.0 160.0 161.0 162.0 163.0 164.0 165.0 166.0 167.0 168.0 169.0 170.0 171.0 172.0 173.0 174.0 175.0 176.0 177.0 178.0 179.0 180.0 181.0 182.0 183.0 184.0 185.0 186.0 187.0 188.0 189.0 190.0 191.0 192.0 193.0 194.0 195.0 196.0 197.0 198.0 199.0 200.0 201.0 202.0 203.0 204.0 205.0 206.0 207.0 208.0 209.0 210.0 211.0 212.0 213.0 214.0 215.0 216.0 217.0 218.0 219.0 220.0 221.0 222.0 223.0 224.0 225.0 226.0 227.0 228.0 229.0 230.0 231.0 232.0 233.0 234.0 235.0 236.0 237.0 238.0 239.0 240.0 241.0 242.0 243.0 244.0 245.0 246.0 247.0 248.0 249.0 250.0 251.0 252.0 253.0 254.0 255.0 -------------------------------------------------------------------------------- /calibs/TUM/pcalib1.txt: -------------------------------------------------------------------------------- 1 | 0.0 1.0 2.0 3.0 4.0 5.0 6.0 7.0 8.0 9.0 10.0 11.0 12.0 13.0 14.0 15.0 16.0 17.0 18.0 19.0 20.0 21.0 22.0 23.0 24.0 25.0 26.0 27.0 28.0 29.0 30.0 31.0 32.0 33.0 34.0 35.0 36.0 37.0 38.0 39.0 40.0 41.0 42.0 43.0 44.0 45.0 46.0 47.0 48.0 49.0 50.0 51.0 52.0 53.0 54.0 55.0 56.0 57.0 58.0 59.0 60.0 61.0 62.0 63.0 64.0 65.0 66.0 67.0 68.0 69.0 70.0 71.0 72.0 73.0 74.0 75.0 76.0 77.0 78.0 79.0 80.0 81.0 82.0 83.0 84.0 85.0 86.0 87.0 88.0 89.0 90.0 91.0 92.0 93.0 94.0 95.0 96.0 97.0 98.0 99.0 100.0 101.0 102.0 103.0 104.0 105.0 106.0 107.0 108.0 109.0 110.0 111.0 112.0 113.0 114.0 115.0 116.0 117.0 118.0 119.0 120.0 121.0 122.0 123.0 124.0 125.0 126.0 127.0 128.0 129.0 130.0 131.0 132.0 133.0 134.0 135.0 136.0 137.0 138.0 139.0 140.0 141.0 142.0 143.0 144.0 145.0 146.0 147.0 148.0 149.0 150.0 151.0 152.0 153.0 154.0 155.0 156.0 157.0 158.0 159.0 160.0 161.0 162.0 163.0 164.0 165.0 166.0 167.0 168.0 169.0 170.0 171.0 172.0 173.0 174.0 175.0 176.0 177.0 178.0 179.0 180.0 181.0 182.0 183.0 184.0 185.0 186.0 187.0 188.0 189.0 190.0 191.0 192.0 193.0 194.0 195.0 196.0 197.0 198.0 199.0 200.0 201.0 202.0 203.0 204.0 205.0 206.0 207.0 208.0 209.0 210.0 211.0 212.0 213.0 214.0 215.0 216.0 217.0 218.0 219.0 220.0 221.0 222.0 223.0 224.0 225.0 226.0 227.0 228.0 229.0 230.0 231.0 232.0 233.0 234.0 235.0 236.0 237.0 238.0 239.0 240.0 241.0 242.0 243.0 244.0 245.0 246.0 247.0 248.0 249.0 250.0 251.0 252.0 253.0 254.0 255.0 -------------------------------------------------------------------------------- /calibs/TUM/calib.yaml: -------------------------------------------------------------------------------- 1 | # transformation of the main camera in IMU frame 2 | T_imu: 3 | cols: 4 4 | rows: 4 5 | data: [-0.99952504, 0.00750192, -0.02989013, 0.04557484, 6 | 0.02961534, -0.03439736, -0.99896935, -0.0711618, 7 | -0.00852233, -0.99938008, 0.03415885, -0.04468125, 8 | 0.0, 0.0, 0.0, 1.0] 9 | 10 | rate_hz: 200 11 | 12 | # Values from allan plots 13 | # sequence: dataset-calib-imu-static2.bag (full data range) 14 | #accelerometer_noise_density: 0.0014 # m/s^1.5 15 | #accelerometer_random_walk: 0.000086 # m/s^2.5 16 | #gyroscope_noise_density: 0.000080 # rad/s^0.5 17 | #gyroscope_random_walk: 0.0000022 # rad/s^1.5 18 | 19 | # Inflated values (to account for unmodelled effects) 20 | # Those values work well with Kalibr cam-imu calibration. 21 | # - white noise multiplied by 2 22 | # - bias random walk multiplied by 10 23 | accelerometer_noise_density: 0.0028 # m/s^1.5 24 | accelerometer_random_walk: 0.00086 # m/s^2.5 25 | gyroscope_noise_density: 0.00016 # rad/s^0.5 26 | gyroscope_random_walk: 0.000022 # rad/s^1.5 27 | 28 | # transformation of the main camera in the other camera frame 29 | T_stereo: 30 | cols: 4 31 | rows: 4 32 | data: [0.99999945, -0.00082336, -0.00065614, -0.1010611, 33 | 0.00079169, 0.99889946, -0.04689604, -0.00197646, 34 | 0.00069403, 0.04689549, 0.99889956, -0.00117564, 35 | 0.0, 0.0, 0.0, 1.0] 36 | -------------------------------------------------------------------------------- /src/util/globalCalib.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | #include "NumType.h" 26 | #include "settings.h" 27 | 28 | namespace dso { 29 | extern int wG[PYR_LEVELS], hG[PYR_LEVELS]; 30 | extern float fxG[PYR_LEVELS], fyG[PYR_LEVELS], cxG[PYR_LEVELS], cyG[PYR_LEVELS]; 31 | 32 | extern float fxiG[PYR_LEVELS], fyiG[PYR_LEVELS], cxiG[PYR_LEVELS], 33 | cyiG[PYR_LEVELS]; 34 | 35 | extern Eigen::Matrix3f KG[PYR_LEVELS], KiG[PYR_LEVELS]; 36 | 37 | extern float wM3G; 38 | extern float hM3G; 39 | 40 | void setGlobalCalib(int w, int h, const Eigen::Matrix3f &K); 41 | } // namespace dso 42 | -------------------------------------------------------------------------------- /src/IOWrapper/ImageRW.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | #include "util/MinimalImage.h" 26 | #include "util/NumType.h" 27 | 28 | namespace dso { 29 | namespace IOWrap { 30 | 31 | MinimalImageB *readImageBW_8U(std::string filename); 32 | MinimalImageB3 *readImageRGB_8U(std::string filename); 33 | MinimalImage *readImageBW_16U(std::string filename); 34 | 35 | MinimalImageB *readStreamBW_8U(char *data, int numBytes); 36 | 37 | void writeImage(std::string filename, MinimalImageB *img); 38 | void writeImage(std::string filename, MinimalImageB3 *img); 39 | void writeImage(std::string filename, MinimalImageF *img); 40 | void writeImage(std::string filename, MinimalImageF3 *img); 41 | 42 | } // namespace IOWrap 43 | } // namespace dso 44 | -------------------------------------------------------------------------------- /src/IOWrapper/ImageRW_dummy.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #include "IOWrapper/ImageRW.h" 25 | 26 | namespace dso { 27 | 28 | namespace IOWrap { 29 | 30 | MinimalImageB *readImageBW_8U(std::string filename) { 31 | printf("not implemented. bye!\n"); 32 | return 0; 33 | }; 34 | MinimalImageB3 *readImageRGB_8U(std::string filename) { 35 | printf("not implemented. bye!\n"); 36 | return 0; 37 | }; 38 | MinimalImage *readImageBW_16U(std::string filename) { 39 | printf("not implemented. bye!\n"); 40 | return 0; 41 | }; 42 | MinimalImageB *readStreamBW_8U(char *data, int numBytes) { 43 | printf("not implemented. bye!\n"); 44 | return 0; 45 | }; 46 | void writeImage(std::string filename, MinimalImageB *img){}; 47 | void writeImage(std::string filename, MinimalImageB3 *img){}; 48 | void writeImage(std::string filename, MinimalImageF *img){}; 49 | void writeImage(std::string filename, MinimalImageF3 *img){}; 50 | 51 | } // namespace IOWrap 52 | 53 | } // namespace dso 54 | -------------------------------------------------------------------------------- /src/OptimizationBackend/RawResidualJacobian.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | 26 | #include "util/NumType.h" 27 | 28 | namespace dso { 29 | struct RawResidualJacobian { 30 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 31 | // ================== new structure: save independently =============. 32 | VecNRf resF; 33 | 34 | // the two rows of d[x,y]/d[xi]. 35 | Vec6f Jpdxi[2]; // 2x6 36 | 37 | // the two rows of d[x,y]/d[C]. 38 | VecCf Jpdc[2]; // 2x4 39 | 40 | // the two rows of d[x,y]/d[idepth]. 41 | Vec2f Jpdd; // 2x1 42 | 43 | // the two columns of d[r]/d[x,y]. 44 | VecNRf JIdx[2]; // 9x2 45 | 46 | // = the two columns of d[r] / d[ab] 47 | VecNRf JabF[2]; // 9x2 48 | 49 | // = JIdx^T * JIdx (inner product). Only as a shorthand. 50 | Mat22f JIdx2; // 2x2 51 | // = Jab^T * JIdx (inner product). Only as a shorthand. 52 | Mat22f JabJIdx; // 2x2 53 | // = Jab^T * Jab (inner product). Only as a shorthand. 54 | Mat22f Jab2; // 2x2 55 | }; 56 | } // namespace dso 57 | -------------------------------------------------------------------------------- /src/FullSystem/PixelSelector2.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | 26 | #include "util/NumType.h" 27 | 28 | namespace dso { 29 | 30 | enum PixelSelectorStatus { PIXSEL_VOID = 0, PIXSEL_1, PIXSEL_2, PIXSEL_3 }; 31 | 32 | class FrameHessian; 33 | 34 | class PixelSelector { 35 | public: 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 37 | int makeMaps(const FrameHessian *const fh, float *map_out, float density, 38 | int recursionsLeft = 1, bool plot = false, float thFactor = 1); 39 | 40 | PixelSelector(int w, int h); 41 | ~PixelSelector(); 42 | int currentPotential; 43 | 44 | bool allowFast; 45 | void makeHists(const FrameHessian *const fh); 46 | 47 | private: 48 | Eigen::Vector3i select(const FrameHessian *const fh, float *map_out, int pot, 49 | float thFactor = 1); 50 | 51 | unsigned char *randomPattern; 52 | 53 | int *gradHist; 54 | float *ths; 55 | float *thsSmoothed; 56 | int thsStep; 57 | const FrameHessian *gradHistFrame; 58 | }; 59 | 60 | } // namespace dso 61 | -------------------------------------------------------------------------------- /src/util/ImageAndExposure.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | #include 26 | #include 27 | 28 | namespace dso { 29 | 30 | class ImageAndExposure { 31 | public: 32 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 33 | float *image; // irradiance. between 0 and 256 34 | int w, h; // width and height; 35 | double timestamp; 36 | float exposure_time; // exposure time in ms. 37 | inline ImageAndExposure(int w_, int h_, double timestamp_ = 0) 38 | : w(w_), h(h_), timestamp(timestamp_) { 39 | image = new float[w * h]; 40 | exposure_time = 1; 41 | } 42 | inline ~ImageAndExposure() { delete[] image; } 43 | 44 | inline void copyMetaTo(ImageAndExposure &other) { 45 | other.exposure_time = exposure_time; 46 | } 47 | 48 | inline ImageAndExposure *getDeepCopy() { 49 | ImageAndExposure *img = new ImageAndExposure(w, h, timestamp); 50 | img->exposure_time = exposure_time; 51 | memcpy(img->image, image, w * h * sizeof(float)); 52 | return img; 53 | } 54 | }; 55 | 56 | } // namespace dso 57 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | PROJECT(spline_vio) 2 | CMAKE_MINIMUM_REQUIRED(VERSION 2.8.3) 3 | 4 | set(BUILD_TYPE Release) 5 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 6 | 7 | find_package(SuiteParse REQUIRED) 8 | find_package(Eigen3 REQUIRED) 9 | find_package(Boost COMPONENTS system thread) 10 | find_package(Pangolin 0.2 REQUIRED) 11 | find_package(OpenCV REQUIRED) 12 | find_package(catkin REQUIRED COMPONENTS 13 | geometry_msgs 14 | roscpp 15 | rosbag 16 | sensor_msgs 17 | cv_bridge 18 | image_transport 19 | ) 20 | 21 | catkin_package() 22 | 23 | add_definitions("-DENABLE_SSE") 24 | set(CMAKE_CXX_FLAGS 25 | "${SSE_FLAGS} -O3 -g -std=c++0x -march=native" 26 | ) 27 | 28 | include_directories( 29 | src 30 | thirdparty/Sophus 31 | thirdparty/sse2neon 32 | ${EIGEN3_INCLUDE_DIR} 33 | ${catkin_INCLUDE_DIRS} 34 | ${Pangolin_INCLUDE_DIRS} 35 | ${OpenCV_INCLUDE_DIRS} 36 | ${CSPARSE_INCLUDE_DIR} 37 | ${CHOLMOD_INCLUDE_DIR} 38 | ) 39 | 40 | add_library(spline_vio_lib 41 | src/FullSystem/FullSystem.cpp 42 | src/FullSystem/FullSystemOptimize.cpp 43 | src/FullSystem/FullSystemOptPoint.cpp 44 | src/FullSystem/FullSystemDebugStuff.cpp 45 | src/FullSystem/FullSystemMarginalize.cpp 46 | src/FullSystem/Residuals.cpp 47 | src/FullSystem/CoarseInitializer.cpp 48 | src/FullSystem/CoarseTracker.cpp 49 | src/FullSystem/ImmaturePoint.cpp 50 | src/FullSystem/HessianBlocks.cpp 51 | src/FullSystem/PixelSelector2.cpp 52 | src/OptimizationBackend/EnergyFunctional.cpp 53 | src/OptimizationBackend/AccumulatedTopHessian.cpp 54 | src/OptimizationBackend/AccumulatedSCHessian.cpp 55 | src/OptimizationBackend/EnergyFunctionalStructs.cpp 56 | src/util/settings.cpp 57 | src/util/Undistort.cpp 58 | src/util/globalCalib.cpp 59 | src/IOWrapper/Pangolin/KeyFrameDisplay.cpp 60 | src/IOWrapper/Pangolin/PangolinDSOViewer.cpp 61 | src/IOWrapper/OpenCV/ImageDisplay_OpenCV.cpp 62 | src/IOWrapper/OpenCV/ImageRW_OpenCV.cpp 63 | ) 64 | 65 | add_executable(spline_vio_node src/main.cpp) 66 | 67 | target_link_libraries(spline_vio_node 68 | spline_vio_lib 69 | ${catkin_LIBRARIES} 70 | ${BOOST_THREAD_LIBRARY} 71 | ${Pangolin_LIBRARIES} 72 | ${OpenCV_LIBS} 73 | boost_system boost_thread cxsparse) 74 | 75 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Continuous-Time Spline Visual-Inertial Odometry 2 | 3 | ## Updates 4 | - [June 2022]: The position and acceleration formulation is suboptimal. Please checkout the [new branch](https://github.com/IRVLab/spline_vio/tree/position_spine_in_imu_frame) for improved formulation. 5 | 6 | ## Related Publications 7 | - **Direct Sparse Odometry**, J. Engel, V. Koltun, D. Cremers, In IEEE Transactions on Pattern Analysis and Machine Intelligence (PAMI), 2018 8 | - **Continuous-Time Spline Visual-Inertial Odometry**, J. Mo and J. Sattar, In IEEE International Conference on Robotics and Automation, 2022, [arXiv](https://arxiv.org/abs/2109.09035). 9 | 10 | ## Dependencies 11 | - [ROS](https://www.ros.org/) 12 | 13 | - [DSO dependencies](https://github.com/JakobEngel/dso#2-installation) 14 | 15 | ## Install 16 | ``` 17 | cd catkin_ws/src 18 | git clone https://github.com/IRVLab/spline_vio.git 19 | cd .. 20 | catkin_make 21 | ``` 22 | 23 | ## Usage 24 | - Use [Kalibr toolbox](https://github.com/ethz-asl/kalibr) to calibrate camera and IMU. 25 | 26 | - Convert camera parameters to [DSO format](https://github.com/JakobEngel/dso#31-dataset-format). 27 | 28 | - Create a launch file following the example of [tum.launch](https://github.com/IRVLab/spline_vio/blob/master/launch/tum.launch). 29 | 30 | ``` 31 | roslaunch spline_vio [YOUR_LAUNCH_FILE] 32 | ``` 33 | 34 | - Ctrl-C to terminate the program, the final trajectory (results.txt) will be written to ~/Desktop folder by default. 35 | 36 | ## Output file 37 | - results.txt: poses of all frames, using the TUM RGB-D / TUM monoVO format ([timestamp x y z qx qy qz qw] of the cameraToWorld transformation). 38 | 39 | ## Modifications to DSO for this project 40 | - ROS interface: main.cpp 41 | - Predict pose using spine for front-end tracking: FullSystem::trackNewCoarse() 42 | - IMU/Spline state: HessianBlocks.h 43 | - IMU/Spline Jacobians (check [Jacobians.pdf](https://github.com/IRVLab/spline_vio/blob/main/Jacobians.pdf)): HessianBlocks::getImuHi() 44 | - Constraints Jacobians (check [Jacobians.pdf](https://github.com/IRVLab/spline_vio/blob/main/Jacobians.pdf)): EnergyFunctional::getImuHessianCurrentFrame() 45 | - Solve the constraint nonlinear optimization problem: EnergyFunctional::solveSystemF() 46 | -------------------------------------------------------------------------------- /src/util/FrameShell.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | 26 | #include "NumType.h" 27 | #include "algorithm" 28 | 29 | namespace dso { 30 | 31 | class FrameShell { 32 | public: 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 34 | int id; // INTERNAL ID, starting at zero. 35 | int incoming_id; // ID passed into DSO 36 | double timestamp; // timestamp passed into DSO. 37 | 38 | // set once after tracking 39 | SE3 camToTrackingRef; 40 | FrameShell *trackingRef; 41 | 42 | // constantly adapted. 43 | double scale; 44 | Vec3 velInWorld; 45 | SE3 camToWorld; // Write: TRACKING, while frame is still fresh; MAPPING: only 46 | // when locked [shellPoseMutex]. 47 | AffLight aff_g2l; 48 | bool poseValid; 49 | 50 | // statisitcs 51 | int statistics_outlierResOnThis; 52 | int statistics_goodResOnThis; 53 | int marginalizedAt; 54 | double movedByOpt; 55 | 56 | inline FrameShell() { 57 | id = 0; 58 | poseValid = true; 59 | camToWorld = SE3(); 60 | timestamp = 0; 61 | marginalizedAt = -1; 62 | movedByOpt = 0; 63 | statistics_outlierResOnThis = statistics_goodResOnThis = 0; 64 | trackingRef = 0; 65 | camToTrackingRef = SE3(); 66 | } 67 | }; 68 | 69 | } // namespace dso 70 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /src/IOWrapper/ImageDisplay_dummy.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #include "IOWrapper/ImageDisplay.h" 25 | 26 | namespace dso { 27 | 28 | namespace IOWrap { 29 | void displayImage(const char *windowName, const MinimalImageB *img, 30 | bool autoSize){}; 31 | void displayImage(const char *windowName, const MinimalImageB3 *img, 32 | bool autoSize){}; 33 | void displayImage(const char *windowName, const MinimalImageF *img, 34 | bool autoSize){}; 35 | void displayImage(const char *windowName, const MinimalImageF3 *img, 36 | bool autoSize){}; 37 | void displayImage(const char *windowName, const MinimalImageB16 *img, 38 | bool autoSize){}; 39 | 40 | void displayImageStitch(const char *windowName, 41 | const std::vector images, int cc, 42 | int rc){}; 43 | void displayImageStitch(const char *windowName, 44 | const std::vector images, int cc, 45 | int rc){}; 46 | void displayImageStitch(const char *windowName, 47 | const std::vector images, int cc, 48 | int rc){}; 49 | void displayImageStitch(const char *windowName, 50 | const std::vector images, int cc, 51 | int rc){}; 52 | 53 | int waitKey(int milliseconds) { return 0; }; 54 | void closeAllWindows(){}; 55 | } // namespace IOWrap 56 | 57 | } // namespace dso 58 | -------------------------------------------------------------------------------- /src/IOWrapper/ImageDisplay.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | #include "util/MinimalImage.h" 26 | #include "util/NumType.h" 27 | #include 28 | 29 | namespace dso { 30 | 31 | namespace IOWrap { 32 | 33 | void displayImage(const char *windowName, const MinimalImageB *img, 34 | bool autoSize = false); 35 | void displayImage(const char *windowName, const MinimalImageB3 *img, 36 | bool autoSize = false); 37 | void displayImage(const char *windowName, const MinimalImageF *img, 38 | bool autoSize = false); 39 | void displayImage(const char *windowName, const MinimalImageF3 *img, 40 | bool autoSize = false); 41 | void displayImage(const char *windowName, const MinimalImageB16 *img, 42 | bool autoSize = false); 43 | 44 | void displayImageStitch(const char *windowName, 45 | const std::vector images, int cc = 0, 46 | int rc = 0); 47 | void displayImageStitch(const char *windowName, 48 | const std::vector images, int cc = 0, 49 | int rc = 0); 50 | void displayImageStitch(const char *windowName, 51 | const std::vector images, int cc = 0, 52 | int rc = 0); 53 | void displayImageStitch(const char *windowName, 54 | const std::vector images, int cc = 0, 55 | int rc = 0); 56 | 57 | int waitKey(int milliseconds); 58 | void closeAllWindows(); 59 | 60 | } // namespace IOWrap 61 | 62 | } // namespace dso 63 | -------------------------------------------------------------------------------- /src/FullSystem/Residuals.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | 26 | #include "util/globalCalib.h" 27 | #include "vector" 28 | 29 | #include "OptimizationBackend/RawResidualJacobian.h" 30 | #include "util/NumType.h" 31 | #include "util/globalFuncs.h" 32 | #include 33 | #include 34 | 35 | namespace dso { 36 | class PointHessian; 37 | class FrameHessian; 38 | class CalibHessian; 39 | 40 | class EFResidual; 41 | 42 | enum ResLocation { ACTIVE = 0, LINEARIZED, MARGINALIZED, NONE }; 43 | enum ResState { IN = 0, OOB, OUTLIER }; 44 | 45 | struct FullJacRowT { 46 | Eigen::Vector2f projectedTo[MAX_RES_PER_POINT]; 47 | }; 48 | 49 | class PointFrameResidual { 50 | public: 51 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 52 | 53 | EFResidual *efResidual; 54 | 55 | static int instanceCounter; 56 | 57 | ResState state_state; 58 | double state_energy; 59 | ResState state_NewState; 60 | double state_NewEnergy; 61 | double state_NewEnergyWithOutlier; 62 | 63 | void setState(ResState s) { state_state = s; } 64 | 65 | PointHessian *point; 66 | FrameHessian *host; 67 | FrameHessian *target; 68 | RawResidualJacobian *J; 69 | 70 | bool isNew; 71 | 72 | Eigen::Vector2f projectedTo[MAX_RES_PER_POINT]; 73 | Vec3f centerProjectedTo; 74 | 75 | ~PointFrameResidual(); 76 | PointFrameResidual(); 77 | PointFrameResidual(PointHessian *point_, FrameHessian *host_, 78 | FrameHessian *target_); 79 | double linearize(CalibHessian *HCalib); 80 | 81 | void resetOOB() { 82 | state_NewEnergy = state_energy = 0; 83 | state_NewState = ResState::OUTLIER; 84 | 85 | setState(ResState::IN); 86 | }; 87 | void applyRes(bool copyJacobians); 88 | 89 | void debugPlot(); 90 | 91 | void printRows(std::vector &v, VecX &r, int nFrames, int nPoints, int M, 92 | int res); 93 | }; 94 | } // namespace dso 95 | -------------------------------------------------------------------------------- /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" ) -------------------------------------------------------------------------------- /src/FullSystem/ResidualProjections.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | 26 | #include "FullSystem.h" 27 | #include "HessianBlocks.h" 28 | #include "util/NumType.h" 29 | #include "util/settings.h" 30 | 31 | namespace dso { 32 | 33 | EIGEN_STRONG_INLINE float derive_idepth(const Vec3f &t, const float &u, 34 | const float &v, const int &dx, 35 | const int &dy, const float &dxInterp, 36 | const float &dyInterp, 37 | const float &drescale) { 38 | return (dxInterp * drescale * (t[0] - t[2] * u) + 39 | dyInterp * drescale * (t[1] - t[2] * v)) * 40 | SCALE_IDEPTH; 41 | } 42 | 43 | EIGEN_STRONG_INLINE bool projectPoint(const float &u_pt, const float &v_pt, 44 | const float &idepth, const Mat33f &KRKi, 45 | const Vec3f &Kt, float &Ku, float &Kv) { 46 | Vec3f ptp = KRKi * Vec3f(u_pt, v_pt, 1) + Kt * idepth; 47 | Ku = ptp[0] / ptp[2]; 48 | Kv = ptp[1] / ptp[2]; 49 | return Ku > 1.1f && Kv > 1.1f && Ku < wM3G && Kv < hM3G; 50 | } 51 | 52 | EIGEN_STRONG_INLINE bool 53 | projectPoint(const float &u_pt, const float &v_pt, const float &idepth, 54 | const int &dx, const int &dy, CalibHessian *const &HCalib, 55 | const Mat33f &R, const Vec3f &t, float &drescale, float &u, 56 | float &v, float &Ku, float &Kv, Vec3f &KliP, float &new_idepth) { 57 | KliP = Vec3f((u_pt + dx - HCalib->cxl()) * HCalib->fxli(), 58 | (v_pt + dy - HCalib->cyl()) * HCalib->fyli(), 1); 59 | 60 | Vec3f ptp = R * KliP + t * idepth; 61 | drescale = 1.0f / ptp[2]; 62 | new_idepth = idepth * drescale; 63 | 64 | if (!(drescale > 0)) 65 | return false; 66 | 67 | u = ptp[0] * drescale; 68 | v = ptp[1] * drescale; 69 | Ku = u * HCalib->fxl() + HCalib->cxl(); 70 | Kv = v * HCalib->fyl() + HCalib->cyl(); 71 | 72 | return Ku > 1.1f && Kv > 1.1f && Ku < wM3G && Kv < hM3G; 73 | } 74 | 75 | } // namespace dso 76 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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/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/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 | -------------------------------------------------------------------------------- /src/FullSystem/ImmaturePoint.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | 26 | #include "util/NumType.h" 27 | 28 | #include "HessianBlocks.h" 29 | namespace dso { 30 | 31 | struct ImmaturePointTemporaryResidual { 32 | public: 33 | ResState state_state; 34 | double state_energy; 35 | ResState state_NewState; 36 | double state_NewEnergy; 37 | FrameHessian *target; 38 | }; 39 | 40 | enum ImmaturePointStatus { 41 | IPS_GOOD = 0, // traced well and good 42 | IPS_OOB, // OOB: end tracking & marginalize! 43 | IPS_OUTLIER, // energy too high: if happens again: outlier! 44 | IPS_SKIPPED, // traced well and good (but not actually traced). 45 | IPS_BADCONDITION, // not traced because of bad condition. 46 | IPS_UNINITIALIZED 47 | }; // not even traced once. 48 | 49 | class ImmaturePoint { 50 | public: 51 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 52 | // static values 53 | float color[MAX_RES_PER_POINT]; 54 | float weights[MAX_RES_PER_POINT]; 55 | 56 | Mat22f gradH; 57 | Vec2f gradH_ev; 58 | Mat22f gradH_eig; 59 | float energyTH; 60 | float u, v; 61 | FrameHessian *host; 62 | int idxInImmaturePoints; 63 | 64 | float quality; 65 | 66 | float my_type; 67 | 68 | float idepth_min; 69 | float idepth_max; 70 | ImmaturePoint(int u_, int v_, FrameHessian *host_, float type, 71 | CalibHessian *HCalib); 72 | ~ImmaturePoint(); 73 | 74 | ImmaturePointStatus traceOn(FrameHessian *frame, 75 | const Mat33f &hostToFrame_KRKi, 76 | const Vec3f &hostToFrame_Kt, 77 | const Vec2f &hostToFrame_affine, 78 | CalibHessian *HCalib, bool debugPrint = false); 79 | 80 | ImmaturePointStatus lastTraceStatus; 81 | Vec2f lastTraceUV; 82 | float lastTracePixelInterval; 83 | 84 | float idepth_GT; 85 | 86 | double linearizeResidual(CalibHessian *HCalib, const float outlierTHSlack, 87 | ImmaturePointTemporaryResidual *tmpRes, float &Hdd, 88 | float &bd, float idepth); 89 | float getdPixdd(CalibHessian *HCalib, ImmaturePointTemporaryResidual *tmpRes, 90 | float idepth); 91 | 92 | float calcResidual(CalibHessian *HCalib, const float outlierTHSlack, 93 | ImmaturePointTemporaryResidual *tmpRes, float idepth); 94 | 95 | private: 96 | }; 97 | 98 | } // namespace dso 99 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /src/util/globalCalib.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #include "globalCalib.h" 25 | #include "stdio.h" 26 | #include 27 | 28 | namespace dso { 29 | int wG[PYR_LEVELS], hG[PYR_LEVELS]; 30 | float fxG[PYR_LEVELS], fyG[PYR_LEVELS], cxG[PYR_LEVELS], cyG[PYR_LEVELS]; 31 | 32 | float fxiG[PYR_LEVELS], fyiG[PYR_LEVELS], cxiG[PYR_LEVELS], cyiG[PYR_LEVELS]; 33 | 34 | Eigen::Matrix3f KG[PYR_LEVELS], KiG[PYR_LEVELS]; 35 | 36 | float wM3G; 37 | float hM3G; 38 | 39 | void setGlobalCalib(int w, int h, const Eigen::Matrix3f &K) { 40 | int wlvl = w; 41 | int hlvl = h; 42 | pyrLevelsUsed = 1; 43 | while (wlvl % 2 == 0 && hlvl % 2 == 0 && wlvl * hlvl > 5000 && 44 | pyrLevelsUsed < PYR_LEVELS) { 45 | wlvl /= 2; 46 | hlvl /= 2; 47 | pyrLevelsUsed++; 48 | } 49 | printf("using pyramid levels 0 to %d. coarsest resolution: %d x %d!\n", 50 | pyrLevelsUsed - 1, wlvl, hlvl); 51 | if (wlvl > 100 && hlvl > 100) { 52 | printf("\n\n===============WARNING!===================\n " 53 | "using not enough pyramid levels.\n" 54 | "Consider scaling to a resolution that is a multiple of a power of " 55 | "2.\n"); 56 | } 57 | if (pyrLevelsUsed < 3) { 58 | printf("\n\n===============WARNING!===================\n " 59 | "I need higher resolution.\n" 60 | "I will probably segfault.\n"); 61 | } 62 | 63 | wM3G = w - 3; 64 | hM3G = h - 3; 65 | 66 | wG[0] = w; 67 | hG[0] = h; 68 | KG[0] = K; 69 | fxG[0] = K(0, 0); 70 | fyG[0] = K(1, 1); 71 | cxG[0] = K(0, 2); 72 | cyG[0] = K(1, 2); 73 | KiG[0] = KG[0].inverse(); 74 | fxiG[0] = KiG[0](0, 0); 75 | fyiG[0] = KiG[0](1, 1); 76 | cxiG[0] = KiG[0](0, 2); 77 | cyiG[0] = KiG[0](1, 2); 78 | 79 | for (int level = 1; level < pyrLevelsUsed; ++level) { 80 | wG[level] = w >> level; 81 | hG[level] = h >> level; 82 | 83 | fxG[level] = fxG[level - 1] * 0.5; 84 | fyG[level] = fyG[level - 1] * 0.5; 85 | cxG[level] = (cxG[0] + 0.5) / ((int)1 << level) - 0.5; 86 | cyG[level] = (cyG[0] + 0.5) / ((int)1 << level) - 0.5; 87 | 88 | KG[level] << fxG[level], 0.0, cxG[level], 0.0, fyG[level], cyG[level], 0.0, 89 | 0.0, 1.0; // synthetic 90 | KiG[level] = KG[level].inverse(); 91 | 92 | fxiG[level] = KiG[level](0, 0); 93 | fyiG[level] = KiG[level](1, 1); 94 | cxiG[level] = KiG[level](0, 2); 95 | cyiG[level] = KiG[level](1, 2); 96 | } 97 | } 98 | 99 | } // namespace dso 100 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /src/IOWrapper/Pangolin/KeyFrameDisplay.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | #undef Success 28 | #include 29 | #include "util/NumType.h" 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | namespace dso 36 | { 37 | class CalibHessian; 38 | class FrameHessian; 39 | class FrameShell; 40 | 41 | namespace IOWrap 42 | { 43 | 44 | template 45 | struct InputPointSparse 46 | { 47 | float u; 48 | float v; 49 | float idpeth; 50 | float idepth_hessian; 51 | float relObsBaseline; 52 | int numGoodRes; 53 | unsigned char color[ppp]; 54 | unsigned char status; 55 | }; 56 | 57 | struct MyVertex 58 | { 59 | float point[3]; 60 | unsigned char color[4]; 61 | }; 62 | 63 | // stores a pointcloud associated to a Keyframe. 64 | class KeyFrameDisplay 65 | { 66 | 67 | public: 68 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 69 | KeyFrameDisplay(); 70 | ~KeyFrameDisplay(); 71 | 72 | // copies points from KF over to internal buffer, 73 | // keeping some additional information so we can render it differently. 74 | void setFromKF(FrameHessian* fh, CalibHessian* HCalib); 75 | 76 | // copies points from KF over to internal buffer, 77 | // keeping some additional information so we can render it differently. 78 | void setFromF(FrameShell* fs, CalibHessian* HCalib); 79 | 80 | // copies & filters internal data to GL buffer for rendering. if nothing to do: does nothing. 81 | bool refreshPC(bool canRefresh, float scaledTH, float absTH, int mode, float minBS, int sparsity); 82 | 83 | // renders cam & pointcloud. 84 | void drawCam(float lineWidth = 1, float* color = 0, float sizeFactor=1); 85 | void drawPC(float pointSize); 86 | 87 | int id; 88 | bool active; 89 | SE3 camToWorld; 90 | 91 | inline bool operator < (const KeyFrameDisplay& other) const 92 | { 93 | return (id < other.id); 94 | } 95 | 96 | 97 | private: 98 | float fx,fy,cx,cy; 99 | float fxi,fyi,cxi,cyi; 100 | int width, height; 101 | 102 | float my_scaledTH, my_absTH, my_scale; 103 | int my_sparsifyFactor; 104 | int my_displayMode; 105 | float my_minRelBS; 106 | bool needRefresh; 107 | 108 | 109 | int numSparsePoints; 110 | int numSparseBufferSize; 111 | InputPointSparse* originalInputSparse; 112 | 113 | 114 | bool bufferValid; 115 | int numGLBufferPoints; 116 | int numGLBufferGoodPoints; 117 | pangolin::GlBuffer vertexBuffer; 118 | pangolin::GlBuffer colorBuffer; 119 | }; 120 | 121 | } 122 | } 123 | 124 | -------------------------------------------------------------------------------- /src/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 | #pragma once 25 | 26 | #include "NumType.h" 27 | #include "algorithm" 28 | 29 | namespace dso { 30 | 31 | template class MinimalImage { 32 | public: 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 34 | int w; 35 | int h; 36 | T *data; 37 | 38 | /* 39 | * creates minimal image with own memory 40 | */ 41 | inline MinimalImage(int w_, int h_) : w(w_), h(h_) { 42 | data = new T[w * h]; 43 | ownData = true; 44 | } 45 | 46 | /* 47 | * creates minimal image wrapping around existing memory 48 | */ 49 | inline MinimalImage(int w_, int h_, T *data_) : w(w_), h(h_) { 50 | data = data_; 51 | ownData = false; 52 | } 53 | 54 | inline ~MinimalImage() { 55 | if (ownData) 56 | delete[] data; 57 | } 58 | 59 | inline MinimalImage *getClone() { 60 | MinimalImage *clone = new MinimalImage(w, h); 61 | memcpy(clone->data, data, sizeof(T) * w * h); 62 | return clone; 63 | } 64 | 65 | inline T &at(int x, int y) { return data[(int)x + ((int)y) * w]; } 66 | inline T &at(int i) { return data[i]; } 67 | 68 | inline void setBlack() { memset(data, 0, sizeof(T) * w * h); } 69 | 70 | inline void setConst(T val) { 71 | for (int i = 0; i < w * h; i++) 72 | data[i] = val; 73 | } 74 | 75 | inline void setPixel1(const float &u, const float &v, T val) { 76 | at(u + 0.5f, v + 0.5f) = val; 77 | } 78 | 79 | inline void setPixel4(const float &u, const float &v, T val) { 80 | at(u + 1.0f, v + 1.0f) = val; 81 | at(u + 1.0f, v) = val; 82 | at(u, v + 1.0f) = val; 83 | at(u, v) = val; 84 | } 85 | 86 | inline void setPixel9(const int &u, const int &v, T val) { 87 | at(u + 1, v - 1) = val; 88 | at(u + 1, v) = val; 89 | at(u + 1, v + 1) = val; 90 | at(u, v - 1) = val; 91 | at(u, v) = val; 92 | at(u, v + 1) = val; 93 | at(u - 1, v - 1) = val; 94 | at(u - 1, v) = val; 95 | at(u - 1, v + 1) = val; 96 | } 97 | 98 | inline void setPixelCirc(const int &u, const int &v, T val) { 99 | for (int i = -3; i <= 3; i++) { 100 | at(u + 3, v + i) = val; 101 | at(u - 3, v + i) = val; 102 | at(u + 2, v + i) = val; 103 | at(u - 2, v + i) = val; 104 | 105 | at(u + i, v - 3) = val; 106 | at(u + i, v + 3) = val; 107 | at(u + i, v - 2) = val; 108 | at(u + i, v + 2) = val; 109 | } 110 | } 111 | 112 | private: 113 | bool ownData; 114 | }; 115 | 116 | typedef Eigen::Matrix Vec3b; 117 | typedef MinimalImage MinimalImageF; 118 | typedef MinimalImage MinimalImageF3; 119 | typedef MinimalImage MinimalImageB; 120 | typedef MinimalImage MinimalImageB3; 121 | typedef MinimalImage MinimalImageB16; 122 | 123 | } // namespace dso 124 | -------------------------------------------------------------------------------- /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 | #pragma once 25 | 26 | #include "RawResidualJacobian.h" 27 | #include "util/NumType.h" 28 | #include "vector" 29 | #include 30 | 31 | namespace dso { 32 | 33 | class PointFrameResidual; 34 | class CalibHessian; 35 | class FrameHessian; 36 | class PointHessian; 37 | 38 | class EFResidual; 39 | class EFPoint; 40 | class EFFrame; 41 | class EnergyFunctional; 42 | 43 | class EFResidual { 44 | public: 45 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 46 | 47 | inline EFResidual(PointFrameResidual *org, EFPoint *point_, EFFrame *host_, 48 | EFFrame *target_) 49 | : data(org), point(point_), host(host_), target(target_) { 50 | isLinearized = false; 51 | isActiveAndIsGoodNEW = false; 52 | J = new RawResidualJacobian(); 53 | assert(((long)this) % 16 == 0); 54 | assert(((long)J) % 16 == 0); 55 | } 56 | inline ~EFResidual() { delete J; } 57 | 58 | void takeDataF(); 59 | 60 | void fixLinearizationF(EnergyFunctional *ef); 61 | 62 | // structural pointers 63 | PointFrameResidual *data; 64 | int hostIDX, targetIDX; 65 | EFPoint *point; 66 | EFFrame *host; 67 | EFFrame *target; 68 | int idxInAll; 69 | 70 | RawResidualJacobian *J; 71 | 72 | VecNRf res_toZeroF; 73 | Vec8f JpJdF; 74 | 75 | // status. 76 | bool isLinearized; 77 | 78 | // if residual is not OOB & not OUTLIER & should be used during accumulations 79 | bool isActiveAndIsGoodNEW; 80 | inline const bool &isActive() const { return isActiveAndIsGoodNEW; } 81 | }; 82 | 83 | enum EFPointStatus { PS_GOOD = 0, PS_MARGINALIZE, PS_DROP }; 84 | 85 | class EFPoint { 86 | public: 87 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 88 | EFPoint(PointHessian *d, EFFrame *host_) : data(d), host(host_) { 89 | takeData(); 90 | stateFlag = EFPointStatus::PS_GOOD; 91 | } 92 | void takeData(); 93 | 94 | PointHessian *data; 95 | 96 | float priorF; 97 | float deltaF; 98 | 99 | // constant info (never changes in-between). 100 | int idxInPoints; 101 | EFFrame *host; 102 | 103 | // contains all residuals. 104 | std::vector residualsAll; 105 | 106 | float bdSumF; 107 | float HdiF; 108 | float Hdd_accLF; 109 | VecCf Hcd_accLF; 110 | float bd_accLF; 111 | float Hdd_accAF; 112 | VecCf Hcd_accAF; 113 | float bd_accAF; 114 | 115 | EFPointStatus stateFlag; 116 | }; 117 | 118 | class EFFrame { 119 | public: 120 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 121 | EFFrame(FrameHessian *d) : data(d) { takeData(); } 122 | void takeData(); 123 | 124 | Vec8 prior; // prior hessian (diagonal) 125 | Vec8 delta_prior; // = state-state_prior (E_prior = (delta_prior)' * 126 | // diag(prior) * (delta_prior) 127 | Vec8 delta; // state - state_zero. 128 | 129 | std::vector points; 130 | FrameHessian *data; 131 | int idx; // idx in frames. 132 | 133 | int frameID; 134 | }; 135 | 136 | } // namespace dso 137 | -------------------------------------------------------------------------------- /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 | #include "EnergyFunctionalStructs.h" 25 | #include "EnergyFunctional.h" 26 | #include "FullSystem/FullSystem.h" 27 | #include "FullSystem/HessianBlocks.h" 28 | #include "FullSystem/Residuals.h" 29 | 30 | #if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) 31 | #include "SSE2NEON.h" 32 | #endif 33 | 34 | namespace dso { 35 | 36 | void EFResidual::takeDataF() { 37 | std::swap(J, data->J); 38 | 39 | Vec2f JI_JI_Jd = J->JIdx2 * J->Jpdd; 40 | 41 | for (int i = 0; i < 6; i++) 42 | JpJdF[i] = J->Jpdxi[0][i] * JI_JI_Jd[0] + J->Jpdxi[1][i] * JI_JI_Jd[1]; 43 | 44 | JpJdF.segment<2>(6) = J->JabJIdx * J->Jpdd; 45 | } 46 | 47 | void EFFrame::takeData() { 48 | prior = data->getPrior().head<8>(); 49 | delta = data->get_state_minus_stateZero().head<8>(); 50 | delta_prior = (data->get_state() - data->getPriorZero()).head<8>(); 51 | 52 | // Vec10 state_zero = data->get_state_zero(); 53 | // state_zero.segment<3>(0) = SCALE_XI_TRANS * state_zero.segment<3>(0); 54 | // state_zero.segment<3>(3) = SCALE_XI_ROT * state_zero.segment<3>(3); 55 | // state_zero[6] = SCALE_A * state_zero[6]; 56 | // state_zero[7] = SCALE_B * state_zero[7]; 57 | // state_zero[8] = SCALE_A * state_zero[8]; 58 | // state_zero[9] = SCALE_B * state_zero[9]; 59 | // 60 | // std::cout << "state_zero: " << state_zero.transpose() << "\n"; 61 | 62 | assert(data->frameID != -1); 63 | 64 | frameID = data->frameID; 65 | } 66 | 67 | void EFPoint::takeData() { 68 | priorF = data->hasDepthPrior 69 | ? setting_idepthFixPrior * SCALE_IDEPTH * SCALE_IDEPTH 70 | : 0; 71 | 72 | deltaF = data->idepth - data->idepth_zero; 73 | } 74 | 75 | void EFResidual::fixLinearizationF(EnergyFunctional *ef) { 76 | Vec8f dp = ef->adHTdeltaF[hostIDX + ef->nFrames * targetIDX]; 77 | 78 | // compute Jp*delta 79 | __m128 Jp_delta_x = 80 | _mm_set1_ps(J->Jpdxi[0].dot(dp.head<6>()) + J->Jpdc[0].dot(ef->cDeltaF) + 81 | J->Jpdd[0] * point->deltaF); 82 | __m128 Jp_delta_y = 83 | _mm_set1_ps(J->Jpdxi[1].dot(dp.head<6>()) + J->Jpdc[1].dot(ef->cDeltaF) + 84 | J->Jpdd[1] * point->deltaF); 85 | __m128 delta_a = _mm_set1_ps((float)(dp[6])); 86 | __m128 delta_b = _mm_set1_ps((float)(dp[7])); 87 | 88 | for (int i = 0; i < patternNum; i += 4) { 89 | // PATTERN: rtz = resF - [JI*Jp Ja]*delta. 90 | __m128 rtz = _mm_load_ps(((float *)&J->resF) + i); 91 | rtz = _mm_sub_ps( 92 | rtz, _mm_mul_ps(_mm_load_ps(((float *)(J->JIdx)) + i), Jp_delta_x)); 93 | rtz = _mm_sub_ps( 94 | rtz, _mm_mul_ps(_mm_load_ps(((float *)(J->JIdx + 1)) + i), Jp_delta_y)); 95 | rtz = _mm_sub_ps( 96 | rtz, _mm_mul_ps(_mm_load_ps(((float *)(J->JabF)) + i), delta_a)); 97 | rtz = _mm_sub_ps( 98 | rtz, _mm_mul_ps(_mm_load_ps(((float *)(J->JabF + 1)) + i), delta_b)); 99 | _mm_store_ps(((float *)&res_toZeroF) + i, rtz); 100 | } 101 | 102 | isLinearized = true; 103 | } 104 | 105 | } // namespace dso 106 | -------------------------------------------------------------------------------- /src/IOWrapper/OpenCV/ImageRW_OpenCV.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | 26 | #include "IOWrapper/ImageRW.h" 27 | #include 28 | 29 | 30 | namespace dso 31 | { 32 | 33 | namespace IOWrap 34 | { 35 | MinimalImageB* readImageBW_8U(std::string filename) 36 | { 37 | cv::Mat m = cv::imread(filename, cv::IMREAD_GRAYSCALE); 38 | if(m.rows*m.cols==0) 39 | { 40 | printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str()); 41 | return 0; 42 | } 43 | if(m.type() != CV_8U) 44 | { 45 | printf("cv::imread did something strange! this may segfault. \n"); 46 | return 0; 47 | } 48 | MinimalImageB* img = new MinimalImageB(m.cols, m.rows); 49 | memcpy(img->data, m.data, m.rows*m.cols); 50 | return img; 51 | } 52 | 53 | MinimalImageB3* readImageRGB_8U(std::string filename) 54 | { 55 | cv::Mat m = cv::imread(filename, cv::IMREAD_COLOR); 56 | if(m.rows*m.cols==0) 57 | { 58 | printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str()); 59 | return 0; 60 | } 61 | if(m.type() != CV_8UC3) 62 | { 63 | printf("cv::imread did something strange! this may segfault. \n"); 64 | return 0; 65 | } 66 | MinimalImageB3* img = new MinimalImageB3(m.cols, m.rows); 67 | memcpy(img->data, m.data, 3*m.rows*m.cols); 68 | return img; 69 | } 70 | 71 | MinimalImage* readImageBW_16U(std::string filename) 72 | { 73 | cv::Mat m = cv::imread(filename, cv::IMREAD_UNCHANGED); 74 | if(m.rows*m.cols==0) 75 | { 76 | printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str()); 77 | return 0; 78 | } 79 | if(m.type() != CV_16U) 80 | { 81 | printf("readImageBW_16U called on image that is not a 16bit grayscale image. this may segfault. \n"); 82 | return 0; 83 | } 84 | MinimalImage* img = new MinimalImage(m.cols, m.rows); 85 | memcpy(img->data, m.data, 2*m.rows*m.cols); 86 | return img; 87 | } 88 | 89 | MinimalImageB* readStreamBW_8U(char* data, int numBytes) 90 | { 91 | cv::Mat m = cv::imdecode(cv::Mat(numBytes,1,CV_8U, data), cv::IMREAD_GRAYSCALE); 92 | if(m.rows*m.cols==0) 93 | { 94 | printf("cv::imdecode could not read stream (%d bytes)! this may segfault. \n", numBytes); 95 | return 0; 96 | } 97 | if(m.type() != CV_8U) 98 | { 99 | printf("cv::imdecode did something strange! this may segfault. \n"); 100 | return 0; 101 | } 102 | MinimalImageB* img = new MinimalImageB(m.cols, m.rows); 103 | memcpy(img->data, m.data, m.rows*m.cols); 104 | return img; 105 | } 106 | 107 | 108 | 109 | void writeImage(std::string filename, MinimalImageB* img) 110 | { 111 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_8U, img->data)); 112 | } 113 | void writeImage(std::string filename, MinimalImageB3* img) 114 | { 115 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_8UC3, img->data)); 116 | } 117 | void writeImage(std::string filename, MinimalImageF* img) 118 | { 119 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_32F, img->data)); 120 | } 121 | void writeImage(std::string filename, MinimalImageF3* img) 122 | { 123 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_32FC3, img->data)); 124 | } 125 | 126 | } 127 | 128 | } 129 | -------------------------------------------------------------------------------- /src/IOWrapper/Pangolin/PangolinDSOViewer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | #include 27 | #include "boost/thread.hpp" 28 | #include "util/MinimalImage.h" 29 | #include "IOWrapper/Output3DWrapper.h" 30 | #include 31 | #include 32 | 33 | 34 | namespace dso 35 | { 36 | 37 | class FrameHessian; 38 | class CalibHessian; 39 | class FrameShell; 40 | 41 | 42 | namespace IOWrap 43 | { 44 | 45 | class KeyFrameDisplay; 46 | 47 | struct GraphConnection 48 | { 49 | KeyFrameDisplay* from; 50 | KeyFrameDisplay* to; 51 | int fwdMarg, bwdMarg, fwdAct, bwdAct; 52 | }; 53 | 54 | 55 | class PangolinDSOViewer : public Output3DWrapper 56 | { 57 | public: 58 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 59 | PangolinDSOViewer(int w, int h, bool startRunThread=true); 60 | virtual ~PangolinDSOViewer(); 61 | 62 | void run(); 63 | void close(); 64 | 65 | void addImageToDisplay(std::string name, MinimalImageB3* image); 66 | void clearAllImagesToDisplay(); 67 | 68 | 69 | // ==================== Output3DWrapper Functionality ====================== 70 | virtual void publishGraph(const std::map, Eigen::aligned_allocator>> &connectivity) override; 71 | virtual void publishKeyframes( std::vector &frames, bool final, CalibHessian* HCalib) override; 72 | virtual void publishCamPose(FrameShell* frame, CalibHessian* HCalib) override; 73 | 74 | 75 | virtual void pushLiveFrame(FrameHessian* image) override; 76 | virtual void pushDepthImage(MinimalImageB3* image) override; 77 | virtual bool needPushDepthImage() override; 78 | 79 | virtual void join() override; 80 | 81 | virtual void reset() override; 82 | private: 83 | 84 | bool needReset; 85 | void reset_internal(); 86 | void drawConstraints(); 87 | 88 | boost::thread runThread; 89 | bool running; 90 | int w,h; 91 | 92 | 93 | 94 | // images rendering 95 | boost::mutex openImagesMutex; 96 | MinimalImageB3* internalVideoImg; 97 | MinimalImageB3* internalKFImg; 98 | MinimalImageB3* internalResImg; 99 | bool videoImgChanged, kfImgChanged, resImgChanged; 100 | 101 | 102 | 103 | // 3D model rendering 104 | boost::mutex model3DMutex; 105 | KeyFrameDisplay* currentCam; 106 | std::vector keyframes; 107 | std::vector> allFramePoses; 108 | std::map keyframesByKFID; 109 | std::vector> connections; 110 | 111 | 112 | 113 | // render settings 114 | bool settings_showKFCameras; 115 | bool settings_showCurrentCamera; 116 | bool settings_showTrajectory; 117 | bool settings_showFullTrajectory; 118 | bool settings_showActiveConstraints; 119 | bool settings_showAllConstraints; 120 | 121 | float settings_scaledVarTH; 122 | float settings_absVarTH; 123 | int settings_pointCloudMode; 124 | float settings_minRelBS; 125 | int settings_sparsity; 126 | 127 | 128 | // timings 129 | struct timeval last_track; 130 | struct timeval last_map; 131 | 132 | 133 | std::deque lastNTrackingMs; 134 | std::deque lastNMappingMs; 135 | }; 136 | 137 | 138 | 139 | } 140 | 141 | 142 | 143 | } 144 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /src/FullSystem/CoarseTracker.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) <2020> 2 | 3 | // This program is free software: you can redistribute it and/or modify 4 | // it under the terms of the GNU General Public License as published by 5 | // the Free Software Foundation, either version 3 of the License, or 6 | // (at your option) any later version. 7 | 8 | // This program is distributed in the hope that it will be useful, 9 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | // GNU General Public License for more details. 12 | 13 | // You should have received a copy of the GNU General Public License 14 | // along with this program. If not, see . 15 | 16 | // This file is modified from 17 | 18 | #pragma once 19 | 20 | #include "IOWrapper/Output3DWrapper.h" 21 | #include "OptimizationBackend/MatrixAccumulators.h" 22 | #include "util/NumType.h" 23 | #include "util/settings.h" 24 | 25 | #include 26 | #include 27 | 28 | namespace dso { 29 | struct CalibHessian; 30 | struct FrameHessian; 31 | struct PointFrameResidual; 32 | 33 | template 34 | T *allocAligned(int size, std::vector &rawPtrVec) { 35 | const int padT = 1 + ((1 << b) / sizeof(T)); 36 | T *ptr = new T[size + padT]; 37 | rawPtrVec.push_back(ptr); 38 | T *alignedPtr = (T *)((((uintptr_t)(ptr + padT)) >> b) << b); 39 | return alignedPtr; 40 | } 41 | 42 | class CoarseTracker { 43 | public: 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 45 | 46 | CoarseTracker(int w, int h); 47 | ~CoarseTracker(); 48 | 49 | void makeK(CalibHessian *HCalib); 50 | 51 | void setCoarseTrackingRef(std::vector frameHessians); 52 | 53 | void scaleCoarseDepthL0(float scale); 54 | 55 | void debugPlotIDepthMap(float *minID, float *maxID, 56 | std::vector &wraps); 57 | void debugPlotIDepthMapFloat(std::vector &wraps); 58 | 59 | bool trackNewestCoarse(FrameHessian *newFrameHessian, SE3 &lastToNew_out, 60 | AffLight &aff_g2l_out, int coarsestLvl, 61 | Vec5 minResForAbort, Vec5 &lastResiduals, 62 | IOWrap::Output3DWrapper *wrap = 0); 63 | 64 | // act as pure ouptut 65 | int refFrameID; 66 | FrameHessian *lastRef; 67 | AffLight lastRef_aff_g2l; 68 | Vec3 lastFlowIndicators; 69 | double firstCoarseRMSE; 70 | 71 | private: 72 | void makeCoarseDepthL0(std::vector frameHessians); 73 | 74 | Vec6 calcRes(int lvl, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH, 75 | bool plot_img = false); 76 | void calcGSSSE(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, 77 | AffLight aff_g2l); 78 | 79 | std::vector ptrToDelete; 80 | 81 | float *idepth_[PYR_LEVELS]; 82 | float *weight_sums_[PYR_LEVELS]; 83 | float *weight_sums_bak_[PYR_LEVELS]; 84 | 85 | Mat33f Ki_[PYR_LEVELS]; 86 | float fx_[PYR_LEVELS]; 87 | float fy_[PYR_LEVELS]; 88 | float cx_[PYR_LEVELS]; 89 | float cy_[PYR_LEVELS]; 90 | int w_[PYR_LEVELS]; 91 | int h_[PYR_LEVELS]; 92 | 93 | // pc buffers 94 | float *pc_u_[PYR_LEVELS]; 95 | float *pc_v_[PYR_LEVELS]; 96 | float *pc_idepth_[PYR_LEVELS]; 97 | float *pc_color_[PYR_LEVELS]; 98 | int pc_n_[PYR_LEVELS]; 99 | 100 | // warped buffers 101 | float *poseBufWarped_idepth; 102 | float *poseBufWarped_u; 103 | float *poseBufWarped_v; 104 | float *poseBufWarped_dx; 105 | float *poseBufWarped_dy; 106 | float *poseBufWarped_residual; 107 | float *poseBufWarped_weight; 108 | float *poseBufWarped_refColor; 109 | int poseBufWarped_n; 110 | 111 | FrameHessian *newFrame; 112 | Accumulator9 poseAcc; 113 | }; 114 | 115 | class CoarseDistanceMap { 116 | public: 117 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 118 | 119 | Mat33f K[PYR_LEVELS]; 120 | Mat33f Ki[PYR_LEVELS]; 121 | 122 | CoarseDistanceMap(int w, int h); 123 | ~CoarseDistanceMap(); 124 | 125 | void makeDistanceMap(std::vector frameHessians, 126 | FrameHessian *frame); 127 | 128 | void makeInlierVotes(std::vector frameHessians); 129 | 130 | void makeK(CalibHessian *HCalib); 131 | 132 | float *fwdWarpedIDDistFinal; 133 | 134 | void addIntoDistFinal(int u, int v); 135 | 136 | private: 137 | int w_[PYR_LEVELS]; 138 | int h_[PYR_LEVELS]; 139 | 140 | PointFrameResidual **coarseProjectionGrid; 141 | int *coarseProjectionGridnum; 142 | Eigen::Vector2i *bfsList1; 143 | Eigen::Vector2i *bfsList2; 144 | 145 | void growDistBFS(int bfsNum); 146 | }; 147 | 148 | } // namespace dso 149 | -------------------------------------------------------------------------------- /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 | #pragma once 25 | 26 | #include "map" 27 | #include "util/IndexThreadReduce.h" 28 | #include "util/NumType.h" 29 | #include "vector" 30 | #include 31 | 32 | namespace dso { 33 | 34 | class PointFrameResidual; 35 | class CalibHessian; 36 | class FrameHessian; 37 | class PointHessian; 38 | 39 | class EFResidual; 40 | class EFPoint; 41 | class EFFrame; 42 | class EnergyFunctional; 43 | class AccumulatedTopHessian; 44 | class AccumulatedTopHessianSSE; 45 | class AccumulatedSCHessian; 46 | class AccumulatedSCHessianSSE; 47 | 48 | extern bool EFAdjointsValid; 49 | extern bool EFIndicesValid; 50 | extern bool EFDeltaValid; 51 | 52 | class EnergyFunctional { 53 | public: 54 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 55 | friend class EFFrame; 56 | friend class EFPoint; 57 | friend class EFResidual; 58 | friend class AccumulatedTopHessian; 59 | friend class AccumulatedTopHessianSSE; 60 | friend class AccumulatedSCHessian; 61 | friend class AccumulatedSCHessianSSE; 62 | 63 | EnergyFunctional(); 64 | ~EnergyFunctional(); 65 | 66 | EFResidual *insertResidual(PointFrameResidual *r); 67 | EFFrame *insertFrame(FrameHessian *fh, CalibHessian *HCalib); 68 | EFPoint *insertPoint(PointHessian *ph); 69 | 70 | void dropResidual(EFResidual *r); 71 | void marginalizeFrame(EFFrame *fh, CalibHessian *HCalib); 72 | void removePoint(EFPoint *ph); 73 | 74 | void marginalizePointsF(); 75 | void dropPointsF(); 76 | void solveSystemF(int iteration, double lambda, CalibHessian *HCalib); 77 | double calcMEnergyF(); 78 | double calcLEnergyF_MT(); 79 | 80 | void makeIDX(); 81 | 82 | void setDeltaF(CalibHessian *HCalib); 83 | 84 | void setAdjointsF(CalibHessian *HCalib); 85 | 86 | std::vector frames; 87 | int nPoints, nFrames, nResiduals; 88 | 89 | int resInA, resInL, resInM; 90 | VecX lastX; 91 | std::vector lastNullspaces_forLogging; 92 | std::vector lastNullspaces_pose; 93 | std::vector lastNullspaces_scale; 94 | std::vector lastNullspaces_affA; 95 | std::vector lastNullspaces_affB; 96 | 97 | IndexThreadReduce *red; 98 | 99 | std::map, 100 | Eigen::aligned_allocator>> 101 | connectivityMap; 102 | 103 | // ToDo: move to private 104 | void getImuHessian(MatXX &H, VecX &b, MatXX &J_cst, VecX &r_cst, 105 | CalibHessian *HCalib, std::vector &is_spline_valid, 106 | bool print = false); 107 | 108 | private: 109 | VecX getStitchedDeltaF() const; 110 | 111 | void resubstituteF_MT(VecX x, CalibHessian *HCalib, bool MT); 112 | void resubstituteFPt(const VecCf &xc, Mat18f *xAd, int min, int max, 113 | Vec10 *stats, int tid); 114 | 115 | void accumulateAF_MT(MatXX &H, VecX &b, bool MT); 116 | void accumulateLF_MT(MatXX &H, VecX &b, bool MT); 117 | void accumulateSCF_MT(MatXX &H, VecX &b, bool MT); 118 | 119 | void expandHbtoFitImu(MatXX &H, VecX &b); 120 | 121 | void getImuHessianCurrentFrame(int fi, CalibHessian *HCalib, MatXX &H, 122 | VecX &b, bool &spline_valid, MatXX &J_vr, 123 | VecX &r_vr, bool print); 124 | 125 | void calcLEnergyPt(int min, int max, Vec10 *stats, int tid); 126 | 127 | void orthogonalize(VecX *b, MatXX *H); 128 | Mat18f *adHTdeltaF; 129 | 130 | Mat88 *adHost; 131 | Mat88 *adTarget; 132 | 133 | Mat88f *adHostF; 134 | Mat88f *adTargetF; 135 | 136 | VecC cPrior; 137 | VecCf cDeltaF; 138 | VecCf cPriorF; 139 | 140 | MatXX HM; 141 | VecX bM; 142 | 143 | MatXX HM_bias; 144 | VecX bM_bias; 145 | 146 | MatXX HM_imu; 147 | VecX bM_imu; 148 | 149 | AccumulatedTopHessianSSE *accSSE_top_L; 150 | AccumulatedTopHessianSSE *accSSE_top_A; 151 | 152 | AccumulatedSCHessianSSE *accSSE_bot; 153 | 154 | std::vector allPoints; 155 | std::vector allPointsToMarg; 156 | 157 | float currentLambda; 158 | }; 159 | } // namespace dso 160 | -------------------------------------------------------------------------------- /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 | #pragma once 25 | 26 | #include "MatrixAccumulators.h" 27 | #include "util/IndexThreadReduce.h" 28 | #include "util/NumType.h" 29 | #include "vector" 30 | #include 31 | 32 | namespace dso { 33 | 34 | class EFPoint; 35 | class EnergyFunctional; 36 | 37 | class AccumulatedSCHessianSSE { 38 | public: 39 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 40 | inline AccumulatedSCHessianSSE() { 41 | for (int i = 0; i < NUM_THREADS; i++) { 42 | accE[i] = 0; 43 | accEB[i] = 0; 44 | accD[i] = 0; 45 | nframes[i] = 0; 46 | } 47 | }; 48 | inline ~AccumulatedSCHessianSSE() { 49 | for (int i = 0; i < NUM_THREADS; i++) { 50 | if (accE[i] != 0) 51 | delete[] accE[i]; 52 | if (accEB[i] != 0) 53 | delete[] accEB[i]; 54 | if (accD[i] != 0) 55 | delete[] accD[i]; 56 | } 57 | }; 58 | 59 | inline void setZero(int n, int min = 0, int max = 1, Vec10 *stats = 0, 60 | int tid = 0) { 61 | if (n != nframes[tid]) { 62 | if (accE[tid] != 0) 63 | delete[] accE[tid]; 64 | if (accEB[tid] != 0) 65 | delete[] accEB[tid]; 66 | if (accD[tid] != 0) 67 | delete[] accD[tid]; 68 | accE[tid] = new AccumulatorXX<8, CPARS>[n * n]; 69 | accEB[tid] = new AccumulatorX<8>[n * n]; 70 | accD[tid] = new AccumulatorXX<8, 8>[n * n * n]; 71 | } 72 | accbc[tid].initialize(); 73 | accHcc[tid].initialize(); 74 | 75 | for (int i = 0; i < n * n; i++) { 76 | accE[tid][i].initialize(); 77 | accEB[tid][i].initialize(); 78 | 79 | for (int j = 0; j < n; j++) 80 | accD[tid][i * n + j].initialize(); 81 | } 82 | nframes[tid] = n; 83 | } 84 | void stitchDouble(MatXX &H_sc, VecX &b_sc, EnergyFunctional const *const EF, 85 | int tid = 0); 86 | void addPoint(EFPoint *p, bool shiftPriorToZero, int tid = 0); 87 | 88 | void stitchDoubleMT(IndexThreadReduce *red, MatXX &H, VecX &b, 89 | EnergyFunctional const *const EF, bool MT) { 90 | // sum up, splitting by bock in square. 91 | if (MT) { 92 | MatXX Hs[NUM_THREADS]; 93 | VecX bs[NUM_THREADS]; 94 | for (int i = 0; i < NUM_THREADS; i++) { 95 | assert(nframes[0] == nframes[i]); 96 | Hs[i] = MatXX::Zero(nframes[0] * 8 + CPARS, nframes[0] * 8 + CPARS); 97 | bs[i] = VecX::Zero(nframes[0] * 8 + CPARS); 98 | } 99 | 100 | red->reduce(boost::bind(&AccumulatedSCHessianSSE::stitchDoubleInternal, 101 | this, Hs, bs, EF, _1, _2, _3, _4), 102 | 0, nframes[0] * nframes[0], 0); 103 | 104 | // sum up results 105 | H = Hs[0]; 106 | b = bs[0]; 107 | 108 | for (int i = 1; i < NUM_THREADS; i++) { 109 | H.noalias() += Hs[i]; 110 | b.noalias() += bs[i]; 111 | } 112 | } else { 113 | H = MatXX::Zero(nframes[0] * 8 + CPARS, nframes[0] * 8 + CPARS); 114 | b = VecX::Zero(nframes[0] * 8 + CPARS); 115 | stitchDoubleInternal(&H, &b, EF, 0, nframes[0] * nframes[0], 0, -1); 116 | } 117 | 118 | // make diagonal by copying over parts. 119 | for (int h = 0; h < nframes[0]; h++) { 120 | int hIdx = CPARS + h * 8; 121 | H.block(0, hIdx).noalias() = 122 | H.block<8, CPARS>(hIdx, 0).transpose(); 123 | } 124 | } 125 | 126 | AccumulatorXX<8, CPARS> *accE[NUM_THREADS]; 127 | AccumulatorX<8> *accEB[NUM_THREADS]; 128 | AccumulatorXX<8, 8> *accD[NUM_THREADS]; 129 | AccumulatorXX accHcc[NUM_THREADS]; 130 | AccumulatorX accbc[NUM_THREADS]; 131 | int nframes[NUM_THREADS]; 132 | 133 | void addPointsInternal(std::vector *points, bool shiftPriorToZero, 134 | int min = 0, int max = 1, Vec10 *stats = 0, 135 | int tid = 0) { 136 | for (int i = min; i < max; i++) 137 | addPoint((*points)[i], shiftPriorToZero, tid); 138 | } 139 | 140 | private: 141 | void stitchDoubleInternal(MatXX *H, VecX *b, EnergyFunctional const *const EF, 142 | int min, int max, Vec10 *stats, int tid); 143 | }; 144 | 145 | } // namespace dso 146 | -------------------------------------------------------------------------------- /src/OptimizationBackend/AccumulatedTopHessian.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | 26 | #include "MatrixAccumulators.h" 27 | #include "util/IndexThreadReduce.h" 28 | #include "util/NumType.h" 29 | #include "vector" 30 | #include 31 | 32 | namespace dso { 33 | 34 | class EFPoint; 35 | class EnergyFunctional; 36 | 37 | class AccumulatedTopHessianSSE { 38 | public: 39 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 40 | inline AccumulatedTopHessianSSE() { 41 | for (int tid = 0; tid < NUM_THREADS; tid++) { 42 | nres[tid] = 0; 43 | acc[tid] = 0; 44 | nframes[tid] = 0; 45 | } 46 | }; 47 | inline ~AccumulatedTopHessianSSE() { 48 | for (int tid = 0; tid < NUM_THREADS; tid++) { 49 | if (acc[tid] != 0) 50 | delete[] acc[tid]; 51 | } 52 | }; 53 | 54 | inline void setZero(int nFrames, int min = 0, int max = 1, Vec10 *stats = 0, 55 | int tid = 0) { 56 | 57 | if (nFrames != nframes[tid]) { 58 | if (acc[tid] != 0) 59 | delete[] acc[tid]; 60 | #if USE_XI_MODEL 61 | acc[tid] = new Accumulator14[nFrames * nFrames]; 62 | #else 63 | acc[tid] = new AccumulatorApprox[nFrames * nFrames]; 64 | #endif 65 | } 66 | 67 | for (int i = 0; i < nFrames * nFrames; i++) { 68 | acc[tid][i].initialize(); 69 | } 70 | 71 | nframes[tid] = nFrames; 72 | nres[tid] = 0; 73 | } 74 | void stitchDouble(MatXX &H, VecX &b, EnergyFunctional const *const EF, 75 | bool usePrior, bool useDelta, int tid = 0); 76 | 77 | template 78 | void addPoint(EFPoint *p, EnergyFunctional const *const ef, int tid = 0); 79 | 80 | void stitchDoubleMT(IndexThreadReduce *red, MatXX &H, VecX &b, 81 | EnergyFunctional const *const EF, bool usePrior, 82 | bool MT) { 83 | // sum up, splitting by bock in square. 84 | if (MT) { 85 | MatXX Hs[NUM_THREADS]; 86 | VecX bs[NUM_THREADS]; 87 | for (int i = 0; i < NUM_THREADS; i++) { 88 | assert(nframes[0] == nframes[i]); 89 | Hs[i] = MatXX::Zero(nframes[0] * 8 + CPARS, nframes[0] * 8 + CPARS); 90 | bs[i] = VecX::Zero(nframes[0] * 8 + CPARS); 91 | } 92 | 93 | red->reduce(boost::bind(&AccumulatedTopHessianSSE::stitchDoubleInternal, 94 | this, Hs, bs, EF, usePrior, _1, _2, _3, _4), 95 | 0, nframes[0] * nframes[0], 0); 96 | 97 | // sum up results 98 | H = Hs[0]; 99 | b = bs[0]; 100 | 101 | for (int i = 1; i < NUM_THREADS; i++) { 102 | H.noalias() += Hs[i]; 103 | b.noalias() += bs[i]; 104 | nres[0] += nres[i]; 105 | } 106 | } else { 107 | H = MatXX::Zero(nframes[0] * 8 + CPARS, nframes[0] * 8 + CPARS); 108 | b = VecX::Zero(nframes[0] * 8 + CPARS); 109 | stitchDoubleInternal(&H, &b, EF, usePrior, 0, nframes[0] * nframes[0], 0, 110 | -1); 111 | } 112 | 113 | // make diagonal by copying over parts. 114 | for (int h = 0; h < nframes[0]; h++) { 115 | int hIdx = CPARS + h * 8; 116 | H.block(0, hIdx).noalias() = 117 | H.block<8, CPARS>(hIdx, 0).transpose(); 118 | 119 | for (int t = h + 1; t < nframes[0]; t++) { 120 | int tIdx = CPARS + t * 8; 121 | H.block<8, 8>(hIdx, tIdx).noalias() += 122 | H.block<8, 8>(tIdx, hIdx).transpose(); 123 | H.block<8, 8>(tIdx, hIdx).noalias() = 124 | H.block<8, 8>(hIdx, tIdx).transpose(); 125 | } 126 | } 127 | } 128 | 129 | int nframes[NUM_THREADS]; 130 | 131 | EIGEN_ALIGN16 AccumulatorApprox *acc[NUM_THREADS]; 132 | 133 | int nres[NUM_THREADS]; 134 | 135 | template 136 | void addPointsInternal(std::vector *points, 137 | EnergyFunctional const *const ef, int min = 0, 138 | int max = 1, Vec10 *stats = 0, int tid = 0) { 139 | for (int i = min; i < max; i++) 140 | addPoint((*points)[i], ef, tid); 141 | } 142 | 143 | private: 144 | void stitchDoubleInternal(MatXX *H, VecX *b, EnergyFunctional const *const EF, 145 | bool usePrior, int min, int max, Vec10 *stats, 146 | int tid); 147 | }; 148 | } // namespace dso 149 | -------------------------------------------------------------------------------- /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 | #pragma once 25 | 26 | #include "IOWrapper/Output3DWrapper.h" 27 | #include "OptimizationBackend/MatrixAccumulators.h" 28 | #include "util/NumType.h" 29 | #include "util/settings.h" 30 | 31 | #include 32 | #include 33 | 34 | namespace dso { 35 | struct CalibHessian; 36 | struct FrameHessian; 37 | 38 | struct Pnt { 39 | public: 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 41 | // index in jacobian. never changes (actually, there is no reason why). 42 | float u, v; 43 | 44 | // idepth / isgood / energy during optimization. 45 | float idepth; 46 | bool isGood; 47 | Vec2f energy; // (UenergyPhotometric, energyRegularizer) 48 | bool isGood_new; 49 | float idepth_new; 50 | Vec2f energy_new; 51 | 52 | float iR; 53 | float iRSumNum; 54 | 55 | float lastHessian; 56 | float lastHessian_new; 57 | 58 | // max stepsize for idepth (corresponding to max. movement in pixel-space). 59 | float maxstep; 60 | 61 | // idx (x+y*w) of closest point one pyramid level above. 62 | int parent; 63 | float parentDist; 64 | 65 | // idx (x+y*w) of up to 10 nearest points in pixel space. 66 | int neighbours[10]; 67 | float neighboursDist[10]; 68 | 69 | float my_type; 70 | float outlierTH; 71 | }; 72 | 73 | class CoarseInitializer { 74 | public: 75 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 76 | CoarseInitializer(int w, int h); 77 | ~CoarseInitializer(); 78 | 79 | void makeK(CalibHessian *HCalib); 80 | 81 | void makeCoarseDepth(); 82 | void setFirst(CalibHessian *HCalib, FrameHessian *newFrameHessian); 83 | bool trackFrame(FrameHessian *newFrameHessian, 84 | std::vector &wraps); 85 | void calcTGrads(FrameHessian *newFrameHessian); 86 | 87 | int frameID; 88 | bool fixAffine; 89 | bool printDebug; 90 | 91 | Pnt *points[PYR_LEVELS]; 92 | int numPoints[PYR_LEVELS]; 93 | AffLight thisToNext_aff; 94 | SE3 thisToNext; 95 | 96 | FrameHessian *firstFrame; 97 | FrameHessian *newFrame; 98 | 99 | private: 100 | bool snapped; 101 | int snappedAt; 102 | 103 | Eigen::DiagonalMatrix wM; 104 | 105 | std::vector ptr_to_delete_; 106 | 107 | float *idepth_[PYR_LEVELS]; 108 | float *weight_sums_[PYR_LEVELS]; 109 | float *weight_sums_bak_[PYR_LEVELS]; 110 | 111 | Mat33f Ki_[PYR_LEVELS]; 112 | float fx_[PYR_LEVELS]; 113 | float fy_[PYR_LEVELS]; 114 | float cx_[PYR_LEVELS]; 115 | float cy_[PYR_LEVELS]; 116 | int w_[PYR_LEVELS]; 117 | int h_[PYR_LEVELS]; 118 | 119 | // pc buffers 120 | float *pc_u_[PYR_LEVELS]; 121 | float *pc_v_[PYR_LEVELS]; 122 | float *pc_idepth_[PYR_LEVELS]; 123 | float *pc_color_[PYR_LEVELS]; 124 | int pc_n_[PYR_LEVELS]; 125 | 126 | // temporary buffers for H and b. 127 | Vec10f *JbBuffer; // 0-7: sum(dd * dp). 8: sum(res*dd). 9: 128 | // 1/(1+sum(dd*dd))=inverse hessian entry. 129 | Vec10f *JbBuffer_new; 130 | 131 | Accumulator9 acc9; 132 | Accumulator9 acc9SC; 133 | 134 | float alphaK; 135 | float alphaW; 136 | float regWeight; 137 | float couplingWeight; 138 | 139 | Vec3f calcResAndGS(int lvl, Mat88f &H_out, Vec8f &b_out, Mat88f &H_out_sc, 140 | Vec8f &b_out_sc, const SE3 &refToNew, 141 | AffLight refToNew_aff, bool plot); 142 | Vec3f calcEC(int lvl); // returns OLD NERGY, NEW ENERGY, NUM TERMS. 143 | void optReg(int lvl); 144 | 145 | void propagateUp(int srcLvl); 146 | void propagateDown(int srcLvl); 147 | float rescale(); 148 | 149 | void resetPoints(int lvl); 150 | void doStep(int lvl, float lambda, Vec8f inc); 151 | void applyStep(int lvl); 152 | 153 | void debugPlot(int lvl, std::vector &wraps); 154 | void makeNN(); 155 | }; 156 | 157 | struct FLANNPointcloud { 158 | inline FLANNPointcloud() { 159 | num = 0; 160 | points = 0; 161 | } 162 | inline FLANNPointcloud(int n, Pnt *p) : num(n), points(p) {} 163 | int num; 164 | Pnt *points; 165 | inline size_t kdtree_get_point_count() const { return num; } 166 | inline float kdtree_distance(const float *p1, const size_t idx_p2, 167 | size_t /*size*/) const { 168 | const float d0 = p1[0] - points[idx_p2].u; 169 | const float d1 = p1[1] - points[idx_p2].v; 170 | return d0 * d0 + d1 * d1; 171 | } 172 | 173 | inline float kdtree_get_pt(const size_t idx, int dim) const { 174 | if (dim == 0) 175 | return points[idx].u; 176 | else 177 | return points[idx].v; 178 | } 179 | template bool kdtree_get_bbox(BBOX & /* bb */) const { 180 | return false; 181 | } 182 | }; 183 | 184 | } // namespace dso 185 | -------------------------------------------------------------------------------- /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 | #pragma once 25 | 26 | #include "Eigen/Core" 27 | #include "ImageAndExposure.h" 28 | #include "MinimalImage.h" 29 | #include "NumType.h" 30 | 31 | namespace dso { 32 | 33 | class PhotometricUndistorter { 34 | public: 35 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 36 | PhotometricUndistorter(std::string file, std::string noiseImage, 37 | std::string vignetteImage, int w_, int h_); 38 | ~PhotometricUndistorter(); 39 | 40 | // removes readout noise, and converts to irradiance. 41 | // affine normalizes values to 0 <= I < 256. 42 | // raw irradiance = a*I + b. 43 | // output will be written in [output]. 44 | template 45 | void processFrame(T *image_in, float exposure_time, float factor = 1); 46 | void unMapFloatImage(float *image); 47 | 48 | ImageAndExposure *output; 49 | 50 | float *getG() { 51 | if (!valid) 52 | return 0; 53 | else 54 | return G; 55 | }; 56 | 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 | class Undistort { 67 | public: 68 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 69 | virtual ~Undistort(); 70 | 71 | virtual void distortCoordinates(float *in_x, float *in_y, float *out_x, 72 | float *out_y, int n) const = 0; 73 | 74 | inline const Mat33 getK() const { return K; }; 75 | inline const Eigen::Vector2i getSize() const { 76 | return Eigen::Vector2i(w, h); 77 | }; 78 | inline const VecX getOriginalParameter() const { return parsOrg; }; 79 | inline const Eigen::Vector2i getOriginalSize() { 80 | return Eigen::Vector2i(wOrg, hOrg); 81 | }; 82 | inline bool isValid() { return valid; }; 83 | 84 | template 85 | ImageAndExposure *undistort(const MinimalImage *image_raw, 86 | float exposure = 0, double timestamp = 0, 87 | float factor = 1) const; 88 | static Undistort *getUndistorterForFile(std::string configFilename, 89 | std::string gammaFilename, 90 | std::string vignetteFilename); 91 | 92 | void loadPhotometricCalibration(std::string file, std::string noiseImage, 93 | 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; 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, 114 | std::string prefix = ""); 115 | }; 116 | 117 | class UndistortFOV : public Undistort { 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, 124 | int n) const; 125 | }; 126 | 127 | class UndistortRadTan : public Undistort { 128 | public: 129 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 130 | UndistortRadTan(const char *configFileName, bool noprefix); 131 | ~UndistortRadTan(); 132 | void distortCoordinates(float *in_x, float *in_y, float *out_x, float *out_y, 133 | int n) const; 134 | }; 135 | 136 | class UndistortEquidistant : public Undistort { 137 | public: 138 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 139 | UndistortEquidistant(const char *configFileName, bool noprefix); 140 | ~UndistortEquidistant(); 141 | void distortCoordinates(float *in_x, float *in_y, float *out_x, float *out_y, 142 | int n) const; 143 | }; 144 | 145 | class UndistortPinhole : public Undistort { 146 | public: 147 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 148 | UndistortPinhole(const char *configFileName, bool noprefix); 149 | ~UndistortPinhole(); 150 | void distortCoordinates(float *in_x, float *in_y, float *out_x, float *out_y, 151 | int n) const; 152 | 153 | private: 154 | float inputCalibration[8]; 155 | }; 156 | 157 | class UndistortKB : public Undistort { 158 | public: 159 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 160 | UndistortKB(const char *configFileName, bool noprefix); 161 | ~UndistortKB(); 162 | void distortCoordinates(float *in_x, float *in_y, float *out_x, float *out_y, 163 | int n) const; 164 | }; 165 | 166 | } // namespace dso 167 | -------------------------------------------------------------------------------- /src/IOWrapper/OutputWrapper/SampleOutputWrapper.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | #include "boost/thread.hpp" 27 | #include "util/MinimalImage.h" 28 | #include "IOWrapper/Output3DWrapper.h" 29 | 30 | 31 | 32 | #include "FullSystem/HessianBlocks.h" 33 | #include "util/FrameShell.h" 34 | 35 | namespace dso 36 | { 37 | 38 | class FrameHessian; 39 | class CalibHessian; 40 | class FrameShell; 41 | 42 | 43 | namespace IOWrap 44 | { 45 | 46 | class SampleOutputWrapper : public Output3DWrapper 47 | { 48 | public: 49 | inline SampleOutputWrapper() 50 | { 51 | printf("OUT: Created SampleOutputWrapper\n"); 52 | } 53 | 54 | virtual ~SampleOutputWrapper() 55 | { 56 | printf("OUT: Destroyed SampleOutputWrapper\n"); 57 | } 58 | 59 | virtual void publishGraph(const std::map, Eigen::aligned_allocator>> &connectivity) override 60 | { 61 | printf("OUT: got graph with %d edges\n", (int)connectivity.size()); 62 | 63 | int maxWrite = 5; 64 | 65 | for(const std::pair &p : connectivity) 66 | { 67 | int idHost = p.first>>32; 68 | int idTarget = p.first & ((uint64_t)0xFFFFFFFF); 69 | printf("OUT: Example Edge %d -> %d has %d active and %d marg residuals\n", idHost, idTarget, p.second[0], p.second[1]); 70 | maxWrite--; 71 | if(maxWrite==0) break; 72 | } 73 | } 74 | 75 | 76 | 77 | virtual void publishKeyframes( std::vector &frames, bool final, CalibHessian* HCalib) override 78 | { 79 | for(FrameHessian* f : frames) 80 | { 81 | printf("OUT: KF %d (%s) (id %d, tme %f): %d active, %d marginalized, %d immature points. CameraToWorld:\n", 82 | f->frameID, 83 | final ? "final" : "non-final", 84 | f->shell->incoming_id, 85 | f->shell->timestamp, 86 | (int)f->pointHessians.size(), (int)f->pointHessiansMarginalized.size(), (int)f->immaturePoints.size()); 87 | std::cout << f->shell->camToWorld.matrix3x4() << "\n"; 88 | 89 | 90 | int maxWrite = 5; 91 | for(PointHessian* p : f->pointHessians) 92 | { 93 | printf("OUT: Example Point x=%.1f, y=%.1f, idepth=%f, idepth std.dev. %f, %d inlier-residuals\n", 94 | p->u, p->v, p->idepth_scaled, sqrt(1.0f / p->idepth_hessian), p->numGoodResiduals ); 95 | maxWrite--; 96 | if(maxWrite==0) break; 97 | } 98 | } 99 | } 100 | 101 | virtual void publishCamPose(FrameShell* frame, CalibHessian* HCalib) override 102 | { 103 | printf("OUT: Current Frame %d (time %f, internal ID %d). CameraToWorld:\n", 104 | frame->incoming_id, 105 | frame->timestamp, 106 | frame->id); 107 | std::cout << frame->camToWorld.matrix3x4() << "\n"; 108 | } 109 | 110 | 111 | virtual void pushLiveFrame(FrameHessian* image) override 112 | { 113 | // can be used to get the raw image / intensity pyramid. 114 | } 115 | 116 | virtual void pushDepthImage(MinimalImageB3* image) override 117 | { 118 | // can be used to get the raw image with depth overlay. 119 | } 120 | virtual bool needPushDepthImage() override 121 | { 122 | return false; 123 | } 124 | 125 | virtual void pushDepthImageFloat(MinimalImageF* image, FrameHessian* KF ) override 126 | { 127 | printf("OUT: Predicted depth for KF %d (id %d, time %f, internal frame-ID %d). CameraToWorld:\n", 128 | KF->frameID, 129 | KF->shell->incoming_id, 130 | KF->shell->timestamp, 131 | KF->shell->id); 132 | std::cout << KF->shell->camToWorld.matrix3x4() << "\n"; 133 | 134 | int maxWrite = 5; 135 | for(int y=0;yh;y++) 136 | { 137 | for(int x=0;xw;x++) 138 | { 139 | if(image->at(x,y) <= 0) continue; 140 | 141 | printf("OUT: Example Idepth at pixel (%d,%d): %f.\n", x,y,image->at(x,y)); 142 | maxWrite--; 143 | if(maxWrite==0) break; 144 | } 145 | if(maxWrite==0) break; 146 | } 147 | } 148 | 149 | 150 | }; 151 | 152 | 153 | 154 | } 155 | 156 | 157 | 158 | } 159 | -------------------------------------------------------------------------------- /src/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 | #pragma once 25 | #include "boost/thread.hpp" 26 | #include "settings.h" 27 | #include 28 | #include 29 | 30 | namespace dso { 31 | 32 | template class IndexThreadReduce { 33 | 34 | public: 35 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 36 | 37 | inline IndexThreadReduce() { 38 | nextIndex = 0; 39 | maxIndex = 0; 40 | stepSize = 1; 41 | callPerIndex = boost::bind(&IndexThreadReduce::callPerIndexDefault, this, 42 | _1, _2, _3, _4); 43 | 44 | running = true; 45 | for (int i = 0; i < NUM_THREADS; i++) { 46 | isDone[i] = false; 47 | gotOne[i] = true; 48 | workerThreads[i] = boost::thread(&IndexThreadReduce::workerLoop, this, i); 49 | } 50 | } 51 | inline ~IndexThreadReduce() { 52 | running = false; 53 | 54 | exMutex.lock(); 55 | todo_signal.notify_all(); 56 | exMutex.unlock(); 57 | 58 | for (int i = 0; i < NUM_THREADS; i++) 59 | workerThreads[i].join(); 60 | 61 | printf("destroyed ThreadReduce\n"); 62 | } 63 | 64 | inline void 65 | reduce(boost::function callPerIndex, 66 | int first, int end, int stepSize = 0) { 67 | 68 | memset(&stats, 0, sizeof(Running)); 69 | 70 | // if(!multiThreading) 71 | // { 72 | // callPerIndex(first, end, &stats, 0); 73 | // return; 74 | // } 75 | 76 | if (stepSize == 0) 77 | stepSize = ((end - first) + NUM_THREADS - 1) / NUM_THREADS; 78 | 79 | // printf("reduce called\n"); 80 | 81 | boost::unique_lock lock(exMutex); 82 | 83 | // save 84 | this->callPerIndex = callPerIndex; 85 | nextIndex = first; 86 | maxIndex = end; 87 | this->stepSize = stepSize; 88 | 89 | // go worker threads! 90 | for (int i = 0; i < NUM_THREADS; i++) { 91 | isDone[i] = false; 92 | gotOne[i] = false; 93 | } 94 | 95 | // let them start! 96 | todo_signal.notify_all(); 97 | 98 | // printf("reduce waiting for threads to finish\n"); 99 | // wait for all worker threads to signal they are done. 100 | while (true) { 101 | // wait for at least one to finish 102 | done_signal.wait(lock); 103 | // printf("thread finished!\n"); 104 | 105 | // check if actually all are finished. 106 | bool allDone = true; 107 | for (int i = 0; i < NUM_THREADS; i++) 108 | allDone = allDone && isDone[i]; 109 | 110 | // all are finished! exit. 111 | if (allDone) 112 | break; 113 | } 114 | 115 | nextIndex = 0; 116 | maxIndex = 0; 117 | this->callPerIndex = boost::bind(&IndexThreadReduce::callPerIndexDefault, 118 | this, _1, _2, _3, _4); 119 | 120 | // printf("reduce done (all threads finished)\n"); 121 | } 122 | 123 | Running stats; 124 | 125 | private: 126 | boost::thread workerThreads[NUM_THREADS]; 127 | bool isDone[NUM_THREADS]; 128 | bool gotOne[NUM_THREADS]; 129 | 130 | boost::mutex exMutex; 131 | boost::condition_variable todo_signal; 132 | boost::condition_variable done_signal; 133 | 134 | int nextIndex; 135 | int maxIndex; 136 | int stepSize; 137 | 138 | bool running; 139 | 140 | boost::function callPerIndex; 141 | 142 | void callPerIndexDefault(int i, int j, Running *k, int tid) { 143 | printf("ERROR: should never be called....\n"); 144 | assert(false); 145 | } 146 | 147 | void workerLoop(int idx) { 148 | boost::unique_lock lock(exMutex); 149 | 150 | while (running) { 151 | // try to get something to do. 152 | int todo = 0; 153 | bool gotSomething = false; 154 | if (nextIndex < maxIndex) { 155 | // got something! 156 | todo = nextIndex; 157 | nextIndex += stepSize; 158 | gotSomething = true; 159 | } 160 | 161 | // if got something: do it (unlock in the meantime) 162 | if (gotSomething) { 163 | lock.unlock(); 164 | 165 | assert(callPerIndex != 0); 166 | 167 | Running s; 168 | memset(&s, 0, sizeof(Running)); 169 | callPerIndex(todo, std::min(todo + stepSize, maxIndex), &s, idx); 170 | gotOne[idx] = true; 171 | lock.lock(); 172 | stats += s; 173 | } 174 | 175 | // otherwise wait on signal, releasing lock in the meantime. 176 | else { 177 | if (!gotOne[idx]) { 178 | lock.unlock(); 179 | assert(callPerIndex != 0); 180 | Running s; 181 | memset(&s, 0, sizeof(Running)); 182 | callPerIndex(0, 0, &s, idx); 183 | gotOne[idx] = true; 184 | lock.lock(); 185 | stats += s; 186 | } 187 | isDone[idx] = true; 188 | // printf("worker %d waiting..\n", idx); 189 | done_signal.notify_all(); 190 | todo_signal.wait(lock); 191 | } 192 | } 193 | } 194 | }; 195 | } // namespace dso 196 | -------------------------------------------------------------------------------- /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 | #pragma once 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include "NumType.h" 31 | 32 | namespace dso { 33 | // ============== PARAMETERS TO BE DECIDED ON COMPILE TIME ================= 34 | #define PYR_LEVELS 6 35 | extern int pyrLevelsUsed; 36 | 37 | extern float setting_keyframesPerSecond; 38 | extern bool setting_realTimeMaxKF; 39 | extern float setting_maxShiftWeightT; 40 | extern float setting_maxShiftWeightR; 41 | extern float setting_maxShiftWeightRT; 42 | extern float setting_maxAffineWeight; 43 | extern float setting_kfGlobalWeight; 44 | 45 | extern float setting_idepthFixPrior; 46 | extern float setting_idepthFixPriorMargFac; 47 | extern float setting_initialRotPrior; 48 | extern float setting_initialTransPrior; 49 | extern float setting_initialAffBPrior; 50 | extern float setting_initialAffAPrior; 51 | extern float setting_initialCalibHessian; 52 | 53 | extern double setting_solverModeDelta; 54 | 55 | extern float setting_minIdepthH_act; 56 | extern float setting_minIdepthH_marg; 57 | 58 | extern float setting_maxIdepth; 59 | extern float setting_maxPixSearch; 60 | extern float setting_desiredImmatureDensity; // done 61 | extern float setting_desiredPointDensity; // done 62 | extern float setting_minPointsRemaining; 63 | extern float setting_maxLogAffFacInWindow; 64 | extern int setting_minFrames; 65 | extern int setting_maxFrames; 66 | extern int setting_minFrameAge; 67 | extern int setting_maxOptIterations; 68 | extern int setting_minOptIterations; 69 | extern float setting_thOptIterations; 70 | extern float setting_outlierTH; 71 | extern float setting_outlierTHSumComponent; 72 | 73 | extern int setting_pattern; 74 | extern float setting_margWeightFac; 75 | extern int setting_GNItsOnPointActivation; 76 | 77 | extern float setting_minTraceQuality; 78 | extern int setting_minTraceTestRadius; 79 | extern float setting_reTrackThreshold; 80 | 81 | extern int setting_minGoodActiveResForMarg; 82 | extern int setting_minGoodResForMarg; 83 | extern int setting_minInlierVotesForMarg; 84 | 85 | extern int setting_photometricCalibration; 86 | extern bool setting_useExposure; 87 | extern float setting_affineOptModeA; 88 | extern float setting_affineOptModeB; 89 | extern int setting_gammaWeightsPixelSelect; 90 | 91 | extern bool setting_forceAceptStep; 92 | 93 | extern float setting_huberTH; 94 | 95 | extern bool setting_logStuff; 96 | extern float benchmarkSetting_fxfyfac; 97 | extern int benchmarkSetting_width; 98 | extern int benchmarkSetting_height; 99 | extern float benchmark_varNoise; 100 | extern float benchmark_varBlurNoise; 101 | extern int benchmark_noiseGridsize; 102 | extern float benchmark_initializerSlackFactor; 103 | 104 | extern float setting_frameEnergyTHConstWeight; 105 | extern float setting_frameEnergyTHN; 106 | 107 | extern float setting_frameEnergyTHFacMedian; 108 | extern float setting_overallEnergyTHWeight; 109 | extern float setting_coarseCutoffTH; 110 | 111 | extern float setting_minGradHistCut; 112 | extern float setting_minGradHistAdd; 113 | extern float setting_gradDownweightPerLevel; 114 | extern bool setting_selectDirectionDistribution; 115 | 116 | extern float setting_trace_stepsize; 117 | extern int setting_trace_GNIterations; 118 | extern float setting_trace_GNThreshold; 119 | extern float setting_trace_extraSlackOnTH; 120 | extern float setting_trace_slackInterval; 121 | extern float setting_trace_minImprovementFactor; 122 | 123 | extern bool setting_render_displayCoarseTrackingFull; 124 | extern bool setting_render_renderWindowFrames; 125 | extern bool setting_render_plotTrackingFull; 126 | extern bool setting_render_display3D; 127 | extern bool setting_render_displayResidual; 128 | extern bool setting_render_displayVideo; 129 | extern bool setting_render_displayDepth; 130 | 131 | extern bool setting_fullResetRequested; 132 | 133 | extern bool setting_debugout_runquiet; 134 | 135 | extern bool disableAllDisplay; 136 | extern bool disableReconfigure; 137 | 138 | extern bool setting_onlyLogKFPoses; 139 | 140 | extern bool debugSaveImages; 141 | 142 | extern int sparsityFactor; 143 | extern bool goStepByStep; 144 | extern bool plotStereoImages; 145 | extern bool multiThreading; 146 | 147 | extern float freeDebugParam1; 148 | extern float freeDebugParam2; 149 | extern float freeDebugParam3; 150 | extern float freeDebugParam4; 151 | extern float freeDebugParam5; 152 | 153 | extern bool setting_enable_imu; 154 | extern bool setting_print_imu; 155 | 156 | extern int setting_min_g_imu; 157 | extern float setting_maxImuInterval; 158 | extern double setting_g_norm; 159 | extern double setting_scale_trap_thres; 160 | 161 | extern Mat33 setting_rot_imu_cam; 162 | extern double setting_weight_imu_dso; 163 | extern Mat66 setting_weight_imu; 164 | extern Mat66 setting_weight_imu_bias; 165 | 166 | void handleKey(char k); 167 | 168 | extern int staticPattern[10][40][2]; 169 | extern int staticPatternNum[10]; 170 | extern int staticPatternPadding[10]; 171 | 172 | //#define patternNum staticPatternNum[setting_pattern] 173 | //#define patternP staticPattern[setting_pattern] 174 | //#define patternPadding staticPatternPadding[setting_pattern] 175 | 176 | // 177 | #define patternNum 8 178 | #define patternP staticPattern[8] 179 | #define patternPadding 2 180 | 181 | } // namespace dso 182 | -------------------------------------------------------------------------------- /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/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 | #pragma once 25 | 26 | #include "Eigen/Core" 27 | #include "sophus/se3.hpp" 28 | #include "sophus/sim3.hpp" 29 | 30 | namespace dso { 31 | 32 | // CAMERA MODEL TO USE 33 | 34 | #define SSEE(val, idx) (*(((float *)&val) + idx)) 35 | 36 | #define MAX_RES_PER_POINT 8 37 | #define NUM_THREADS 6 38 | 39 | #define todouble(x) (x).cast() 40 | 41 | typedef Sophus::SE3d SE3; 42 | typedef Sophus::Sim3d Sim3; 43 | typedef Sophus::SO3d SO3; 44 | 45 | #define CPARS 4 46 | 47 | typedef Eigen::Matrix MatXX; 48 | typedef Eigen::Matrix MatCC; 49 | #define MatToDynamic(x) MatXX(x) 50 | 51 | typedef Eigen::Matrix MatC10; 52 | typedef Eigen::Matrix Mat1010; 53 | typedef Eigen::Matrix Mat1313; 54 | 55 | typedef Eigen::Matrix Mat810; 56 | typedef Eigen::Matrix Mat83; 57 | typedef Eigen::Matrix Mat66; 58 | typedef Eigen::Matrix Mat53; 59 | typedef Eigen::Matrix Mat43; 60 | typedef Eigen::Matrix Mat42; 61 | typedef Eigen::Matrix Mat33; 62 | typedef Eigen::Matrix Mat22; 63 | typedef Eigen::Matrix Mat8C; 64 | typedef Eigen::Matrix MatC8; 65 | typedef Eigen::Matrix Mat8Cf; 66 | typedef Eigen::Matrix MatC8f; 67 | 68 | typedef Eigen::Matrix Mat88; 69 | typedef Eigen::Matrix Mat77; 70 | 71 | typedef Eigen::Matrix VecC; 72 | typedef Eigen::Matrix VecCf; 73 | typedef Eigen::Matrix Vec21; 74 | typedef Eigen::Matrix Vec13; 75 | typedef Eigen::Matrix Vec10; 76 | typedef Eigen::Matrix Vec9; 77 | typedef Eigen::Matrix Vec8; 78 | typedef Eigen::Matrix Vec7; 79 | typedef Eigen::Matrix Vec6; 80 | typedef Eigen::Matrix Vec5; 81 | typedef Eigen::Matrix Vec4; 82 | typedef Eigen::Matrix Vec3; 83 | typedef Eigen::Matrix Vec2; 84 | typedef Eigen::Matrix VecX; 85 | 86 | typedef Eigen::Matrix Mat33f; 87 | typedef Eigen::Matrix Mat103f; 88 | typedef Eigen::Matrix Mat22f; 89 | typedef Eigen::Matrix Vec3f; 90 | typedef Eigen::Matrix Vec2f; 91 | typedef Eigen::Matrix Vec6f; 92 | 93 | typedef Eigen::Matrix Mat49; 94 | typedef Eigen::Matrix Mat89; 95 | 96 | typedef Eigen::Matrix Mat94; 97 | typedef Eigen::Matrix Mat98; 98 | 99 | typedef Eigen::Matrix Mat81; 100 | typedef Eigen::Matrix Mat18; 101 | typedef Eigen::Matrix Mat91; 102 | typedef Eigen::Matrix Mat19; 103 | 104 | typedef Eigen::Matrix Mat84; 105 | typedef Eigen::Matrix Mat48; 106 | typedef Eigen::Matrix Mat44; 107 | 108 | typedef Eigen::Matrix VecNRf; 109 | typedef Eigen::Matrix Vec12f; 110 | typedef Eigen::Matrix Mat18f; 111 | typedef Eigen::Matrix Mat66f; 112 | typedef Eigen::Matrix Mat88f; 113 | typedef Eigen::Matrix Mat84f; 114 | typedef Eigen::Matrix Vec8f; 115 | typedef Eigen::Matrix Vec10f; 116 | typedef Eigen::Matrix Mat66f; 117 | typedef Eigen::Matrix Vec4f; 118 | typedef Eigen::Matrix Mat44f; 119 | typedef Eigen::Matrix Mat1212f; 120 | typedef Eigen::Matrix Vec12f; 121 | typedef Eigen::Matrix Mat1313f; 122 | typedef Eigen::Matrix Mat1010f; 123 | typedef Eigen::Matrix Vec13f; 124 | typedef Eigen::Matrix Mat99f; 125 | typedef Eigen::Matrix Vec9f; 126 | 127 | typedef Eigen::Matrix Mat42f; 128 | typedef Eigen::Matrix Mat62f; 129 | typedef Eigen::Matrix Mat12f; 130 | 131 | typedef Eigen::Matrix Mat99; 132 | typedef Eigen::Matrix Mat1515; 133 | typedef Eigen::Matrix Vec15; 134 | 135 | typedef Eigen::Matrix VecXf; 136 | typedef Eigen::Matrix MatXXf; 137 | 138 | typedef Eigen::Matrix MatPCPC; 139 | typedef Eigen::Matrix MatPCPCf; 140 | typedef Eigen::Matrix VecPC; 141 | typedef Eigen::Matrix VecPCf; 142 | 143 | typedef Eigen::Matrix Mat1414f; 144 | typedef Eigen::Matrix Vec14f; 145 | typedef Eigen::Matrix Mat1414; 146 | typedef Eigen::Matrix Vec14; 147 | 148 | // transforms points from one frame to another. 149 | struct AffLight { 150 | AffLight(double a_, double b_) : a(a_), b(b_){}; 151 | AffLight() : a(0), b(0){}; 152 | 153 | // Affine Parameters: 154 | double a, 155 | b; // I_frame = exp(a)*I_global + b. // I_global = exp(-a)*(I_frame - b). 156 | 157 | static Vec2 fromToVecExposure(float exposureF, float exposureT, AffLight g2F, 158 | AffLight g2T) { 159 | if (exposureF == 0 || exposureT == 0) { 160 | exposureT = exposureF = 1; 161 | // printf("got exposure value of 0! please choose the correct model.\n"); 162 | // assert(setting_brightnessTransferFunc < 2); 163 | } 164 | 165 | double a = exp(g2T.a - g2F.a) * exposureT / exposureF; 166 | double b = g2T.b - a * g2F.b; 167 | return Vec2(a, b); 168 | } 169 | 170 | Vec2 vec() { return Vec2(a, b); } 171 | }; 172 | 173 | } // namespace dso 174 | -------------------------------------------------------------------------------- /src/FullSystem/FullSystemOptPoint.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | /* 25 | * KFBuffer.cpp 26 | * 27 | * Created on: Jan 7, 2014 28 | * Author: engelj 29 | */ 30 | 31 | #include "FullSystem.h" 32 | 33 | #include "IOWrapper/ImageDisplay.h" 34 | #include "stdio.h" 35 | #include "util/globalCalib.h" 36 | #include "util/globalFuncs.h" 37 | #include 38 | #include 39 | 40 | #include "ImmaturePoint.h" 41 | #include "math.h" 42 | #include 43 | #include 44 | 45 | namespace dso { 46 | 47 | PointHessian * 48 | FullSystem::optimizeImmaturePoint(ImmaturePoint *point, int minObs, 49 | ImmaturePointTemporaryResidual *residuals) { 50 | int nres = 0; 51 | for (FrameHessian *fh : frameHessians) { 52 | if (fh != point->host) { 53 | residuals[nres].state_NewEnergy = residuals[nres].state_energy = 0; 54 | residuals[nres].state_NewState = ResState::OUTLIER; 55 | residuals[nres].state_state = ResState::IN; 56 | residuals[nres].target = fh; 57 | nres++; 58 | } 59 | } 60 | assert(nres == ((int)frameHessians.size()) - 1); 61 | 62 | bool print = false; // rand()%50==0; 63 | 64 | float lastEnergy = 0; 65 | float lastHdd = 0; 66 | float lastbd = 0; 67 | float currentIdepth = (point->idepth_max + point->idepth_min) * 0.5f; 68 | 69 | for (int i = 0; i < nres; i++) { 70 | lastEnergy += point->linearizeResidual(&HCalib, 1000, residuals + i, 71 | lastHdd, lastbd, currentIdepth); 72 | residuals[i].state_state = residuals[i].state_NewState; 73 | residuals[i].state_energy = residuals[i].state_NewEnergy; 74 | } 75 | 76 | if (!std::isfinite(lastEnergy) || lastHdd < setting_minIdepthH_act) { 77 | if (print) 78 | printf("OptPoint: Not well-constrained (%d res, H=%.1f). E=%f. SKIP!\n", 79 | nres, lastHdd, lastEnergy); 80 | return 0; 81 | } 82 | 83 | if (print) 84 | printf("Activate point. %d residuals. H=%f. Initial Energy: %f. Initial " 85 | "Id=%f\n", 86 | nres, lastHdd, lastEnergy, currentIdepth); 87 | 88 | float lambda = 0.1; 89 | for (int iteration = 0; iteration < setting_GNItsOnPointActivation; 90 | iteration++) { 91 | float H = lastHdd; 92 | H *= 1 + lambda; 93 | float step = (1.0 / H) * lastbd; 94 | float newIdepth = currentIdepth - step; 95 | 96 | float newHdd = 0; 97 | float newbd = 0; 98 | float newEnergy = 0; 99 | for (int i = 0; i < nres; i++) 100 | newEnergy += point->linearizeResidual(&HCalib, 1, residuals + i, newHdd, 101 | newbd, newIdepth); 102 | 103 | if (!std::isfinite(lastEnergy) || newHdd < setting_minIdepthH_act) { 104 | if (print) 105 | printf("OptPoint: Not well-constrained (%d res, H=%.1f). E=%f. SKIP!\n", 106 | nres, newHdd, lastEnergy); 107 | return 0; 108 | } 109 | 110 | if (print) 111 | printf("%s %d (L %.2f) %s: %f -> %f (idepth %f)!\n", 112 | (true || newEnergy < lastEnergy) ? "ACCEPT" : "REJECT", iteration, 113 | log10(lambda), "", lastEnergy, newEnergy, newIdepth); 114 | 115 | if (newEnergy < lastEnergy) { 116 | currentIdepth = newIdepth; 117 | lastHdd = newHdd; 118 | lastbd = newbd; 119 | lastEnergy = newEnergy; 120 | for (int i = 0; i < nres; i++) { 121 | residuals[i].state_state = residuals[i].state_NewState; 122 | residuals[i].state_energy = residuals[i].state_NewEnergy; 123 | } 124 | 125 | lambda *= 0.5; 126 | } else { 127 | lambda *= 5; 128 | } 129 | 130 | if (fabsf(step) < 0.0001 * currentIdepth) 131 | break; 132 | } 133 | 134 | if (!std::isfinite(currentIdepth)) { 135 | printf("MAJOR ERROR! point idepth is nan after initialization (%f).\n", 136 | currentIdepth); 137 | return (PointHessian *)(( 138 | long)(-1)); // yeah I'm like 99% sure this is OK on 32bit systems. 139 | } 140 | 141 | int numGoodRes = 0; 142 | for (int i = 0; i < nres; i++) 143 | if (residuals[i].state_state == ResState::IN) 144 | numGoodRes++; 145 | 146 | if (numGoodRes < minObs) { 147 | if (print) 148 | printf("OptPoint: OUTLIER!\n"); 149 | return (PointHessian *)(( 150 | long)(-1)); // yeah I'm like 99% sure this is OK on 32bit systems. 151 | } 152 | 153 | PointHessian *p = new PointHessian(point, &HCalib); 154 | if (!std::isfinite(p->energyTH)) { 155 | delete p; 156 | return (PointHessian *)((long)(-1)); 157 | } 158 | 159 | p->lastResiduals[0].first = 0; 160 | p->lastResiduals[0].second = ResState::OOB; 161 | p->lastResiduals[1].first = 0; 162 | p->lastResiduals[1].second = ResState::OOB; 163 | p->setIdepthZero(currentIdepth); 164 | p->setIdepth(currentIdepth); 165 | p->setPointStatus(PointHessian::ACTIVE); 166 | 167 | for (int i = 0; i < nres; i++) 168 | if (residuals[i].state_state == ResState::IN) { 169 | PointFrameResidual *r = 170 | new PointFrameResidual(p, p->host, residuals[i].target); 171 | r->state_NewEnergy = r->state_energy = 0; 172 | r->state_NewState = ResState::OUTLIER; 173 | r->setState(ResState::IN); 174 | p->residuals.push_back(r); 175 | 176 | if (r->target == frameHessians.back()) { 177 | p->lastResiduals[0].first = r; 178 | p->lastResiduals[0].second = ResState::IN; 179 | } else if (r->target == (frameHessians.size() < 2 180 | ? 0 181 | : frameHessians[frameHessians.size() - 2])) { 182 | p->lastResiduals[1].first = r; 183 | p->lastResiduals[1].second = ResState::IN; 184 | } 185 | } 186 | 187 | if (print) 188 | printf("point activated!\n"); 189 | 190 | statistics_numActivatedPoints++; 191 | return p; 192 | } 193 | 194 | } // namespace dso 195 | -------------------------------------------------------------------------------- /src/FullSystem/FullSystemMarginalize.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | /* 25 | * KFBuffer.cpp 26 | * 27 | * Created on: Jan 7, 2014 28 | * Author: engelj 29 | */ 30 | 31 | #include "FullSystem.h" 32 | 33 | #include "IOWrapper/ImageDisplay.h" 34 | #include "stdio.h" 35 | #include "util/globalCalib.h" 36 | #include "util/globalFuncs.h" 37 | #include 38 | #include 39 | 40 | #include "ImmaturePoint.h" 41 | #include "ResidualProjections.h" 42 | #include 43 | #include 44 | 45 | #include "OptimizationBackend/EnergyFunctional.h" 46 | #include "OptimizationBackend/EnergyFunctionalStructs.h" 47 | 48 | #include "IOWrapper/Output3DWrapper.h" 49 | 50 | #include "CoarseTracker.h" 51 | 52 | namespace dso { 53 | 54 | void FullSystem::flagFramesForMarginalization(FrameHessian *newFH) { 55 | if (setting_minFrameAge > setting_maxFrames) { 56 | for (int i = setting_maxFrames; i < (int)frameHessians.size(); i++) { 57 | FrameHessian *fh = frameHessians[i - setting_maxFrames]; 58 | fh->flaggedForMarginalization = true; 59 | } 60 | return; 61 | } 62 | 63 | int flagged = 0; 64 | // marginalize all frames that have not enough points. 65 | for (int i = 0; i < (int)frameHessians.size(); i++) { 66 | FrameHessian *fh = frameHessians[i]; 67 | int in = fh->pointHessians.size() + fh->immaturePoints.size(); 68 | int out = 69 | fh->pointHessiansMarginalized.size() + fh->pointHessiansOut.size(); 70 | 71 | Vec2 refToFh = AffLight::fromToVecExposure( 72 | frameHessians.back()->ab_exposure, fh->ab_exposure, 73 | frameHessians.back()->aff_g2l(), fh->aff_g2l()); 74 | 75 | if ((in < setting_minPointsRemaining * (in + out) || 76 | fabs(logf((float)refToFh[0])) > setting_maxLogAffFacInWindow) && 77 | ((int)frameHessians.size()) - flagged > setting_minFrames) { 78 | // printf("MARGINALIZE frame %d, as only %'d/%'d 79 | // points remaining (%'d %'d %'d %'d). VisInLast %'d / %'d. traces %d, 80 | // activated %d!\n", fh->frameID, in, 81 | // in+out, 82 | // (int)fh->pointHessians.size(), (int)fh->immaturePoints.size(), 83 | // (int)fh->pointHessiansMarginalized.size(), 84 | //(int)fh->pointHessiansOut.size(), 85 | // visInLast, outInLast, 86 | // fh->statistics_tracesCreatedForThisFrame, 87 | // fh->statistics_pointsActivatedForThisFrame); 88 | fh->flaggedForMarginalization = true; 89 | flagged++; 90 | } else { 91 | // printf("May Keep frame %d, as %'d/%'d points 92 | // remaining (%'d %'d %'d %'d). VisInLast %'d / %'d. traces %d, activated 93 | //%d!\n", fh->frameID, in, in+out, 94 | //(int)fh->pointHessians.size(), (int)fh->immaturePoints.size(), 95 | // (int)fh->pointHessiansMarginalized.size(), 96 | //(int)fh->pointHessiansOut.size(), 97 | // visInLast, outInLast, 98 | // fh->statistics_tracesCreatedForThisFrame, 99 | // fh->statistics_pointsActivatedForThisFrame); 100 | } 101 | } 102 | 103 | // marginalize one. 104 | if ((int)frameHessians.size() - flagged >= setting_maxFrames) { 105 | double smallestScore = 1; 106 | FrameHessian *toMarginalize = 0; 107 | FrameHessian *latest = frameHessians.back(); 108 | 109 | for (FrameHessian *fh : frameHessians) { 110 | if (fh->frameID > latest->frameID - setting_minFrameAge || 111 | fh->frameID == 0) 112 | continue; 113 | // if(fh==frameHessians.front() == 0) continue; 114 | 115 | double distScore = 0; 116 | for (FrameFramePrecalc &ffh : fh->targetPrecalc) { 117 | if (ffh.target->frameID > latest->frameID - setting_minFrameAge + 1 || 118 | ffh.target == ffh.host) 119 | continue; 120 | distScore += 1 / (1e-5 + ffh.distanceLL); 121 | } 122 | distScore *= -sqrtf(fh->targetPrecalc.back().distanceLL); 123 | 124 | if (distScore < smallestScore) { 125 | smallestScore = distScore; 126 | toMarginalize = fh; 127 | } 128 | } 129 | 130 | // printf("MARGINALIZE frame %d, as it is the closest (score 131 | //%.2f)!\n", toMarginalize->frameID, 132 | // smallestScore); 133 | toMarginalize->flaggedForMarginalization = true; 134 | flagged++; 135 | } 136 | 137 | // printf("FRAMES LEFT: "); 138 | // for(FrameHessian* fh : frameHessians) 139 | // printf("%d ", fh->frameID); 140 | // printf("\n"); 141 | } 142 | 143 | void FullSystem::marginalizeFrame(FrameHessian *frame) { 144 | // marginalize or remove all this frames points. 145 | 146 | assert((int)frame->pointHessians.size() == 0); 147 | 148 | ef->marginalizeFrame(frame->efFrame, &HCalib); 149 | 150 | // drop all observations of existing points in that frame. 151 | 152 | for (FrameHessian *fh : frameHessians) { 153 | if (fh == frame) 154 | continue; 155 | 156 | for (PointHessian *ph : fh->pointHessians) { 157 | for (unsigned int i = 0; i < ph->residuals.size(); i++) { 158 | PointFrameResidual *r = ph->residuals[i]; 159 | if (r->target == frame) { 160 | if (ph->lastResiduals[0].first == r) 161 | ph->lastResiduals[0].first = 0; 162 | else if (ph->lastResiduals[1].first == r) 163 | ph->lastResiduals[1].first = 0; 164 | 165 | if (r->host->frameID < r->target->frameID) 166 | statistics_numForceDroppedResFwd++; 167 | else 168 | statistics_numForceDroppedResBwd++; 169 | 170 | ef->dropResidual(r->efResidual); 171 | deleteOut(ph->residuals, i); 172 | break; 173 | } 174 | } 175 | } 176 | } 177 | 178 | { 179 | std::vector v; 180 | v.push_back(frame); 181 | for (IOWrap::Output3DWrapper *ow : outputWrapper) 182 | ow->publishKeyframes(v, true, &HCalib); 183 | } 184 | 185 | frame->shell->marginalizedAt = frameHessians.back()->shell->id; 186 | frame->shell->movedByOpt = frame->c2w_leftEps().norm(); 187 | 188 | assert(frameHessians[frame->idx] == frame); 189 | frameHessians[frame->idx + 1]->imu_data.insert( 190 | frameHessians[frame->idx + 1]->imu_data.begin(), frame->imu_data.begin(), 191 | frame->imu_data.end()); 192 | 193 | deleteOutOrder(frameHessians, frame); 194 | for (unsigned int i = 0; i < frameHessians.size(); i++) 195 | frameHessians[i]->idx = i; 196 | 197 | setPrecalcValues(); 198 | ef->setAdjointsF(&HCalib); 199 | } 200 | 201 | } // namespace dso 202 | -------------------------------------------------------------------------------- /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 | #include "AccumulatedSCHessian.h" 25 | #include "EnergyFunctional.h" 26 | #include "EnergyFunctionalStructs.h" 27 | 28 | #include "FullSystem/HessianBlocks.h" 29 | 30 | namespace dso { 31 | 32 | void AccumulatedSCHessianSSE::addPoint(EFPoint *p, bool shiftPriorToZero, 33 | int tid) { 34 | int ngoodres = 0; 35 | for (EFResidual *r : p->residualsAll) 36 | if (r->isActive()) 37 | ngoodres++; 38 | if (ngoodres == 0) { 39 | p->HdiF = 0; 40 | p->bdSumF = 0; 41 | p->data->idepth_hessian = 0; 42 | p->data->maxRelBaseline = 0; 43 | return; 44 | } 45 | 46 | float H = p->Hdd_accAF + p->Hdd_accLF + p->priorF; 47 | if (H < 1e-10) 48 | H = 1e-10; 49 | 50 | p->data->idepth_hessian = H; 51 | 52 | p->HdiF = 1.0 / H; 53 | p->bdSumF = p->bd_accAF + p->bd_accLF; 54 | if (shiftPriorToZero) 55 | p->bdSumF += p->priorF * p->deltaF; 56 | VecCf Hcd = p->Hcd_accAF + p->Hcd_accLF; 57 | accHcc[tid].update(Hcd, Hcd, p->HdiF); 58 | accbc[tid].update(Hcd, p->bdSumF * p->HdiF); 59 | 60 | assert(std::isfinite((float)(p->HdiF))); 61 | 62 | int nFrames2 = nframes[tid] * nframes[tid]; 63 | for (EFResidual *r1 : p->residualsAll) { 64 | if (!r1->isActive()) 65 | continue; 66 | int r1ht = r1->hostIDX + r1->targetIDX * nframes[tid]; 67 | 68 | for (EFResidual *r2 : p->residualsAll) { 69 | if (!r2->isActive()) 70 | continue; 71 | 72 | accD[tid][r1ht + r2->targetIDX * nFrames2].update(r1->JpJdF, r2->JpJdF, 73 | p->HdiF); 74 | } 75 | 76 | accE[tid][r1ht].update(r1->JpJdF, Hcd, p->HdiF); 77 | accEB[tid][r1ht].update(r1->JpJdF, p->HdiF * p->bdSumF); 78 | } 79 | } 80 | void AccumulatedSCHessianSSE::stitchDoubleInternal( 81 | MatXX *H, VecX *b, EnergyFunctional const *const EF, int min, int max, 82 | Vec10 *stats, int tid) { 83 | int toAggregate = NUM_THREADS; 84 | if (tid == -1) { 85 | toAggregate = 1; 86 | tid = 0; 87 | } // special case: if we dont do multithreading, dont aggregate. 88 | if (min == max) 89 | return; 90 | 91 | int nf = nframes[0]; 92 | int nframes2 = nf * nf; 93 | 94 | for (int k = min; k < max; k++) { 95 | int i = k % nf; 96 | int j = k / nf; 97 | 98 | int iIdx = CPARS + i * 8; 99 | int jIdx = CPARS + j * 8; 100 | int ijIdx = i + nf * j; 101 | 102 | Mat8C Hpc = Mat8C::Zero(); 103 | Vec8 bp = Vec8::Zero(); 104 | 105 | for (int tid2 = 0; tid2 < toAggregate; tid2++) { 106 | accE[tid2][ijIdx].finish(); 107 | accEB[tid2][ijIdx].finish(); 108 | Hpc += accE[tid2][ijIdx].A1m.cast(); 109 | bp += accEB[tid2][ijIdx].A1m.cast(); 110 | } 111 | 112 | H[tid].block<8, CPARS>(iIdx, 0) += EF->adHost[ijIdx] * Hpc; 113 | H[tid].block<8, CPARS>(jIdx, 0) += EF->adTarget[ijIdx] * Hpc; 114 | b[tid].segment<8>(iIdx) += EF->adHost[ijIdx] * bp; 115 | b[tid].segment<8>(jIdx) += EF->adTarget[ijIdx] * bp; 116 | 117 | for (int k = 0; k < nf; k++) { 118 | int kIdx = CPARS + k * 8; 119 | int ijkIdx = ijIdx + k * nframes2; 120 | int ikIdx = i + nf * k; 121 | 122 | Mat88 accDM = Mat88::Zero(); 123 | 124 | for (int tid2 = 0; tid2 < toAggregate; tid2++) { 125 | accD[tid2][ijkIdx].finish(); 126 | if (accD[tid2][ijkIdx].num == 0) 127 | continue; 128 | accDM += accD[tid2][ijkIdx].A1m.cast(); 129 | } 130 | 131 | H[tid].block<8, 8>(iIdx, iIdx) += 132 | EF->adHost[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); 133 | H[tid].block<8, 8>(jIdx, kIdx) += 134 | EF->adTarget[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); 135 | H[tid].block<8, 8>(jIdx, iIdx) += 136 | EF->adTarget[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); 137 | H[tid].block<8, 8>(iIdx, kIdx) += 138 | EF->adHost[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); 139 | } 140 | } 141 | 142 | if (min == 0) { 143 | for (int tid2 = 0; tid2 < toAggregate; tid2++) { 144 | accHcc[tid2].finish(); 145 | accbc[tid2].finish(); 146 | H[tid].topLeftCorner() += accHcc[tid2].A1m.cast(); 147 | b[tid].head() += accbc[tid2].A1m.cast(); 148 | } 149 | } 150 | 151 | // // ----- new: copy transposed parts for calibration only. 152 | // for(int h=0;h(0,hIdx).noalias() = 156 | //H.block<8,4>(hIdx,0).transpose(); 157 | // } 158 | } 159 | 160 | void AccumulatedSCHessianSSE::stitchDouble(MatXX &H, VecX &b, 161 | EnergyFunctional const *const EF, 162 | int tid) { 163 | 164 | int nf = nframes[0]; 165 | int nframes2 = nf * nf; 166 | 167 | H = MatXX::Zero(nf * 8 + CPARS, nf * 8 + CPARS); 168 | b = VecX::Zero(nf * 8 + CPARS); 169 | 170 | for (int i = 0; i < nf; i++) 171 | for (int j = 0; j < nf; j++) { 172 | int iIdx = CPARS + i * 8; 173 | int jIdx = CPARS + j * 8; 174 | int ijIdx = i + nf * j; 175 | 176 | accE[tid][ijIdx].finish(); 177 | accEB[tid][ijIdx].finish(); 178 | 179 | Mat8C accEM = accE[tid][ijIdx].A1m.cast(); 180 | Vec8 accEBV = accEB[tid][ijIdx].A1m.cast(); 181 | 182 | H.block<8, CPARS>(iIdx, 0) += EF->adHost[ijIdx] * accEM; 183 | H.block<8, CPARS>(jIdx, 0) += EF->adTarget[ijIdx] * accEM; 184 | 185 | b.segment<8>(iIdx) += EF->adHost[ijIdx] * accEBV; 186 | b.segment<8>(jIdx) += EF->adTarget[ijIdx] * accEBV; 187 | 188 | for (int k = 0; k < nf; k++) { 189 | int kIdx = CPARS + k * 8; 190 | int ijkIdx = ijIdx + k * nframes2; 191 | int ikIdx = i + nf * k; 192 | 193 | accD[tid][ijkIdx].finish(); 194 | if (accD[tid][ijkIdx].num == 0) 195 | continue; 196 | Mat88 accDM = accD[tid][ijkIdx].A1m.cast(); 197 | 198 | H.block<8, 8>(iIdx, iIdx) += 199 | EF->adHost[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); 200 | 201 | H.block<8, 8>(jIdx, kIdx) += 202 | EF->adTarget[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); 203 | 204 | H.block<8, 8>(jIdx, iIdx) += 205 | EF->adTarget[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); 206 | 207 | H.block<8, 8>(iIdx, kIdx) += 208 | EF->adHost[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); 209 | } 210 | } 211 | 212 | accHcc[tid].finish(); 213 | accbc[tid].finish(); 214 | H.topLeftCorner() = accHcc[tid].A1m.cast(); 215 | b.head() = accbc[tid].A1m.cast(); 216 | 217 | // ----- new: copy transposed parts for calibration only. 218 | for (int h = 0; h < nf; h++) { 219 | int hIdx = CPARS + h * 8; 220 | H.block(0, hIdx).noalias() = 221 | H.block<8, CPARS>(hIdx, 0).transpose(); 222 | } 223 | } 224 | 225 | } // namespace dso 226 | -------------------------------------------------------------------------------- /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 | #pragma once 25 | #include 26 | #include 27 | 28 | #include "map" 29 | #include "util/MinimalImage.h" 30 | #include "util/NumType.h" 31 | 32 | namespace cv { 33 | class Mat; 34 | } 35 | 36 | namespace dso { 37 | 38 | class FrameHessian; 39 | class CalibHessian; 40 | class FrameShell; 41 | 42 | namespace IOWrap { 43 | 44 | /* ======================= Some typical usecases: =============== 45 | * 46 | * (1) always get the pose of the most recent frame: 47 | * -> Implement [publishCamPose]. 48 | * 49 | * (2) always get the depthmap of the most recent keyframe 50 | * -> Implement [pushDepthImageFloat] (use inverse depth in [image], and 51 | *pose / frame information from [KF]). 52 | * 53 | * (3) accumulate final model 54 | * -> Implement [publishKeyframes] (skip for final!=false), and accumulate 55 | *frames. 56 | * 57 | * (4) get evolving model in real-time 58 | * -> Implement [publishKeyframes] (update all frames for final==false). 59 | * 60 | * 61 | * 62 | * 63 | * ==================== How to use the structs: =================== 64 | * [FrameShell]: minimal struct kept for each frame ever tracked. 65 | * ->camToWorld = camera to world transformation 66 | * ->poseValid = false if [camToWorld] is invalid (only happens for frames 67 | *during initialization). 68 | * ->trackingRef = Shell of the frame this frame was tracked on. 69 | * ->id = ID of that frame, starting with 0 for the very first frame. 70 | * 71 | * ->incoming_id = ID passed into [addActiveFrame( ImageAndExposure* image, 72 | *int id )]. 73 | * ->timestamp = timestamp passed into [addActiveFrame( ImageAndExposure* 74 | *image, int id )] as image.timestamp. 75 | * 76 | * [FrameHessian] 77 | * ->immaturePoints: contains points that have not been "activated" (they 78 | *do however have a depth initialization). 79 | * ->pointHessians: contains active points. 80 | * ->pointHessiansMarginalized: contains marginalized points. 81 | * ->pointHessiansOut: contains outlier points. 82 | * 83 | * ->frameID: incremental ID for keyframes only. 84 | * ->shell: corresponding [FrameShell] struct. 85 | * 86 | * 87 | * [CalibHessian] 88 | * ->fxl(), fyl(), cxl(), cyl(): get optimized, most recent (pinhole) 89 | *camera intrinsics. 90 | * 91 | * 92 | * [PointHessian] 93 | * ->u,v: pixel-coordinates of point. 94 | * ->idepth_scaled: inverse depth of point. 95 | * DO NOT USE [idepth], since it may be scaled with 96 | *[SCALE_IDEPTH] ... however that is currently set to 1 so never mind. 97 | * ->host: pointer to host-frame of point. 98 | * ->status: current status of point (ACTIVE=0, INACTIVE, OUTLIER, OOB, 99 | *MARGINALIZED) 100 | * ->numGoodResiduals: number of non-outlier residuals supporting this 101 | *point (approximate). 102 | * ->maxRelBaseline: value roughly proportional to the relative baseline 103 | *this point was observed with (0 = no baseline). points for which this value is 104 | *low are badly contrained. 105 | * ->idepth_hessian: hessian value (inverse variance) of inverse depth. 106 | * 107 | * [ImmaturePoint] 108 | * ->u,v: pixel-coordinates of point. 109 | * ->idepth_min, idepth_max: the initialization sais that the inverse depth 110 | *of this point is very likely between these two thresholds (their mean being 111 | *the best guess) 112 | * ->host: pointer to host-frame of point. 113 | */ 114 | 115 | class Output3DWrapper { 116 | public: 117 | Output3DWrapper() {} 118 | virtual ~Output3DWrapper() {} 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 123 | * residuals in between them, and [1] the number of marginalized reisduals 124 | * between them. frame-frame pairs are encoded as HASH_IDX = 125 | * [(int)hostFrameKFID << 32 + (int)targetFrameKFID]. the [***frameKFID] used 126 | * for hashing correspond to the [FrameHessian]->frameID. 127 | * 128 | * Calling: 129 | * Always called, no overhead if not used. 130 | */ 131 | virtual void publishGraph( 132 | const std::map< 133 | uint64_t, Eigen::Vector2i, std::less, 134 | Eigen::aligned_allocator>> 135 | &connectivity) {} 136 | 137 | /* Usage: 138 | * Called after each new Keyframe is inserted & optimized, with all keyframes 139 | * that were part of the active window during that optimization in [frames] 140 | * (with final=false). Use to access the new frame pose and points. Also 141 | * called just before a frame is marginalized (with final=true) with only that 142 | * frame in [frames]; at that point, no further updates will ever occur to 143 | * it's optimized values (pose & idepth values of it's points). 144 | * 145 | * If you want always all most recent values for all frames, just use the 146 | * [final=false] calls. If you only want to get the final model, but don't 147 | * care about it being delay-free, only use the [final=true] calls, to save 148 | * compute. 149 | * 150 | * Calling: 151 | * Always called, negligible overhead if not used. 152 | */ 153 | virtual void publishKeyframes(std::vector &frames, bool final, 154 | CalibHessian *HCalib) {} 155 | 156 | /* Usage: 157 | * Called once for each tracked frame, with the real-time, low-delay frame 158 | * pose. 159 | * 160 | * Calling: 161 | * Always called, no overhead if not used. 162 | */ 163 | virtual void publishCamPose(FrameShell *frame, CalibHessian *HCalib) {} 164 | 165 | /* Usage: 166 | * Called once for each new frame, before it is tracked (i.e., it doesn't have 167 | * a pose yet). 168 | * 169 | * Calling: 170 | * Always called, no overhead if not used. 171 | */ 172 | virtual void pushLiveFrame(FrameHessian *image) {} 173 | 174 | /* called once after a new keyframe is created, with the color-coded, 175 | * forward-warped inverse depthmap for that keyframe, which is used for 176 | * initial alignment of future frames. Meant for visualization. 177 | * 178 | * Calling: 179 | * Needs to prepare the depth image, so it is only called if 180 | * [needPushDepthImage()] returned true. 181 | */ 182 | virtual void pushDepthImage(MinimalImageB3 *image) {} 183 | virtual bool needPushDepthImage() { return false; } 184 | 185 | /* Usage: 186 | * called once after a new keyframe is created, with the forward-warped 187 | * inverse depthmap for that keyframe. 188 | * (<= 0 for pixels without inv. depth value, >0 for pixels with inv. depth 189 | * value) 190 | * 191 | * Calling: 192 | * Always called, almost no overhead if not used. 193 | */ 194 | virtual void pushDepthImageFloat(MinimalImageF *image, FrameHessian *KF) {} 195 | 196 | /* call on finish */ 197 | virtual void join() {} 198 | 199 | /* call on reset */ 200 | virtual void reset() {} 201 | }; 202 | 203 | } // namespace IOWrap 204 | } // namespace dso 205 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /src/FullSystem/PixelSelector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | #pragma once 25 | 26 | #include "util/NumType.h" 27 | 28 | namespace dso { 29 | 30 | const float minUseGrad_pixsel = 10; 31 | 32 | template 33 | inline int gridMaxSelection(Eigen::Vector3f *grads, bool *map_out, int w, int h, 34 | float THFac) { 35 | 36 | memset(map_out, 0, sizeof(bool) * w * h); 37 | 38 | int numGood = 0; 39 | for (int y = 1; y < h - pot; y += pot) { 40 | for (int x = 1; x < w - pot; x += pot) { 41 | int bestXXID = -1; 42 | int bestYYID = -1; 43 | int bestXYID = -1; 44 | int bestYXID = -1; 45 | 46 | float bestXX = 0, bestYY = 0, bestXY = 0, bestYX = 0; 47 | 48 | Eigen::Vector3f *grads0 = grads + x + y * w; 49 | for (int dx = 0; dx < pot; dx++) 50 | for (int dy = 0; dy < pot; dy++) { 51 | int idx = dx + dy * w; 52 | Eigen::Vector3f g = grads0[idx]; 53 | float sqgd = g.tail<2>().squaredNorm(); 54 | float TH = THFac * minUseGrad_pixsel * (0.75f); 55 | 56 | if (sqgd > TH * TH) { 57 | float agx = fabs((float)g[1]); 58 | if (agx > bestXX) { 59 | bestXX = agx; 60 | bestXXID = idx; 61 | } 62 | 63 | float agy = fabs((float)g[2]); 64 | if (agy > bestYY) { 65 | bestYY = agy; 66 | bestYYID = idx; 67 | } 68 | 69 | float gxpy = fabs((float)(g[1] - g[2])); 70 | if (gxpy > bestXY) { 71 | bestXY = gxpy; 72 | bestXYID = idx; 73 | } 74 | 75 | float gxmy = fabs((float)(g[1] + g[2])); 76 | if (gxmy > bestYX) { 77 | bestYX = gxmy; 78 | bestYXID = idx; 79 | } 80 | } 81 | } 82 | 83 | bool *map0 = map_out + x + y * w; 84 | 85 | if (bestXXID >= 0) { 86 | if (!map0[bestXXID]) 87 | numGood++; 88 | map0[bestXXID] = true; 89 | } 90 | if (bestYYID >= 0) { 91 | if (!map0[bestYYID]) 92 | numGood++; 93 | map0[bestYYID] = true; 94 | } 95 | if (bestXYID >= 0) { 96 | if (!map0[bestXYID]) 97 | numGood++; 98 | map0[bestXYID] = true; 99 | } 100 | if (bestYXID >= 0) { 101 | if (!map0[bestYXID]) 102 | numGood++; 103 | map0[bestYXID] = true; 104 | } 105 | } 106 | } 107 | 108 | return numGood; 109 | } 110 | 111 | inline int gridMaxSelection(Eigen::Vector3f *grads, bool *map_out, int w, int h, 112 | int pot, float THFac) { 113 | 114 | memset(map_out, 0, sizeof(bool) * w * h); 115 | 116 | int numGood = 0; 117 | for (int y = 1; y < h - pot; y += pot) { 118 | for (int x = 1; x < w - pot; x += pot) { 119 | int bestXXID = -1; 120 | int bestYYID = -1; 121 | int bestXYID = -1; 122 | int bestYXID = -1; 123 | 124 | float bestXX = 0, bestYY = 0, bestXY = 0, bestYX = 0; 125 | 126 | Eigen::Vector3f *grads0 = grads + x + y * w; 127 | for (int dx = 0; dx < pot; dx++) 128 | for (int dy = 0; dy < pot; dy++) { 129 | int idx = dx + dy * w; 130 | Eigen::Vector3f g = grads0[idx]; 131 | float sqgd = g.tail<2>().squaredNorm(); 132 | float TH = THFac * minUseGrad_pixsel * (0.75f); 133 | 134 | if (sqgd > TH * TH) { 135 | float agx = fabs((float)g[1]); 136 | if (agx > bestXX) { 137 | bestXX = agx; 138 | bestXXID = idx; 139 | } 140 | 141 | float agy = fabs((float)g[2]); 142 | if (agy > bestYY) { 143 | bestYY = agy; 144 | bestYYID = idx; 145 | } 146 | 147 | float gxpy = fabs((float)(g[1] - g[2])); 148 | if (gxpy > bestXY) { 149 | bestXY = gxpy; 150 | bestXYID = idx; 151 | } 152 | 153 | float gxmy = fabs((float)(g[1] + g[2])); 154 | if (gxmy > bestYX) { 155 | bestYX = gxmy; 156 | bestYXID = idx; 157 | } 158 | } 159 | } 160 | 161 | bool *map0 = map_out + x + y * w; 162 | 163 | if (bestXXID >= 0) { 164 | if (!map0[bestXXID]) 165 | numGood++; 166 | map0[bestXXID] = true; 167 | } 168 | if (bestYYID >= 0) { 169 | if (!map0[bestYYID]) 170 | numGood++; 171 | map0[bestYYID] = true; 172 | } 173 | if (bestXYID >= 0) { 174 | if (!map0[bestXYID]) 175 | numGood++; 176 | map0[bestXYID] = true; 177 | } 178 | if (bestYXID >= 0) { 179 | if (!map0[bestYXID]) 180 | numGood++; 181 | map0[bestYXID] = true; 182 | } 183 | } 184 | } 185 | 186 | return numGood; 187 | } 188 | 189 | inline int makePixelStatus(Eigen::Vector3f *grads, bool *map, int w, int h, 190 | float desiredDensity, int recsLeft = 5, 191 | float THFac = 1) { 192 | if (sparsityFactor < 1) 193 | sparsityFactor = 1; 194 | 195 | int numGoodPoints; 196 | 197 | if (sparsityFactor == 1) 198 | numGoodPoints = gridMaxSelection<1>(grads, map, w, h, THFac); 199 | else if (sparsityFactor == 2) 200 | numGoodPoints = gridMaxSelection<2>(grads, map, w, h, THFac); 201 | else if (sparsityFactor == 3) 202 | numGoodPoints = gridMaxSelection<3>(grads, map, w, h, THFac); 203 | else if (sparsityFactor == 4) 204 | numGoodPoints = gridMaxSelection<4>(grads, map, w, h, THFac); 205 | else if (sparsityFactor == 5) 206 | numGoodPoints = gridMaxSelection<5>(grads, map, w, h, THFac); 207 | else if (sparsityFactor == 6) 208 | numGoodPoints = gridMaxSelection<6>(grads, map, w, h, THFac); 209 | else if (sparsityFactor == 7) 210 | numGoodPoints = gridMaxSelection<7>(grads, map, w, h, THFac); 211 | else if (sparsityFactor == 8) 212 | numGoodPoints = gridMaxSelection<8>(grads, map, w, h, THFac); 213 | else if (sparsityFactor == 9) 214 | numGoodPoints = gridMaxSelection<9>(grads, map, w, h, THFac); 215 | else if (sparsityFactor == 10) 216 | numGoodPoints = gridMaxSelection<10>(grads, map, w, h, THFac); 217 | else if (sparsityFactor == 11) 218 | numGoodPoints = gridMaxSelection<11>(grads, map, w, h, THFac); 219 | else 220 | numGoodPoints = gridMaxSelection(grads, map, w, h, sparsityFactor, THFac); 221 | 222 | /* 223 | * #points is approximately proportional to sparsityFactor^2. 224 | */ 225 | 226 | float quotia = numGoodPoints / (float)(desiredDensity); 227 | 228 | int newSparsity = (sparsityFactor * sqrtf(quotia)) + 0.7f; 229 | 230 | if (newSparsity < 1) 231 | newSparsity = 1; 232 | 233 | float oldTHFac = THFac; 234 | if (newSparsity == 1 && sparsityFactor == 1) 235 | THFac = 0.5; 236 | 237 | if ((abs(newSparsity - sparsityFactor) < 1 && THFac == oldTHFac) || 238 | (quotia > 0.8 && 1.0f / quotia > 0.8) || recsLeft == 0) { 239 | 240 | // printf(" \n"); 241 | // all good 242 | sparsityFactor = newSparsity; 243 | return numGoodPoints; 244 | } else { 245 | // printf(" -> re-evaluate! \n"); 246 | // re-evaluate. 247 | sparsityFactor = newSparsity; 248 | return makePixelStatus(grads, map, w, h, desiredDensity, recsLeft - 1, 249 | THFac); 250 | } 251 | } 252 | 253 | } // namespace dso 254 | --------------------------------------------------------------------------------