├── .gitignore ├── CMakeLists.txt ├── Doxyfile.in ├── LICENSE ├── README.md ├── manifest.xml └── src ├── CMakeLists.txt ├── EDS.h ├── bundles ├── AccumulatedSCHessian.cpp ├── AccumulatedSCHessian.h ├── AccumulatedTopHessian.cpp ├── AccumulatedTopHessian.h ├── Config.hpp ├── EnergyFunctional.cpp ├── EnergyFunctional.h ├── EnergyFunctionalStructs.cpp ├── EnergyFunctionalStructs.h ├── MatrixAccumulators.h └── RawResidualJacobian.h ├── eds.pc.in ├── init ├── CoarseInitializer.cpp └── CoarseInitializer.h ├── io ├── ImageConvert.cpp ├── ImageConvert.h ├── ImageRW.cpp ├── ImageRW.h ├── OutputMaps.cpp └── OutputMaps.h ├── mapping ├── Config.hpp ├── DepthPoints.cpp ├── DepthPoints.hpp ├── PixelSelector.cpp ├── PixelSelector.h └── Types.hpp ├── sophus ├── rxso3.hpp ├── se2.hpp ├── se3.hpp ├── sim3.hpp ├── so2.hpp ├── so3.hpp └── sophus.hpp ├── tracking ├── CoarseTracker.cpp ├── CoarseTracker.h ├── Config.hpp ├── EventFrame.cpp ├── EventFrame.hpp ├── HessianBlocks.cpp ├── HessianBlocks.h ├── ImmaturePoint.cpp ├── ImmaturePoint.h ├── KeyFrame.cpp ├── KeyFrame.hpp ├── PhotometricError.hpp ├── PhotometricErrorNC.hpp ├── ResidualProjections.h ├── Residuals.cpp ├── Residuals.h ├── Tracker.cpp ├── Tracker.hpp └── Types.hpp └── utils ├── Calib.cpp ├── Calib.hpp ├── Colormap.cpp ├── Colormap.hpp ├── Config.hpp ├── FrameShell.h ├── ImageAndExposure.h ├── IndexThreadReduce.h ├── KDTree.hpp ├── MinimalImage.h ├── NumType.h ├── Transforms.hpp ├── Undistort.cpp ├── Undistort.h ├── Utils.cpp ├── Utils.hpp ├── globalCalib.cpp ├── globalCalib.h ├── globalFuncs.h ├── nanoflann.h ├── settings.cpp └── settings.h /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | tags 3 | *.sw? 4 | Doxyfile 5 | *.pc 6 | *.orig 7 | CMakeFiles 8 | CMakeCache.txt 9 | *.kdev4 10 | *.*~ 11 | patches-*-stamp 12 | .vscode 13 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # CMakeLists.txt has to be located in the project folder and cmake has to be 2 | # executed from 'project/build' with 'cmake ../'. 3 | cmake_minimum_required(VERSION 3.9) 4 | project(eds 5 | VERSION 0.1 6 | DESCRIPTION "Event-based Sparse Odometry") 7 | find_package(Rock) 8 | 9 | set(CMAKE_CXX_STANDARD 14) 10 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 11 | set(ROCK_TEST_ENABLED OFF) 12 | 13 | if(COVERAGE) 14 | if(CMAKE_BUILD_TYPE MATCHES Debug) 15 | add_definitions(-fprofile-arcs -ftest-coverage) 16 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fprofile-arcs -ftest-coverage -lgcov") 17 | set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -fprofile-arcs -ftest-coverage -lgcov") 18 | set(CMAKE_MODULE_LINKER_FLAGS "${CMAKE_MODULE_LINKER_FLAGS} -fprofile-arcs -ftest-coverage -lgcov") 19 | 20 | add_custom_target(coverage 21 | COMMAND lcov --directory . --capture --output-file cov.info 22 | COMMAND lcov --remove cov.info 'tests/*' '/usr/*' '*/install/include/*' --output-file cov.info.cleaned 23 | COMMAND genhtml -o ./cov cov.info.cleaned 24 | COMMAND cmake -E remove cov.info cov.info.cleaned 25 | WORKING_DIRECTORY ${CMAKE_BINARY_DIR} 26 | ) 27 | else() 28 | message(FATAL_ERROR "Code coverage only works in Debug versions" ) 29 | endif() 30 | endif() 31 | 32 | rock_init() 33 | rock_opencv_autodetect(OPENCV_PACKAGE) 34 | rock_standard_layout() 35 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | Event-aided Direct Sparse Odometry (EDS) 3 | ============= 4 | Direct visual odometry approach using events and frames (CVPR 2022) 5 | 6 | License 7 | ------- 8 | See LICENSE file 9 | 10 | 11 | Installation 12 | ------------ 13 | The easiest way to build and install this package is to use Rock's build system. 14 | See [this page](http://rock-robotics.org/documentation/installation.html) 15 | on how to install Rock. You can use the [EDS builconf](https://github.com/uzh-rpg/eds-buildconf) 16 | which tells you how to install and use this library from scratch. There is also the option 17 | to build and use this library in a Docker container. 18 | 19 | However, if you feel that it's too heavy for your needs, Rock aims at having 20 | most of its "library" packages (such as this one) to follow best practices. See 21 | [this page](http://rock-robotics.org/documentation/packages/outside_of_rock.html) 22 | for installation instructions outside of Rock. It means this library and 23 | its dependencies are standard C++ libraries. Therefore you can build your own 24 | EDS system based on this code which is out of the Rock ecosystem. 25 | 26 | Dependencies 27 | ----------------- 28 | This is an standard C++ library which generates a shared library by default. 29 | Dependencies are listed in the manifest file, those are: 30 | 31 | * [base/cmake](https://github.com/rock-core/base-cmake): the CMake pure function to build this library 32 | * [base/types](https://github.com/rock-core/base-types): C++ basic types (types depends on std C++ and Eigen) 33 | * [base/logging](https://github.com/rock-core/base-logging): C++ library for logging (similar to Google glog) 34 | * [slam/ceres_solver](https://github.com/ceres-solver/ceres-solver): the Ceres solver library 35 | * [slam/pcl](https://pointclouds.org): the point cloud library 36 | * [opencv](https://github.com/opencv/opencv/tree/4.2.0): Open source Computer Vision library. Ubuntu 20.04 uses 4.2.0 37 | * [yaml-cpp](https://github.com/jbeder/yaml-cpp): YAML config parser for the configuration parameters 38 | 39 | 40 | Rock CMake Macros 41 | ----------------- 42 | This package uses a set of CMake helper shipped as the Rock CMake macros. 43 | Documentations is available on [this page](http://rock-robotics.org/documentation/packages/cmake_macros.html). 44 | These macros are pure CMake functions, so they are totally independent of the Rock 45 | ecosystem. 46 | 47 | Library Standard Layout 48 | -------------------- 49 | This directory structure follows some simple rules, to allow for generic build 50 | processes and simplify reuse of this project. Following these rules ensures that 51 | the Rock CMake macros automatically handle the project's build process and 52 | install setup properly. 53 | 54 | ### EDS Folder Structure 55 | 56 | | directory | purpose | 57 | | ----------------- | ------------------------------------------------------ | 58 | | src/ | Contains all header (*.h/*.hpp) and source files | 59 | | src/bundles | This is the backend optimation from DSO | 60 | | src/init | This is the DSO initializer class | 61 | | src/io | I/O wrapper from images and maps different types | 62 | | src/mapping | This is the pixel selector based on DSO | 63 | | src/sophus | The Sophus template header-only library based on Eigen | 64 | | src/tracking | This is the EDS tracker to perform Events to Model alignment | 65 | | src/utils | Some utils functions | 66 | | build/ * | The target directory for the build process, temporary content | 67 | | test/ | Boost Unit test to check basic class functionalities | 68 | -------------------------------------------------------------------------------- /manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Events-based Sparse Odometry. Direct approach using events and frames 4 | 5 | Javier Hidalgo-Carrió/havyhidalgo@gmail.com 6 | GPL v3 or higher (see LICENSE file) 7 | https://rpg.ifi.uzh.ch/eds 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | needs_opt 18 | 19 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | rock_find_pkgconfig(EIGEN3 eigen3 REQUIRED) 2 | find_package(Boost REQUIRED COMPONENTS filesystem thread system) 3 | find_package(OpenCV REQUIRED core imgproc calib3d highgui) 4 | find_package(Ceres REQUIRED) 5 | find_package(PCL 1.7 REQUIRED COMPONENTS common filters) 6 | 7 | rock_library(eds 8 | SOURCES 9 | bundles/AccumulatedSCHessian.cpp 10 | bundles/AccumulatedTopHessian.cpp 11 | bundles/EnergyFunctional.cpp 12 | bundles/EnergyFunctionalStructs.cpp 13 | init/CoarseInitializer.cpp 14 | io/ImageRW.cpp 15 | io/ImageConvert.cpp 16 | io/OutputMaps.cpp 17 | mapping/PixelSelector.cpp 18 | mapping/DepthPoints.cpp 19 | tracking/CoarseTracker.cpp 20 | tracking/EventFrame.cpp 21 | tracking/HessianBlocks.cpp 22 | tracking/ImmaturePoint.cpp 23 | tracking/KeyFrame.cpp 24 | tracking/Residuals.cpp 25 | tracking/Tracker.cpp 26 | utils/Calib.cpp 27 | utils/Colormap.cpp 28 | utils/globalCalib.cpp 29 | utils/settings.cpp 30 | utils/Undistort.cpp 31 | utils/Utils.cpp 32 | 33 | HEADERS 34 | bundles/Config.hpp 35 | bundles/AccumulatedSCHessian.h 36 | bundles/AccumulatedTopHessian.h 37 | bundles/EnergyFunctional.h 38 | bundles/EnergyFunctionalStructs.h 39 | bundles/MatrixAccumulators.h 40 | bundles/RawResidualJacobian.h 41 | init/CoarseInitializer.h 42 | io/ImageRW.h 43 | io/ImageConvert.h 44 | io/OutputMaps.h 45 | mapping/Config.hpp 46 | mapping/DepthPoints.hpp 47 | mapping/Types.hpp 48 | mapping/PixelSelector.h 49 | sophus/rxso3.hpp 50 | sophus/se2.hpp 51 | sophus/se3.hpp 52 | sophus/sim3.hpp 53 | sophus/so2.hpp 54 | sophus/so3.hpp 55 | sophus/sophus.hpp 56 | tracking/Config.hpp 57 | tracking/EventFrame.hpp 58 | tracking/KeyFrame.hpp 59 | tracking/PhotometricError.hpp 60 | tracking/PhotometricErrorNC.hpp 61 | tracking/Tracker.hpp 62 | tracking/Types.hpp 63 | tracking/CoarseTracker.h 64 | tracking/HessianBlocks.h 65 | tracking/ImmaturePoint.h 66 | tracking/ResidualProjections.h 67 | tracking/Residuals.h 68 | utils/Calib.hpp 69 | utils/Colormap.hpp 70 | utils/Config.hpp 71 | utils/KDTree.hpp 72 | utils/Transforms.hpp 73 | utils/Utils.hpp 74 | utils/FrameShell.h 75 | utils/globalCalib.h 76 | utils/globalFuncs.h 77 | utils/ImageAndExposure.h 78 | utils/IndexThreadReduce.h 79 | utils/MinimalImage.h 80 | utils/nanoflann.h 81 | utils/NumType.h 82 | utils/settings.h 83 | utils/Undistort.h 84 | EDS.h 85 | DEPS_TARGET 86 | ceres 87 | DEPS_PKGCONFIG 88 | base-types 89 | base-logging 90 | yaml-cpp 91 | frame_helper 92 | pcl_common-${PCL_VERSION_MAJOR}.${PCL_VERSION_MINOR} 93 | pcl_filters-${PCL_VERSION_MAJOR}.${PCL_VERSION_MINOR} 94 | ${OPENCV_PACKAGE} 95 | DEPS_PLAIN Boost_SYSTEM Boost_FILESYSTEM Boost_THREAD 96 | ) 97 | -------------------------------------------------------------------------------- /src/EDS.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the EDS: Event-aided Direct Sparse Odometry 3 | * (https://rpg.ifi.uzh.ch/eds.html) 4 | * 5 | * Copyright (c) 2022 Javier Hidalgo-Carrió, Robotics and Perception 6 | * Group (RPG) University of Zurich. 7 | * 8 | * EDS is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, version 3. 11 | * 12 | * EDS is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #ifndef EDS_EDS_H 22 | #define EDS_EDS_H 23 | 24 | /** Base types **/ 25 | #include 26 | 27 | /** Settings **/ 28 | #include 29 | 30 | /** Utils **/ 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | /** I/O **/ 42 | #include 43 | #include 44 | #include 45 | 46 | /** Initialization **/ 47 | #include 48 | 49 | /** Event Tracker (EDS) **/ 50 | #include 51 | #include 52 | #include 53 | 54 | /** Frame Tracker (DSO) **/ 55 | #include 56 | #include 57 | #include 58 | #include 59 | 60 | /** Mapping **/ 61 | #include 62 | #include 63 | #include 64 | 65 | /** Bundles **/ 66 | #include 67 | #include 68 | #include 69 | #include 70 | 71 | #endif //EDS_EDS_H -------------------------------------------------------------------------------- /src/bundles/AccumulatedSCHessian.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #include "eds/bundles/AccumulatedSCHessian.h" 26 | #include "eds/bundles/EnergyFunctional.h" 27 | #include "eds/bundles/EnergyFunctionalStructs.h" 28 | 29 | #include "eds/tracking/HessianBlocks.h" 30 | 31 | namespace dso 32 | { 33 | 34 | void AccumulatedSCHessianSSE::addPoint(EFPoint* p, bool shiftPriorToZero, int tid) 35 | { 36 | int ngoodres = 0; 37 | for(EFResidual* r : p->residualsAll) if(r->isActive()) ngoodres++; 38 | if(ngoodres==0) 39 | { 40 | p->HdiF=0; 41 | p->bdSumF=0; 42 | p->data->idepth_hessian=0; 43 | p->data->maxRelBaseline=0; 44 | return; 45 | } 46 | 47 | float H = p->Hdd_accAF+p->Hdd_accLF+p->priorF; 48 | if(H < 1e-10) H = 1e-10; 49 | 50 | p->data->idepth_hessian=H; 51 | 52 | p->HdiF = 1.0 / H; 53 | p->bdSumF = p->bd_accAF + p->bd_accLF; 54 | if(shiftPriorToZero) p->bdSumF += p->priorF*p->deltaF; 55 | VecCf Hcd = p->Hcd_accAF + p->Hcd_accLF; 56 | accHcc[tid].update(Hcd,Hcd,p->HdiF); 57 | accbc[tid].update(Hcd, p->bdSumF * p->HdiF); 58 | 59 | assert(std::isfinite((float)(p->HdiF))); 60 | 61 | int nFrames2 = nframes[tid]*nframes[tid]; 62 | for(EFResidual* r1 : p->residualsAll) 63 | { 64 | if(!r1->isActive()) continue; 65 | int r1ht = r1->hostIDX + r1->targetIDX*nframes[tid]; 66 | 67 | for(EFResidual* r2 : p->residualsAll) 68 | { 69 | if(!r2->isActive()) continue; 70 | 71 | accD[tid][r1ht+r2->targetIDX*nFrames2].update(r1->JpJdF, r2->JpJdF, p->HdiF); 72 | } 73 | 74 | accE[tid][r1ht].update(r1->JpJdF, Hcd, p->HdiF); 75 | accEB[tid][r1ht].update(r1->JpJdF,p->HdiF*p->bdSumF); 76 | } 77 | } 78 | void AccumulatedSCHessianSSE::stitchDoubleInternal( 79 | MatXX* H, VecX* b, EnergyFunctional const * const EF, 80 | int min, int max, Vec10* stats, int tid) 81 | { 82 | int toAggregate = NUM_THREADS; 83 | if(tid == -1) { toAggregate = 1; tid = 0; } // special case: if we dont do multithreading, dont aggregate. 84 | if(min==max) return; 85 | 86 | 87 | int nf = nframes[0]; 88 | int nframes2 = nf*nf; 89 | 90 | for(int k=min;k(); 107 | bp += accEB[tid2][ijIdx].A1m.cast(); 108 | } 109 | 110 | H[tid].block<8,CPARS>(iIdx,0) += EF->adHost[ijIdx] * Hpc; 111 | H[tid].block<8,CPARS>(jIdx,0) += EF->adTarget[ijIdx] * Hpc; 112 | b[tid].segment<8>(iIdx) += EF->adHost[ijIdx] * bp; 113 | b[tid].segment<8>(jIdx) += EF->adTarget[ijIdx] * bp; 114 | 115 | 116 | 117 | for(int k=0;k(); 130 | } 131 | 132 | H[tid].block<8,8>(iIdx, iIdx) += EF->adHost[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); 133 | H[tid].block<8,8>(jIdx, kIdx) += EF->adTarget[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); 134 | H[tid].block<8,8>(jIdx, iIdx) += EF->adTarget[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); 135 | H[tid].block<8,8>(iIdx, kIdx) += EF->adHost[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); 136 | } 137 | } 138 | 139 | if(min==0) 140 | { 141 | for(int tid2=0;tid2 < toAggregate;tid2++) 142 | { 143 | accHcc[tid2].finish(); 144 | accbc[tid2].finish(); 145 | H[tid].topLeftCorner() += accHcc[tid2].A1m.cast(); 146 | b[tid].head() += accbc[tid2].A1m.cast(); 147 | } 148 | } 149 | 150 | 151 | // // ----- new: copy transposed parts for calibration only. 152 | // for(int h=0;h(0,hIdx).noalias() = H.block<8,4>(hIdx,0).transpose(); 156 | // } 157 | } 158 | 159 | void AccumulatedSCHessianSSE::stitchDouble(MatXX &H, VecX &b, EnergyFunctional const * const EF, int tid) 160 | { 161 | 162 | int nf = nframes[0]; 163 | int nframes2 = nf*nf; 164 | 165 | H = MatXX::Zero(nf*8+CPARS, nf*8+CPARS); 166 | b = VecX::Zero(nf*8+CPARS); 167 | 168 | 169 | for(int i=0;i(); 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(); 197 | 198 | H.block<8,8>(iIdx, iIdx) += EF->adHost[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); 199 | 200 | H.block<8,8>(jIdx, kIdx) += EF->adTarget[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); 201 | 202 | H.block<8,8>(jIdx, iIdx) += EF->adTarget[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); 203 | 204 | H.block<8,8>(iIdx, kIdx) += EF->adHost[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); 205 | } 206 | } 207 | 208 | accHcc[tid].finish(); 209 | accbc[tid].finish(); 210 | H.topLeftCorner() = accHcc[tid].A1m.cast(); 211 | b.head() = accbc[tid].A1m.cast(); 212 | 213 | // ----- new: copy transposed parts for calibration only. 214 | for(int h=0;h(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose(); 218 | } 219 | } 220 | 221 | } 222 | -------------------------------------------------------------------------------- /src/bundles/AccumulatedSCHessian.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | 28 | #include "eds/utils/NumType.h" 29 | #include "eds/utils/IndexThreadReduce.h" 30 | #include "eds/bundles/MatrixAccumulators.h" 31 | #include "vector" 32 | #include 33 | 34 | namespace dso 35 | { 36 | 37 | class EFPoint; 38 | class EnergyFunctional; 39 | 40 | 41 | class AccumulatedSCHessianSSE 42 | { 43 | public: 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 45 | inline AccumulatedSCHessianSSE() 46 | { 47 | for(int i=0;i[n*n]; 73 | accEB[tid] = new AccumulatorX<8>[n*n]; 74 | accD[tid] = new AccumulatorXX<8,8>[n*n*n]; 75 | } 76 | accbc[tid].initialize(); 77 | accHcc[tid].initialize(); 78 | 79 | for(int i=0;i* red, MatXX &H, VecX &b, EnergyFunctional const * const EF, bool MT) 94 | { 95 | // sum up, splitting by bock in square. 96 | if(MT) 97 | { 98 | MatXX Hs[NUM_THREADS]; 99 | VecX bs[NUM_THREADS]; 100 | for(int i=0;ireduce(boost::bind(&AccumulatedSCHessianSSE::stitchDoubleInternal, 108 | this,Hs, bs, EF, _1, _2, _3, _4), 0, nframes[0]*nframes[0], 0); 109 | 110 | // sum up results 111 | H = Hs[0]; 112 | b = bs[0]; 113 | 114 | for(int i=1;i(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose(); 132 | } 133 | } 134 | 135 | 136 | AccumulatorXX<8,CPARS>* accE[NUM_THREADS]; 137 | AccumulatorX<8>* accEB[NUM_THREADS]; 138 | AccumulatorXX<8,8>* accD[NUM_THREADS]; 139 | AccumulatorXX accHcc[NUM_THREADS]; 140 | AccumulatorX accbc[NUM_THREADS]; 141 | int nframes[NUM_THREADS]; 142 | 143 | 144 | void addPointsInternal( 145 | std::vector* points, bool shiftPriorToZero, 146 | int min=0, int max=1, Vec10* stats=0, int tid=0) 147 | { 148 | for(int i=min;i, 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #include "eds/bundles/AccumulatedTopHessian.h" 26 | #include "eds/bundles/EnergyFunctional.h" 27 | #include "eds/bundles/EnergyFunctionalStructs.h" 28 | #include 29 | 30 | #if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) 31 | #include "SSE2NEON.h" 32 | #endif 33 | 34 | namespace dso 35 | { 36 | 37 | 38 | 39 | template 40 | void AccumulatedTopHessianSSE::addPoint(EFPoint* p, EnergyFunctional const * const ef, int tid) // 0 = active, 1 = linearized, 2=marginalize 41 | { 42 | 43 | 44 | assert(mode==0 || mode==1 || mode==2); 45 | 46 | VecCf dc = ef->cDeltaF; 47 | float dd = p->deltaF; 48 | 49 | float bd_acc=0; 50 | float Hdd_acc=0; 51 | VecCf Hcd_acc = VecCf::Zero(); 52 | 53 | for(EFResidual* r : p->residualsAll) 54 | { 55 | if(mode==0) 56 | { 57 | if(r->isLinearized || !r->isActive()) continue; 58 | } 59 | if(mode==1) 60 | { 61 | if(!r->isLinearized || !r->isActive()) continue; 62 | } 63 | if(mode==2) 64 | { 65 | if(!r->isActive()) continue; 66 | assert(r->isLinearized); 67 | } 68 | 69 | 70 | RawResidualJacobian* rJ = r->J; 71 | int htIDX = r->hostIDX + r->targetIDX*nframes[tid]; 72 | Mat18f dp = ef->adHTdeltaF[htIDX]; 73 | 74 | 75 | 76 | VecNRf resApprox; 77 | if(mode==0) 78 | resApprox = rJ->resF; 79 | if(mode==2) 80 | resApprox = r->res_toZeroF; 81 | if(mode==1) 82 | { 83 | // compute Jp*delta 84 | __m128 Jp_delta_x = _mm_set1_ps(rJ->Jpdxi[0].dot(dp.head<6>())+rJ->Jpdc[0].dot(dc)+rJ->Jpdd[0]*dd); 85 | __m128 Jp_delta_y = _mm_set1_ps(rJ->Jpdxi[1].dot(dp.head<6>())+rJ->Jpdc[1].dot(dc)+rJ->Jpdd[1]*dd); 86 | __m128 delta_a = _mm_set1_ps((float)(dp[6])); 87 | __m128 delta_b = _mm_set1_ps((float)(dp[7])); 88 | 89 | for(int i=0;ires_toZeroF)+i); 93 | rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JIdx))+i),Jp_delta_x)); 94 | rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JIdx+1))+i),Jp_delta_y)); 95 | rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JabF))+i),delta_a)); 96 | rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JabF+1))+i),delta_b)); 97 | _mm_store_ps(((float*)&resApprox)+i, rtz); 98 | } 99 | } 100 | 101 | // need to compute JI^T * r, and Jab^T * r. (both are 2-vectors). 102 | Vec2f JI_r(0,0); 103 | Vec2f Jab_r(0,0); 104 | float rr=0; 105 | for(int i=0;iJIdx[0][i]; 108 | JI_r[1] += resApprox[i] *rJ->JIdx[1][i]; 109 | Jab_r[0] += resApprox[i] *rJ->JabF[0][i]; 110 | Jab_r[1] += resApprox[i] *rJ->JabF[1][i]; 111 | rr += resApprox[i]*resApprox[i]; 112 | } 113 | 114 | 115 | acc[tid][htIDX].update( 116 | rJ->Jpdc[0].data(), rJ->Jpdxi[0].data(), 117 | rJ->Jpdc[1].data(), rJ->Jpdxi[1].data(), 118 | rJ->JIdx2(0,0),rJ->JIdx2(0,1),rJ->JIdx2(1,1)); 119 | 120 | acc[tid][htIDX].updateBotRight( 121 | rJ->Jab2(0,0), rJ->Jab2(0,1), Jab_r[0], 122 | rJ->Jab2(1,1), Jab_r[1],rr); 123 | 124 | acc[tid][htIDX].updateTopRight( 125 | rJ->Jpdc[0].data(), rJ->Jpdxi[0].data(), 126 | rJ->Jpdc[1].data(), rJ->Jpdxi[1].data(), 127 | rJ->JabJIdx(0,0), rJ->JabJIdx(0,1), 128 | rJ->JabJIdx(1,0), rJ->JabJIdx(1,1), 129 | JI_r[0], JI_r[1]); 130 | 131 | 132 | Vec2f Ji2_Jpdd = rJ->JIdx2 * rJ->Jpdd; 133 | bd_acc += JI_r[0]*rJ->Jpdd[0] + JI_r[1]*rJ->Jpdd[1]; 134 | Hdd_acc += Ji2_Jpdd.dot(rJ->Jpdd); 135 | Hcd_acc += rJ->Jpdc[0]*Ji2_Jpdd[0] + rJ->Jpdc[1]*Ji2_Jpdd[1]; 136 | 137 | nres[tid]++; 138 | } 139 | 140 | if(mode==0) 141 | { 142 | p->Hdd_accAF = Hdd_acc; 143 | p->bd_accAF = bd_acc; 144 | p->Hcd_accAF = Hcd_acc; 145 | } 146 | if(mode==1 || mode==2) 147 | { 148 | p->Hdd_accLF = Hdd_acc; 149 | p->bd_accLF = bd_acc; 150 | p->Hcd_accLF = Hcd_acc; 151 | } 152 | if(mode==2) 153 | { 154 | p->Hcd_accAF.setZero(); 155 | p->Hdd_accAF = 0; 156 | p->bd_accAF = 0; 157 | } 158 | 159 | } 160 | template void AccumulatedTopHessianSSE::addPoint<0>(EFPoint* p, EnergyFunctional const * const ef, int tid); 161 | template void AccumulatedTopHessianSSE::addPoint<1>(EFPoint* p, EnergyFunctional const * const ef, int tid); 162 | template void AccumulatedTopHessianSSE::addPoint<2>(EFPoint* p, EnergyFunctional const * const ef, int tid); 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | void AccumulatedTopHessianSSE::stitchDouble(MatXX &H, VecX &b, EnergyFunctional const * const EF, bool usePrior, bool useDelta, int tid) 172 | { 173 | H = MatXX::Zero(nframes[tid]*8+CPARS, nframes[tid]*8+CPARS); 174 | b = VecX::Zero(nframes[tid]*8+CPARS); 175 | 176 | 177 | for(int h=0;h(); 190 | 191 | 192 | H.block<8,8>(hIdx, hIdx).noalias() += EF->adHost[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adHost[aidx].transpose(); 193 | 194 | H.block<8,8>(tIdx, tIdx).noalias() += EF->adTarget[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adTarget[aidx].transpose(); 195 | 196 | H.block<8,8>(hIdx, tIdx).noalias() += EF->adHost[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adTarget[aidx].transpose(); 197 | 198 | H.block<8,CPARS>(hIdx,0).noalias() += EF->adHost[aidx] * accH.block<8,CPARS>(CPARS,0); 199 | 200 | H.block<8,CPARS>(tIdx,0).noalias() += EF->adTarget[aidx] * accH.block<8,CPARS>(CPARS,0); 201 | 202 | H.topLeftCorner().noalias() += accH.block(0,0); 203 | 204 | b.segment<8>(hIdx).noalias() += EF->adHost[aidx] * accH.block<8,1>(CPARS,8+CPARS); 205 | 206 | b.segment<8>(tIdx).noalias() += EF->adTarget[aidx] * accH.block<8,1>(CPARS,8+CPARS); 207 | 208 | b.head().noalias() += accH.block(0,8+CPARS); 209 | } 210 | 211 | 212 | // ----- new: copy transposed parts. 213 | for(int h=0;h(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose(); 217 | 218 | for(int t=h+1;t(hIdx, tIdx).noalias() += H.block<8,8>(tIdx, hIdx).transpose(); 222 | H.block<8,8>(tIdx, hIdx).noalias() = H.block<8,8>(hIdx, tIdx).transpose(); 223 | } 224 | } 225 | 226 | 227 | if(usePrior) 228 | { 229 | assert(useDelta); 230 | H.diagonal().head() += EF->cPrior; 231 | b.head() += EF->cPrior.cwiseProduct(EF->cDeltaF.cast()); 232 | for(int h=0;h(CPARS+h*8) += EF->frames[h]->prior; 235 | b.segment<8>(CPARS+h*8) += EF->frames[h]->prior.cwiseProduct(EF->frames[h]->delta_prior); 236 | } 237 | } 238 | } 239 | 240 | 241 | void AccumulatedTopHessianSSE::stitchDoubleInternal( 242 | MatXX* H, VecX* b, EnergyFunctional const * const EF, bool usePrior, 243 | int min, int max, Vec10* stats, int tid) 244 | { 245 | int toAggregate = NUM_THREADS; 246 | if(tid == -1) { toAggregate = 1; tid = 0; } // special case: if we dont do multithreading, dont aggregate. 247 | if(min==max) return; 248 | 249 | 250 | for(int k=min;k(); 268 | } 269 | 270 | H[tid].block<8,8>(hIdx, hIdx).noalias() += EF->adHost[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adHost[aidx].transpose(); 271 | 272 | H[tid].block<8,8>(tIdx, tIdx).noalias() += EF->adTarget[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adTarget[aidx].transpose(); 273 | 274 | H[tid].block<8,8>(hIdx, tIdx).noalias() += EF->adHost[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adTarget[aidx].transpose(); 275 | 276 | H[tid].block<8,CPARS>(hIdx,0).noalias() += EF->adHost[aidx] * accH.block<8,CPARS>(CPARS,0); 277 | 278 | H[tid].block<8,CPARS>(tIdx,0).noalias() += EF->adTarget[aidx] * accH.block<8,CPARS>(CPARS,0); 279 | 280 | H[tid].topLeftCorner().noalias() += accH.block(0,0); 281 | 282 | b[tid].segment<8>(hIdx).noalias() += EF->adHost[aidx] * accH.block<8,1>(CPARS,CPARS+8); 283 | 284 | b[tid].segment<8>(tIdx).noalias() += EF->adTarget[aidx] * accH.block<8,1>(CPARS,CPARS+8); 285 | 286 | b[tid].head().noalias() += accH.block(0,CPARS+8); 287 | 288 | } 289 | 290 | 291 | // only do this on one thread. 292 | if(min==0 && usePrior) 293 | { 294 | H[tid].diagonal().head() += EF->cPrior; 295 | b[tid].head() += EF->cPrior.cwiseProduct(EF->cDeltaF.cast()); 296 | for(int h=0;h(CPARS+h*8) += EF->frames[h]->prior; 299 | b[tid].segment<8>(CPARS+h*8) += EF->frames[h]->prior.cwiseProduct(EF->frames[h]->delta_prior); 300 | 301 | } 302 | } 303 | } 304 | 305 | 306 | 307 | } 308 | 309 | 310 | -------------------------------------------------------------------------------- /src/bundles/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 | 25 | #pragma once 26 | 27 | 28 | #include "eds/utils/NumType.h" 29 | #include "eds/bundles/MatrixAccumulators.h" 30 | #include "vector" 31 | #include 32 | #include "eds/utils/IndexThreadReduce.h" 33 | 34 | 35 | namespace dso 36 | { 37 | 38 | class EFPoint; 39 | class EnergyFunctional; 40 | 41 | 42 | 43 | class AccumulatedTopHessianSSE 44 | { 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 47 | inline AccumulatedTopHessianSSE() 48 | { 49 | for(int tid=0;tid < NUM_THREADS; tid++) 50 | { 51 | nres[tid]=0; 52 | acc[tid]=0; 53 | nframes[tid]=0; 54 | } 55 | 56 | }; 57 | inline ~AccumulatedTopHessianSSE() 58 | { 59 | for(int tid=0;tid < NUM_THREADS; tid++) 60 | { 61 | if(acc[tid] != 0) delete[] acc[tid]; 62 | } 63 | }; 64 | 65 | inline void setZero(int nFrames, int min=0, int max=1, Vec10* stats=0, int tid=0) 66 | { 67 | 68 | if(nFrames != nframes[tid]) 69 | { 70 | if(acc[tid] != 0) delete[] acc[tid]; 71 | #if USE_XI_MODEL 72 | acc[tid] = new Accumulator14[nFrames*nFrames]; 73 | #else 74 | acc[tid] = new AccumulatorApprox[nFrames*nFrames]; 75 | #endif 76 | } 77 | 78 | for(int i=0;i void addPoint(EFPoint* p, EnergyFunctional const * const ef, int tid=0); 88 | 89 | 90 | 91 | void stitchDoubleMT(IndexThreadReduce* red, MatXX &H, VecX &b, EnergyFunctional const * const EF, bool usePrior, bool MT) 92 | { 93 | // sum up, splitting by bock in square. 94 | if(MT) 95 | { 96 | MatXX Hs[NUM_THREADS]; 97 | VecX bs[NUM_THREADS]; 98 | for(int i=0;ireduce(boost::bind(&AccumulatedTopHessianSSE::stitchDoubleInternal, 106 | this,Hs, bs, EF, usePrior, _1, _2, _3, _4), 0, nframes[0]*nframes[0], 0); 107 | 108 | // sum up results 109 | H = Hs[0]; 110 | b = bs[0]; 111 | 112 | for(int i=1;i(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose(); 131 | 132 | for(int t=h+1;t(hIdx, tIdx).noalias() += H.block<8,8>(tIdx, hIdx).transpose(); 136 | H.block<8,8>(tIdx, hIdx).noalias() = H.block<8,8>(hIdx, tIdx).transpose(); 137 | } 138 | } 139 | } 140 | 141 | 142 | 143 | 144 | int nframes[NUM_THREADS]; 145 | 146 | EIGEN_ALIGN16 AccumulatorApprox* acc[NUM_THREADS]; 147 | 148 | 149 | int nres[NUM_THREADS]; 150 | 151 | 152 | template void addPointsInternal( 153 | std::vector* points, EnergyFunctional const * const ef, 154 | int min=0, int max=1, Vec10* stats=0, int tid=0) 155 | { 156 | for(int i=min;i((*points)[i],ef,tid); 157 | } 158 | 159 | 160 | 161 | private: 162 | 163 | void stitchDoubleInternal( 164 | MatXX* H, VecX* b, EnergyFunctional const * const EF, bool usePrior, 165 | int min, int max, Vec10* stats, int tid); 166 | }; 167 | } 168 | 169 | -------------------------------------------------------------------------------- /src/bundles/Config.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the EDS: Event-aided Direct Sparse Odometry 3 | * (https://rpg.ifi.uzh.ch/eds.html) 4 | * 5 | * Copyright (c) 2022 Javier Hidalgo-Carrió, Robotics and Perception 6 | * Group (RPG) University of Zurich. 7 | * 8 | * EDS is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, version 3. 11 | * 12 | * EDS is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | 22 | #ifndef _EDS_BUNDLES_CONFIG_HPP_ 23 | #define _EDS_BUNDLES_CONFIG_HPP_ 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | namespace eds { namespace bundles{ 31 | 32 | enum LOSS_FUNCTION{NONE, HUBER, CAUCHY}; 33 | enum LINEAR_SOLVER_TYPE{DENSE_QR, DENSE_SCHUR, SPARSE_SCHUR, SPARSE_NORMAL_CHOLESKY}; 34 | 35 | struct SolverOptions 36 | { 37 | LINEAR_SOLVER_TYPE linear_solver_type; 38 | int num_threads; 39 | int max_num_iterations; 40 | double function_tolerance; 41 | bool minimizer_progress_to_stdout; 42 | }; 43 | 44 | struct Config 45 | { 46 | std::string type; 47 | double percent_points; 48 | double percent_marginalize_vis; 49 | uint16_t window_size; 50 | LOSS_FUNCTION loss_type; 51 | std::vector loss_params; 52 | SolverOptions options; 53 | }; 54 | 55 | struct PBAInfo 56 | { 57 | base::Time time; 58 | double meas_time_ms; 59 | int num_iterations; 60 | double time_seconds; 61 | uint8_t success; 62 | }; 63 | 64 | inline ::eds::bundles::LOSS_FUNCTION selectLoss(const std::string &loss_name) 65 | { 66 | if (loss_name.compare("Huber") == 0) 67 | return eds::bundles::HUBER; 68 | else if (loss_name.compare("Cauchy") == 0) 69 | return eds::bundles::CAUCHY; 70 | else 71 | return eds::bundles::NONE; 72 | }; 73 | 74 | inline ::eds::bundles::LINEAR_SOLVER_TYPE selectSolver(const std::string &solver_name) 75 | { 76 | if (solver_name.compare("DENSE_QR") == 0) 77 | return eds::bundles::DENSE_QR; 78 | else if (solver_name.compare("DENSE_SCHUR") == 0) 79 | return eds::bundles::DENSE_SCHUR; 80 | else if (solver_name.compare("SPARSE_SCHUR") == 0) 81 | return eds::bundles::SPARSE_SCHUR; 82 | else 83 | return eds::bundles::SPARSE_NORMAL_CHOLESKY; 84 | }; 85 | 86 | inline ::eds::bundles::Config readBundlesConfig(YAML::Node config) 87 | { 88 | ::eds::bundles::Config bundles_config; 89 | 90 | /** Number of points to optimize within the current window **/ 91 | bundles_config.percent_points = config["percent_points"].as(); 92 | /** Percent of visual point to seletc the kf to marginalize **/ 93 | bundles_config.percent_marginalize_vis = config["percent_marginalize_vis"].as(); 94 | /** BA Windows size **/ 95 | bundles_config.window_size = config["window_size"].as(); 96 | /** Config for tracker type (only ceres) **/ 97 | bundles_config.type = config["type"].as(); 98 | 99 | /** Config the loss **/ 100 | YAML::Node bundles_loss = config["loss_function"]; 101 | std::string loss_name = bundles_loss["type"].as(); 102 | bundles_config.loss_type = ::eds::bundles::selectLoss(loss_name); 103 | bundles_config.loss_params = bundles_loss["param"].as< std::vector >(); 104 | 105 | /** Config for ceres options **/ 106 | YAML::Node tracker_options = config["options"]; 107 | bundles_config.options.linear_solver_type = ::eds::bundles::selectSolver(tracker_options["solver_type"].as()); 108 | bundles_config.options.num_threads = tracker_options["num_threads"].as(); 109 | bundles_config.options.max_num_iterations = tracker_options["max_num_iterations"].as(); 110 | bundles_config.options.function_tolerance = tracker_options["function_tolerance"].as(); 111 | bundles_config.options.minimizer_progress_to_stdout = tracker_options["minimizer_progress_to_stdout"].as(); 112 | 113 | return bundles_config; 114 | }; 115 | 116 | } //bundles namespace 117 | } // end namespace 118 | 119 | #endif -------------------------------------------------------------------------------- /src/bundles/EnergyFunctional.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | 28 | #include "eds/utils/NumType.h" 29 | #include "eds/utils/IndexThreadReduce.h" 30 | #include "vector" 31 | #include 32 | #include "map" 33 | 34 | 35 | namespace dso 36 | { 37 | 38 | class PointFrameResidual; 39 | class CalibHessian; 40 | class FrameHessian; 41 | class PointHessian; 42 | 43 | 44 | class EFResidual; 45 | class EFPoint; 46 | class EFFrame; 47 | class EnergyFunctional; 48 | class AccumulatedTopHessian; 49 | class AccumulatedTopHessianSSE; 50 | class AccumulatedSCHessian; 51 | class AccumulatedSCHessianSSE; 52 | 53 | 54 | extern bool EFAdjointsValid; 55 | extern bool EFIndicesValid; 56 | extern bool EFDeltaValid; 57 | 58 | 59 | 60 | class EnergyFunctional { 61 | public: 62 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 63 | friend class EFFrame; 64 | friend class EFPoint; 65 | friend class EFResidual; 66 | friend class AccumulatedTopHessian; 67 | friend class AccumulatedTopHessianSSE; 68 | friend class AccumulatedSCHessian; 69 | friend class AccumulatedSCHessianSSE; 70 | 71 | EnergyFunctional(); 72 | ~EnergyFunctional(); 73 | 74 | 75 | EFResidual* insertResidual(PointFrameResidual* r); 76 | EFFrame* insertFrame(FrameHessian* fh, CalibHessian* Hcalib); 77 | EFPoint* insertPoint(PointHessian* ph); 78 | 79 | void dropResidual(EFResidual* r); 80 | void marginalizeFrame(EFFrame* fh); 81 | void removePoint(EFPoint* ph); 82 | 83 | 84 | 85 | void marginalizePointsF(); 86 | void dropPointsF(); 87 | void solveSystemF(int iteration, double lambda, CalibHessian* HCalib); 88 | double calcMEnergyF(); 89 | double calcLEnergyF_MT(); 90 | 91 | 92 | void makeIDX(); 93 | 94 | void setDeltaF(CalibHessian* HCalib); 95 | 96 | void setAdjointsF(CalibHessian* Hcalib); 97 | 98 | std::vector frames; 99 | int nPoints, nFrames, nResiduals; 100 | 101 | MatXX HM; 102 | VecX bM; 103 | 104 | int resInA, resInL, resInM; 105 | MatXX lastHS; 106 | VecX lastbS; 107 | VecX lastX; 108 | std::vector lastNullspaces_forLogging; 109 | std::vector lastNullspaces_pose; 110 | std::vector lastNullspaces_scale; 111 | std::vector lastNullspaces_affA; 112 | std::vector lastNullspaces_affB; 113 | 114 | IndexThreadReduce* red; 115 | 116 | 117 | std::map, 120 | Eigen::aligned_allocator> 121 | > connectivityMap; 122 | 123 | private: 124 | 125 | VecX getStitchedDeltaF() const; 126 | 127 | void resubstituteF_MT(VecX x, CalibHessian* HCalib, bool MT); 128 | void resubstituteFPt(const VecCf &xc, Mat18f* xAd, int min, int max, Vec10* stats, int tid); 129 | 130 | void accumulateAF_MT(MatXX &H, VecX &b, bool MT); 131 | void accumulateLF_MT(MatXX &H, VecX &b, bool MT); 132 | void accumulateSCF_MT(MatXX &H, VecX &b, bool MT); 133 | 134 | void calcLEnergyPt(int min, int max, Vec10* stats, int tid); 135 | 136 | void orthogonalize(VecX* b, MatXX* H); 137 | Mat18f* adHTdeltaF; 138 | 139 | Mat88* adHost; 140 | Mat88* adTarget; 141 | 142 | Mat88f* adHostF; 143 | Mat88f* adTargetF; 144 | 145 | 146 | VecC cPrior; 147 | VecCf cDeltaF; 148 | VecCf cPriorF; 149 | 150 | AccumulatedTopHessianSSE* accSSE_top_L; 151 | AccumulatedTopHessianSSE* accSSE_top_A; 152 | 153 | 154 | AccumulatedSCHessianSSE* accSSE_bot; 155 | 156 | std::vector allPoints; 157 | std::vector allPointsToMarg; 158 | 159 | float currentLambda; 160 | }; 161 | } 162 | 163 | -------------------------------------------------------------------------------- /src/bundles/EnergyFunctionalStructs.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #include "eds/bundles/EnergyFunctionalStructs.h" 26 | #include "eds/bundles/EnergyFunctional.h" 27 | #include "eds/tracking/HessianBlocks.h" 28 | #include "eds/tracking/Residuals.h" 29 | 30 | #if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) 31 | #include "SSE2NEON.h" 32 | #endif 33 | 34 | namespace dso 35 | { 36 | 37 | 38 | void EFResidual::takeDataF() 39 | { 40 | std::swap(J, data->J); 41 | 42 | Vec2f JI_JI_Jd = J->JIdx2 * J->Jpdd; 43 | 44 | for(int i=0;i<6;i++) 45 | JpJdF[i] = J->Jpdxi[0][i]*JI_JI_Jd[0] + J->Jpdxi[1][i] * JI_JI_Jd[1]; 46 | 47 | JpJdF.segment<2>(6) = J->JabJIdx*J->Jpdd; 48 | } 49 | 50 | 51 | void EFFrame::takeData() 52 | { 53 | prior = data->getPrior().head<8>(); 54 | delta = data->get_state_minus_stateZero().head<8>(); 55 | delta_prior = (data->get_state() - data->getPriorZero()).head<8>(); 56 | 57 | 58 | 59 | // Vec10 state_zero = data->get_state_zero(); 60 | // state_zero.segment<3>(0) = SCALE_XI_TRANS * state_zero.segment<3>(0); 61 | // state_zero.segment<3>(3) = SCALE_XI_ROT * state_zero.segment<3>(3); 62 | // state_zero[6] = SCALE_A * state_zero[6]; 63 | // state_zero[7] = SCALE_B * state_zero[7]; 64 | // state_zero[8] = SCALE_A * state_zero[8]; 65 | // state_zero[9] = SCALE_B * state_zero[9]; 66 | // 67 | // std::cout << "state_zero: " << state_zero.transpose() << "\n"; 68 | 69 | 70 | assert(data->frameID != -1); 71 | 72 | frameID = data->frameID; 73 | } 74 | 75 | 76 | 77 | 78 | void EFPoint::takeData() 79 | { 80 | priorF = data->hasDepthPrior ? setting_idepthFixPrior*SCALE_IDEPTH*SCALE_IDEPTH : 0; 81 | if(setting_solverMode & SOLVER_REMOVE_POSEPRIOR) priorF=0; 82 | 83 | deltaF = data->idepth-data->idepth_zero; 84 | } 85 | 86 | 87 | void EFResidual::fixLinearizationF(EnergyFunctional* ef) 88 | { 89 | Vec8f dp = ef->adHTdeltaF[hostIDX+ef->nFrames*targetIDX]; 90 | 91 | // compute Jp*delta 92 | __m128 Jp_delta_x = _mm_set1_ps(J->Jpdxi[0].dot(dp.head<6>()) 93 | +J->Jpdc[0].dot(ef->cDeltaF) 94 | +J->Jpdd[0]*point->deltaF); 95 | __m128 Jp_delta_y = _mm_set1_ps(J->Jpdxi[1].dot(dp.head<6>()) 96 | +J->Jpdc[1].dot(ef->cDeltaF) 97 | +J->Jpdd[1]*point->deltaF); 98 | __m128 delta_a = _mm_set1_ps((float)(dp[6])); 99 | __m128 delta_b = _mm_set1_ps((float)(dp[7])); 100 | 101 | for(int i=0;iresF)+i); 105 | rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JIdx))+i),Jp_delta_x)); 106 | rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JIdx+1))+i),Jp_delta_y)); 107 | rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JabF))+i),delta_a)); 108 | rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JabF+1))+i),delta_b)); 109 | _mm_store_ps(((float*)&res_toZeroF)+i, rtz); 110 | } 111 | 112 | isLinearized = true; 113 | } 114 | 115 | } 116 | -------------------------------------------------------------------------------- /src/bundles/EnergyFunctionalStructs.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | 28 | #include "eds/utils/NumType.h" 29 | #include "eds/bundles/RawResidualJacobian.h" 30 | #include "vector" 31 | #include 32 | 33 | namespace dso 34 | { 35 | 36 | class PointFrameResidual; 37 | class CalibHessian; 38 | class FrameHessian; 39 | class PointHessian; 40 | 41 | class EFResidual; 42 | class EFPoint; 43 | class EFFrame; 44 | class EnergyFunctional; 45 | 46 | 47 | 48 | 49 | 50 | 51 | class EFResidual 52 | { 53 | public: 54 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 55 | 56 | inline EFResidual(PointFrameResidual* org, EFPoint* point_, EFFrame* host_, EFFrame* target_) : 57 | data(org), point(point_), host(host_), target(target_) 58 | { 59 | isLinearized=false; 60 | isActiveAndIsGoodNEW=false; 61 | J = new RawResidualJacobian(); 62 | assert(((long)this)%16==0); 63 | assert(((long)J)%16==0); 64 | } 65 | inline ~EFResidual() 66 | { 67 | delete J; 68 | } 69 | 70 | 71 | void takeDataF(); 72 | 73 | 74 | void fixLinearizationF(EnergyFunctional* ef); 75 | 76 | 77 | // structural pointers 78 | PointFrameResidual* data; 79 | int hostIDX, targetIDX; 80 | EFPoint* point; 81 | EFFrame* host; 82 | EFFrame* target; 83 | int idxInAll; 84 | 85 | RawResidualJacobian* J; 86 | 87 | VecNRf res_toZeroF; 88 | Vec8f JpJdF; 89 | 90 | 91 | // status. 92 | bool isLinearized; 93 | 94 | // if residual is not OOB & not OUTLIER & should be used during accumulations 95 | bool isActiveAndIsGoodNEW; 96 | inline const bool &isActive() const {return isActiveAndIsGoodNEW;} 97 | }; 98 | 99 | 100 | enum EFPointStatus {PS_GOOD=0, PS_MARGINALIZE, PS_DROP}; 101 | 102 | class EFPoint 103 | { 104 | public: 105 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 106 | EFPoint(PointHessian* d, EFFrame* host_) : data(d),host(host_) 107 | { 108 | takeData(); 109 | stateFlag=EFPointStatus::PS_GOOD; 110 | } 111 | void takeData(); 112 | 113 | PointHessian* data; 114 | 115 | 116 | 117 | float priorF; 118 | float deltaF; 119 | 120 | 121 | // constant info (never changes in-between). 122 | int idxInPoints; 123 | EFFrame* host; 124 | 125 | // contains all residuals. 126 | std::vector residualsAll; 127 | 128 | float bdSumF; 129 | float HdiF; 130 | float Hdd_accLF; 131 | VecCf Hcd_accLF; 132 | float bd_accLF; 133 | float Hdd_accAF; 134 | VecCf Hcd_accAF; 135 | float bd_accAF; 136 | 137 | 138 | EFPointStatus stateFlag; 139 | }; 140 | 141 | 142 | 143 | class EFFrame 144 | { 145 | public: 146 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 147 | EFFrame(FrameHessian* d) : data(d) 148 | { 149 | takeData(); 150 | } 151 | void takeData(); 152 | 153 | 154 | Vec8 prior; // prior hessian (diagonal) 155 | Vec8 delta_prior; // = state-state_prior (E_prior = (delta_prior)' * diag(prior) * (delta_prior) 156 | Vec8 delta; // state - state_zero. 157 | 158 | 159 | 160 | std::vector points; 161 | FrameHessian* data; 162 | int idx; // idx in frames. 163 | 164 | int frameID; 165 | }; 166 | 167 | } 168 | 169 | -------------------------------------------------------------------------------- /src/bundles/RawResidualJacobian.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | 28 | #include "eds/utils/NumType.h" 29 | 30 | namespace dso 31 | { 32 | struct RawResidualJacobian 33 | { 34 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 35 | // ================== new structure: save independently =============. 36 | VecNRf resF; 37 | 38 | // the two rows of d[x,y]/d[xi]. 39 | Vec6f Jpdxi[2]; // 2x6 40 | 41 | // the two rows of d[x,y]/d[C]. 42 | VecCf Jpdc[2]; // 2x4 43 | 44 | // the two rows of d[x,y]/d[idepth]. 45 | Vec2f Jpdd; // 2x1 46 | 47 | // the two columns of d[r]/d[x,y]. 48 | VecNRf JIdx[2]; // 9x2 49 | 50 | // = the two columns of d[r] / d[ab] 51 | VecNRf JabF[2]; // 9x2 52 | 53 | 54 | // = JIdx^T * JIdx (inner product). Only as a shorthand. 55 | Mat22f JIdx2; // 2x2 56 | // = Jab^T * JIdx (inner product). Only as a shorthand. 57 | Mat22f JabJIdx; // 2x2 58 | // = Jab^T * Jab (inner product). Only as a shorthand. 59 | Mat22f Jab2; // 2x2 60 | 61 | }; 62 | } 63 | 64 | -------------------------------------------------------------------------------- /src/eds.pc.in: -------------------------------------------------------------------------------- 1 | prefix=@CMAKE_INSTALL_PREFIX@ 2 | exec_prefix=@CMAKE_INSTALL_PREFIX@ 3 | libdir=${prefix}/lib 4 | includedir=${prefix}/include 5 | 6 | Name: @TARGET_NAME@ 7 | Description: @PROJECT_DESCRIPTION@ 8 | Version: @PROJECT_VERSION@ 9 | Requires: @PKGCONFIG_REQUIRES@ 10 | Libs: -L${libdir} -l@TARGET_NAME@ @PKGCONFIG_LIBS@ 11 | Cflags: -I${includedir} @PKGCONFIG_CFLAGS@ 12 | 13 | -------------------------------------------------------------------------------- /src/io/ImageConvert.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the EDS: Event-aided Direct Sparse Odometry 3 | * (https://rpg.ifi.uzh.ch/eds.html) 4 | * 5 | * Copyright (c) 2022 Javier Hidalgo-Carrió, Robotics and Perception 6 | * Group (RPG) University of Zurich. 7 | * 8 | * EDS is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, version 3. 11 | * 12 | * EDS is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #include 22 | #include 23 | 24 | namespace eds { namespace io 25 | { 26 | 27 | dso::MinimalImageB3* toMinimalImageB3(const Eigen::Vector3f *fd, const int &w, const int &h) 28 | { 29 | int wh = w*h; 30 | dso::MinimalImageB3* img_b3 = new dso::MinimalImageB3(w, h); 31 | for(int i=0;i255) c=255; 35 | img_b3->at(i) = ::dso::Vec3b(c,c,c); 36 | } 37 | return img_b3; 38 | } 39 | 40 | void MinimalImageB3ToFrame(const dso::MinimalImageB3 *img_b3, const ::base::Time ×tamp, ::base::samples::frame::Frame &frame) 41 | { 42 | cv::Mat img = cv::Mat(img_b3->h, img_b3->w, CV_8UC3, img_b3->data); 43 | //cv::imwrite("/tmp/dso_img_"+std::to_string(this->frame_idx)+".png", frame); 44 | 45 | frame.image.clear(); 46 | frame_helper::FrameHelper::copyMatToFrame(img, frame); 47 | frame.time = timestamp; 48 | 49 | } 50 | 51 | } 52 | }//end of dso namespace 53 | -------------------------------------------------------------------------------- /src/io/ImageConvert.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the EDS: Event-aided Direct Sparse Odometry 3 | * (https://rpg.ifi.uzh.ch/eds.html) 4 | * 5 | * Copyright (c) 2022 Javier Hidalgo-Carrió, Robotics and Perception 6 | * Group (RPG) University of Zurich. 7 | * 8 | * EDS is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, version 3. 11 | * 12 | * EDS is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #pragma once 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | 29 | namespace eds 30 | { 31 | 32 | namespace io 33 | { 34 | 35 | dso::MinimalImageB3* toMinimalImageB3(const Eigen::Vector3f *fd, const int &w, const int &h); 36 | void MinimalImageB3ToFrame(const dso::MinimalImageB3 *input, const ::base::Time ×tamp, ::base::samples::frame::Frame &frame); 37 | 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /src/io/ImageRW.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 27 | #include 28 | 29 | 30 | namespace dso 31 | { 32 | 33 | namespace io 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/io/ImageRW.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | 26 | #pragma once 27 | #include "eds/utils/NumType.h" 28 | #include "eds/utils/MinimalImage.h" 29 | 30 | namespace dso 31 | { 32 | namespace io 33 | { 34 | 35 | MinimalImageB* readImageBW_8U(std::string filename); 36 | MinimalImageB3* readImageRGB_8U(std::string filename); 37 | MinimalImage* readImageBW_16U(std::string filename); 38 | 39 | 40 | MinimalImageB* readStreamBW_8U(char* data, int numBytes); 41 | 42 | void writeImage(std::string filename, MinimalImageB* img); 43 | void writeImage(std::string filename, MinimalImageB3* img); 44 | void writeImage(std::string filename, MinimalImageF* img); 45 | void writeImage(std::string filename, MinimalImageF3* img); 46 | 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /src/io/OutputMaps.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the EDS: Event-aided Direct Sparse Odometry 3 | * (https://rpg.ifi.uzh.ch/eds.html) 4 | * 5 | * Copyright (c) 2022 Javier Hidalgo-Carrió, Robotics and Perception 6 | * Group (RPG) University of Zurich. 7 | * 8 | * EDS is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, version 3. 11 | * 12 | * EDS is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see 19 | */ 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | namespace dso { namespace io 30 | { 31 | 32 | template 33 | bool is_not_nan(const Eigen::MatrixBase& x) 34 | { 35 | return ((x.array() == x.array())).all(); 36 | } 37 | 38 | base::samples::Pointcloud getMap(const dso::FrameHessian *fh, dso::CalibHessian *hcalib, const base::Vector4d color, const bool &single_point) 39 | { 40 | base::samples::Pointcloud pcl; 41 | 42 | float fx = hcalib->fxl(); 43 | float fy = hcalib->fyl(); 44 | float cx = hcalib->cxl(); 45 | float cy = hcalib->cyl(); 46 | 47 | base::Transform3d T_w_kf = dso::SE3ToBaseTransform(fh->get_worldToCam_evalPT()).inverse(); 48 | 49 | for(::dso::PointHessian* p : fh->pointHessians) 50 | { 51 | double d_i = 1.0/p->idepth_scaled; 52 | if (is_not_nan(color)) 53 | { 54 | /** Push points in the world frame coordinate **/ 55 | pcl.points.push_back(T_w_kf * ::base::Point(d_i*(p->u-cx)/fx, d_i*(p->v-cy)/fy, d_i)); 56 | pcl.colors.push_back(color);//point intensity 57 | } 58 | else 59 | { 60 | pcl.points.push_back(T_w_kf * ::base::Point(d_i*(p->u-cx)/fx, d_i*(p->v-cy)/fy, d_i));//u-v point8 61 | //pcl.colors.push_back(::base::Vector4d(p->color[7], p->color[7], p->color[7], 1.0));//point intensity 62 | } 63 | 64 | if (!single_point) 65 | { 66 | pcl.points.push_back(T_w_kf * ::base::Point(d_i*(p->u-cx)/fx, d_i*(p->v+2-cy)/fy, d_i));//x-y point1 67 | if (is_not_nan(color)) pcl.colors.push_back(color);//point intensity 68 | //pcl.colors.push_back(::base::Vector4d(p->color[0], p->color[0], p->color[0], 1.0));//point intensity 69 | pcl.points.push_back(T_w_kf * ::base::Point(d_i*(p->u-1-cx)/fx, d_i*(p->v+1-cy)/fy, d_i ));//x-y point2 70 | if (is_not_nan(color)) pcl.colors.push_back(color);//point intensity 71 | //pcl.colors.push_back(::base::Vector4d(p->color[1], p->color[1], p->color[1], 1.0));//point intensity 72 | pcl.points.push_back(T_w_kf * ::base::Point(d_i*(p->u-2-cx)/fx, d_i*(p->v-cy)/fy, d_i ));//x-y point3 73 | if (is_not_nan(color)) pcl.colors.push_back(color);//point intensity 74 | //pcl.colors.push_back(::base::Vector4d(p->color[2], p->color[2], p->color[2], 1.0));//point intensity 75 | pcl.points.push_back(T_w_kf * ::base::Point(d_i*(p->u-1-cx)/fx, d_i*(p->v-1-cy)/fy, d_i ));//x-y point4 76 | if (is_not_nan(color)) pcl.colors.push_back(color);//point intensity 77 | //pcl.colors.push_back(::base::Vector4d(p->color[3], p->color[3], p->color[3], 1.0));//point intensity 78 | pcl.points.push_back(T_w_kf * ::base::Point(d_i*(p->u-cx)/fx, d_i*(p->v-2-cy)/fy, d_i ));//x-y point5 79 | if (is_not_nan(color)) pcl.colors.push_back(color);//point intensity 80 | //pcl.colors.push_back(::base::Vector4d(p->color[4], p->color[4], p->color[4], 1.0));//point intensity 81 | pcl.points.push_back(T_w_kf * ::base::Point(d_i*(p->u+1-cx)/fx, d_i*(p->v-1-cy)/fy, d_i ));//x-y point6 82 | if (is_not_nan(color)) pcl.colors.push_back(color);//point intensity 83 | //pcl.colors.push_back(::base::Vector4d(p->color[5], p->color[5], p->color[5], 1.0));//point intensity 84 | pcl.points.push_back(T_w_kf * ::base::Point(d_i*(p->u+2-cx)/fx, d_i*(p->v-cy)/fy, d_i ));//x-y point7 85 | if (is_not_nan(color)) pcl.colors.push_back(color);//point intensity 86 | //pcl.colors.push_back(::base::Vector4d(p->color[6], p->color[6], p->color[6], 1.0));//point intensity 87 | } 88 | } 89 | 90 | return pcl; 91 | } 92 | 93 | base::samples::Pointcloud getImmatureMap(const dso::FrameHessian *fh, dso::CalibHessian *hcalib, const bool &single_point) 94 | { 95 | base::samples::Pointcloud pcl; 96 | 97 | float fx = hcalib->fxl(); 98 | float fy = hcalib->fyl(); 99 | float cx = hcalib->cxl(); 100 | float cy = hcalib->cyl(); 101 | 102 | base::Transform3d T_w_kf = dso::SE3ToBaseTransform(fh->get_worldToCam_evalPT()).inverse(); 103 | 104 | for(::dso::ImmaturePoint* p : fh->immaturePoints) 105 | { 106 | /** This is the depth. Same maner as Hessian points are initialized from Immature Points **/ 107 | double d_i = 1.0/(0.5 * (p->idepth_max+p->idepth_min)); 108 | 109 | if (d_i < 0 || !std::isfinite(d_i)) continue; 110 | 111 | ::base::Vector4d color; 112 | if(p->lastTraceStatus==dso::ImmaturePointStatus::IPS_GOOD) 113 | color = ::base::Vector4d(0,1.0,0, 1.0);//GREEN 114 | if(p->lastTraceStatus==dso::ImmaturePointStatus::IPS_OOB) 115 | color = ::base::Vector4d(1.0,0,0,1.0);//RED 116 | if(p->lastTraceStatus==dso::ImmaturePointStatus::IPS_OUTLIER) 117 | color = ::base::Vector4d(0,0,1.0,1.0);//BLUE 118 | if(p->lastTraceStatus==dso::ImmaturePointStatus::IPS_SKIPPED) 119 | color = ::base::Vector4d(0.0,1.0,1.0,1.0);//CYAN 120 | if(p->lastTraceStatus==dso::ImmaturePointStatus::IPS_BADCONDITION) 121 | color = ::base::Vector4d(1.0,1.0,1.0,1.0); //WHITE 122 | if(p->lastTraceStatus==dso::ImmaturePointStatus::IPS_UNINITIALIZED) 123 | color = ::base::Vector4d(0,0,0,1.0);//BLACK 124 | 125 | pcl.points.push_back(T_w_kf * ::base::Point(d_i*(p->u-cx)/fx, d_i*(p->v-cy)/fy, d_i));//u-v point8 126 | pcl.colors.push_back(color);//point intensity 127 | 128 | if (!single_point) 129 | { 130 | pcl.points.push_back(T_w_kf * ::base::Point(d_i*(p->u-cx)/fx, d_i*(p->v+2-cy)/fy, d_i));//x-y point1 131 | pcl.colors.push_back(color);//point intensity 132 | pcl.points.push_back(T_w_kf * ::base::Point(d_i*(p->u-1-cx)/fx, d_i*(p->v+1-cy)/fy, d_i ));//x-y point2 133 | pcl.colors.push_back(color);//point intensity 134 | pcl.points.push_back(T_w_kf * ::base::Point(d_i*(p->u-2-cx)/fx, d_i*(p->v-cy)/fy, d_i ));//x-y point3 135 | pcl.colors.push_back(color);//point intensity 136 | pcl.points.push_back(T_w_kf * ::base::Point(d_i*(p->u-1-cx)/fx, d_i*(p->v-1-cy)/fy, d_i ));//x-y point4 137 | pcl.colors.push_back(color);//point intensity 138 | pcl.points.push_back(T_w_kf * ::base::Point(d_i*(p->u-cx)/fx, d_i*(p->v-2-cy)/fy, d_i ));//x-y point5 139 | pcl.colors.push_back(color);//point intensity 140 | pcl.points.push_back(T_w_kf * ::base::Point(d_i*(p->u+1-cx)/fx, d_i*(p->v-1-cy)/fy, d_i ));//x-y point6 141 | pcl.colors.push_back(color);//point intensity 142 | pcl.points.push_back(T_w_kf * ::base::Point(d_i*(p->u+2-cx)/fx, d_i*(p->v-cy)/fy, d_i ));//x-y point7 143 | pcl.colors.push_back(color);//point intensity 144 | } 145 | } 146 | 147 | return pcl; 148 | } 149 | 150 | } 151 | }//end of dso namespace 152 | -------------------------------------------------------------------------------- /src/io/OutputMaps.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the EDS: Event-aided Direct Sparse Odometry 3 | * (https://rpg.ifi.uzh.ch/eds.html) 4 | * 5 | * Copyright (c) 2022 Javier Hidalgo-Carrió, Robotics and Perception 6 | * Group (RPG) University of Zurich. 7 | * 8 | * EDS is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, version 3. 11 | * 12 | * EDS is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see 19 | */ 20 | 21 | #pragma once 22 | #include 23 | #include 24 | #include 25 | 26 | /** Rock base types **/ 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | namespace cv { 36 | class Mat; 37 | } 38 | 39 | namespace dso 40 | { 41 | 42 | class FrameHessian; 43 | class CalibHessian; 44 | class FrameShell; 45 | 46 | namespace io 47 | { 48 | 49 | base::samples::Pointcloud getMap(const dso::FrameHessian *fh, dso::CalibHessian *hcalib, 50 | const base::Vector4d color= ::base::Vector4d(::base::NaN(), ::base::NaN(), 51 | ::base::NaN(), ::base::NaN()), const bool &single_point = true); 52 | base::samples::Pointcloud getImmatureMap(const dso::FrameHessian *fh, dso::CalibHessian *hcalib, const bool &single_point = true); 53 | 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /src/mapping/Config.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the EDS: Event-aided Direct Sparse Odometry 3 | * (https://rpg.ifi.uzh.ch/eds.html) 4 | * 5 | * Copyright (c) 2022 Javier Hidalgo-Carrió, Robotics and Perception 6 | * Group (RPG) University of Zurich. 7 | * 8 | * EDS is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, version 3. 11 | * 12 | * EDS is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see 19 | */ 20 | 21 | #ifndef _EDS_MAPPING_CONFIG_HPP_ 22 | #define _EDS_MAPPING_CONFIG_HPP_ 23 | 24 | #include 25 | #include 26 | 27 | namespace eds { namespace mapping{ 28 | 29 | struct Config 30 | { 31 | double min_depth; 32 | double max_depth; 33 | double convergence_sigma2_thresh; 34 | bool sor_active; 35 | uint16_t sor_nb_points; 36 | double sor_radius; 37 | int num_desired_points; 38 | float points_rel_baseline; 39 | }; 40 | 41 | inline ::eds::mapping::Config readMappingConfig(YAML::Node config) 42 | { 43 | ::eds::mapping::Config mapping_config; 44 | 45 | mapping_config.min_depth = config["min_depth"].as(); 46 | if (mapping_config.min_depth < 0) mapping_config.min_depth= 1e0-6; 47 | mapping_config.max_depth = config["max_depth"].as(); 48 | if (mapping_config.max_depth < 0) mapping_config.max_depth= 1e0-6; 49 | mapping_config.convergence_sigma2_thresh = config["convergence_sigma2_thresh"].as(); 50 | if (mapping_config.convergence_sigma2_thresh < 0) mapping_config.convergence_sigma2_thresh= 10.0; 51 | YAML::Node sor_config = config["sor"]; 52 | mapping_config.sor_active = sor_config["active"].as(); 53 | mapping_config.sor_nb_points = sor_config["nb_points"].as(); 54 | mapping_config.sor_radius = sor_config["radius"].as(); 55 | if (config["num_desired_points"]) mapping_config.num_desired_points = config["num_desired_points"].as(); 56 | else mapping_config.num_desired_points = 2000; 57 | if (config["points_rel_baseline"]) mapping_config.points_rel_baseline = config["points_rel_baseline"].as(); 58 | else mapping_config.points_rel_baseline = 0.1; 59 | 60 | return mapping_config; 61 | }; 62 | 63 | } //mapping namespace 64 | } // end namespace 65 | 66 | #endif -------------------------------------------------------------------------------- /src/mapping/DepthPoints.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the EDS: Event-aided Direct Sparse Odometry 3 | * (https://rpg.ifi.uzh.ch/eds.html) 4 | * 5 | * Copyright (c) 2022 Javier Hidalgo-Carrió, Robotics and Perception 6 | * Group (RPG) University of Zurich. 7 | * 8 | * EDS is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, version 3. 11 | * 12 | * EDS is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #ifndef _EDS_MAPPING_DEPTH_POINTS_HPP_ 22 | #define _EDS_MAPPING_DEPTH_POINTS_HPP_ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | 30 | namespace eds { namespace mapping { 31 | 32 | enum DEPTH_FILTER{VOGIATZIS, GAUSS}; 33 | 34 | class DepthPoints 35 | { 36 | public: 37 | static constexpr double px_noise = 3.0; 38 | typedef typename Eigen::Vector4d data_type; 39 | typedef typename std::vector vector_type; 40 | typedef typename vector_type::iterator iterator; 41 | typedef typename vector_type::const_iterator const_iterator; 42 | 43 | private: 44 | /** Intrinsic camera matrix **/ 45 | cv::Mat K_; double fx_, fy_, cx_, cy_; 46 | /** Depth mean range **/ 47 | double mu_range, seed_mu_range; 48 | /** Converge sigma2 threshold **/ 49 | double convergence_sigma2_thresh; 50 | /** Pixel error angle **/ 51 | double px_error_angle; 52 | /* Mu mean inv depth **/ 53 | vector_type data; //vector of 4x1 elements [mu, sigma2, a, b] 54 | 55 | public: 56 | /** @brief Default constructor */ 57 | DepthPoints(); 58 | 59 | /** @brief Default constructor */ 60 | DepthPoints(const cv::Mat &K, const uint32_t &num_points, const double &min_depth, const double &max_depth, const double &threshold=100, 61 | const double &init_a=2.0, const double &init_b=5.0); 62 | 63 | /** @brief Default constructor */ 64 | DepthPoints(const cv::Mat &K, const std::vector &inv_depth, const double &min_depth, const double &max_depth, const double &threshold=100, 65 | const double &init_a=2.0, const double &init_b=5.0); 66 | 67 | /** Initialization **/ 68 | void init(const cv::Mat &K, const uint32_t &num_points, const double &min_depth, const double &max_depth, const double &threshold=100, 69 | const double &init_a=2.0, const double &init_b=5.0); 70 | 71 | /** Initialization **/ 72 | void init(const cv::Mat &K, const std::vector &inv_depth, const double &min_depth, const double &max_depth, const double &threshold=100, 73 | const double &init_a=2.0, const double &init_b=5.0); 74 | 75 | /** Update method given event frame coordinates **/ 76 | void update(const ::base::Transform3d &T_kf_ef, const std::vector &kf_coord, const std::vector &ef_coord, 77 | const eds::mapping::DEPTH_FILTER &filter = VOGIATZIS); 78 | 79 | /** Update method given points tracks **/ 80 | void update(const ::base::Transform3d &T_kf_ef, const std::vector &kf_coord, const std::vector &tracks, 81 | const eds::mapping::DEPTH_FILTER &filter = VOGIATZIS); 82 | 83 | /** Vogiatzis filter **/ 84 | bool filterVogiatzis(const double &z, const double &tau2, const double &mu_range, data_type &state); 85 | 86 | /** Get mu inverse depth **/ 87 | void getIDepth(std::vector &x); 88 | 89 | /** Return inverse depth **/ 90 | std::vector getIDepth(); 91 | 92 | /** Median and Q_3 (third quantile) inverse depth **/ 93 | void meanIDepth(double &mean, double &st_dev); 94 | 95 | /** Median and Q_3 (third quantile) inverse depth **/ 96 | void medianIDepth(double &median, double &third_q); 97 | 98 | double depthRange(){return this->mu_range;}; 99 | cv::Mat &K(){return this->K_;}; 100 | double &fx(){return this->fx_;}; 101 | double &fy(){return this->fy_;}; 102 | double &cx(){return this->cx_;}; 103 | double &cy(){return this->cy_;}; 104 | 105 | /** Overloading [] operator to access elements in array style **/ 106 | data_type& operator[](size_t); 107 | 108 | /** Size with number of points **/ 109 | size_t size(){return this->data.size();}; 110 | bool empty(){return this->data.empty();}; 111 | inline iterator begin() noexcept {return this->data.begin();}; 112 | inline const_iterator cbegin() const noexcept {return this->data.cbegin();}; 113 | inline iterator end() noexcept {return this->data.end();}; 114 | inline const_iterator cend() const noexcept {return this->data.cend();}; 115 | inline iterator erase(const int &idx){return this->data.erase(this->data.begin()+idx);}; 116 | inline iterator erase(DepthPoints::iterator &it) {return this->data.erase(it);}; 117 | 118 | /** Visualize the points sigma in a given image **/ 119 | cv::Mat sigmaViz(const cv::Mat &img, const std::vector &coord, double &min_sigma, double &max_sigma); 120 | 121 | /** Visualize the points convergence in a given image **/ 122 | cv::Mat convergenceViz(const cv::Mat &img, const std::vector &coord); 123 | 124 | private: 125 | /* 126 | * \param[in] pox1 3x1 matrix: homogeneous coords of the 2D point in image 1 (Key Frame) 127 | * \param[in] pox2 3x1 matrix: homogeneous coords of the 2D point in image 2 (Event Frame) 128 | * \param[in] poP1 3x4 projection matrix for image 1 (Key Frame). Euclidean (finite) camera! 129 | * \param[in] poP2 3x4 projection matrix for image 2 (Event Frame). Euclidean (finite) camera! 130 | * \param[out] poX3d 4x1 matrix: homogeneous coordinates of 3D point 131 | * 132 | * \warning It only works for Euclidean camera matrices 133 | * \see http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/FUSIELLO4/tutorial.html#x1-15002r24 134 | */ 135 | void linTriangTwoPointsEucl(const cv::Mat& ox1, const cv::Mat& ox2, 136 | const cv::Mat& oP1, const cv::Mat& oP2, cv::Mat& oX3d); 137 | 138 | /* 139 | * \param[in] pox1 3x1 matrix: homogeneous coords of the 2D point in image 1 (Key Frame) 140 | * \param[in] pox2 3x1 matrix: homogeneous coords of the 2D point in image 2 (Event Frame) 141 | * \param[in] poP1 3x4 projection matrix for image 1 (Key Frame). Euclidean (finite) camera! 142 | * \param[in] poP2 3x4 projection matrix for image 2 (Event Frame). Euclidean (finite) camera! 143 | * \param[out] inv_depth double inverse depth of the 3D point in camera 1 (Key Frame) 144 | * 145 | * \warning It only works for Euclidean camera matrices 146 | * \see http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/FUSIELLO4/tutorial.html#x1-15002r24 147 | */ 148 | void invDepthTwoPointsEucl(const cv::Mat& ox1, const cv::Mat& ox2, 149 | const cv::Mat& oP1, const cv::Mat& oP2, double& inv_depth); 150 | 151 | double getAngleError(double img_err) const 152 | { 153 | return std::atan(img_err/(2.0*this->fx_)) + std::atan(img_err/(2.0*this->fy_)); 154 | }; 155 | 156 | 157 | double computeTau( const ::base::Transform3d& T_ref_cur, 158 | const Eigen::Vector2d& x_norm, 159 | const double z, 160 | const double px_error_angle) 161 | { 162 | const Eigen::Vector3d& t = T_ref_cur.translation(); 163 | Eigen::Vector3d x_bearing (x_norm[0], x_norm[1], 1.0); 164 | x_bearing /= x_bearing.norm(); //unit norm vector 165 | const Eigen::Vector3d a = x_bearing*z-t; 166 | double t_norm = t.norm(); 167 | double a_norm = a.norm(); 168 | double alpha = std::acos(x_bearing.dot(t)/t_norm); // dot product 169 | double beta = std::acos(a.dot(-t)/(t_norm*a_norm)); // dot product 170 | double beta_plus = beta + px_error_angle; 171 | double gamma_plus = M_PI-alpha-beta_plus; // triangle angles sum to PI 172 | double z_plus = t_norm*std::sin(beta_plus)/std::sin(gamma_plus); // law of sines 173 | return (z_plus - z); // tau 174 | }; 175 | 176 | inline double getSigma2FromDepthSigma(const double &depth, const double &depth_sigma) 177 | { 178 | const double sigma = 0.5 * (1.0 / std::max(0.000000000001, depth - depth_sigma) 179 | - 1.0 / (depth + depth_sigma)); 180 | return sigma * sigma; 181 | }; 182 | 183 | inline bool isConverged(const Eigen::Ref& mu_sigma2_a_b, 184 | double mu_range, 185 | double sigma2_convergence_threshold) 186 | { 187 | /** If initial uncertainty was reduced by factor sigma2_convergence_threshold 188 | we accept the seed as converged. **/ 189 | const double thresh = mu_range / sigma2_convergence_threshold; 190 | return (mu_sigma2_a_b(1) < thresh * thresh); 191 | }; 192 | }; 193 | 194 | inline double mu(const Eigen::Ref& mu_sigma2_a_b) 195 | { 196 | return mu_sigma2_a_b(0); 197 | } 198 | 199 | inline double sigma2(const Eigen::Ref& mu_sigma2_a_b) 200 | { 201 | return mu_sigma2_a_b(1); 202 | } 203 | 204 | inline double a(const Eigen::Ref& mu_sigma2_a_b) 205 | { 206 | return mu_sigma2_a_b(2); 207 | } 208 | 209 | inline double b(const Eigen::Ref& mu_sigma2_a_b) 210 | { 211 | return mu_sigma2_a_b(3); 212 | } 213 | 214 | inline double& mu(DepthPoints::data_type& mu_sigma2_a_b) 215 | { 216 | return mu_sigma2_a_b[0]; 217 | } 218 | 219 | inline double& sigma2(DepthPoints::data_type& mu_sigma2_a_b) 220 | { 221 | return mu_sigma2_a_b(1); 222 | } 223 | 224 | inline double& a(DepthPoints::data_type& mu_sigma2_a_b) 225 | { 226 | return mu_sigma2_a_b(2); 227 | } 228 | 229 | inline double& b(DepthPoints::data_type& mu_sigma2_a_b) 230 | { 231 | return mu_sigma2_a_b(3); 232 | } 233 | 234 | inline double getMeanRangeFromDepthMinMax(double depth_min, double depth_max) 235 | { 236 | return 1.0 / depth_min; 237 | } 238 | 239 | 240 | } // mapping namespace 241 | } // end namespace 242 | #endif // _EDS_MAPPING_DEPTH_POINTS_HPP_ 243 | 244 | -------------------------------------------------------------------------------- /src/mapping/PixelSelector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | #include "eds/utils/NumType.h" 28 | 29 | namespace dso 30 | { 31 | 32 | enum PixelSelectorStatus {PIXSEL_VOID=0, PIXSEL_1, PIXSEL_2, PIXSEL_3}; 33 | 34 | 35 | class FrameHessian; 36 | 37 | class PixelSelector 38 | { 39 | public: 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 41 | int makeMaps( 42 | const FrameHessian* const fh, 43 | float* map_out, float density, int recursionsLeft=1, bool plot=false, float thFactor=1); 44 | 45 | PixelSelector(int w, int h); 46 | ~PixelSelector(); 47 | int currentPotential; 48 | 49 | 50 | bool allowFast; 51 | void makeHists(const FrameHessian* const fh); 52 | private: 53 | 54 | Eigen::Vector3i select(const FrameHessian* const fh, 55 | float* map_out, int pot, float thFactor=1); 56 | 57 | 58 | unsigned char* randomPattern; 59 | 60 | 61 | int* gradHist; 62 | float* ths; 63 | float* thsSmoothed; 64 | int thsStep; 65 | const FrameHessian* gradHistFrame; 66 | }; 67 | 68 | 69 | 70 | 71 | } 72 | 73 | -------------------------------------------------------------------------------- /src/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/tracking/CoarseTracker.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | 28 | #include "vector" 29 | #include 30 | #include "eds/utils/NumType.h" 31 | #include "eds/utils/settings.h" 32 | #include "eds/bundles/MatrixAccumulators.h" 33 | 34 | namespace dso 35 | { 36 | struct CalibHessian; 37 | struct FrameHessian; 38 | struct PointHessian; 39 | struct PointFrameResidual; 40 | 41 | class CoarseTracker { 42 | public: 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 44 | 45 | CoarseTracker(int w, int h); 46 | ~CoarseTracker(); 47 | 48 | bool trackNewestCoarse( 49 | FrameHessian* newFrameHessian, 50 | SE3 &lastToNew_out, AffLight &aff_g2l_out, 51 | int coarsestLvl, Vec5 minResForAbort); 52 | 53 | void setCoarseTrackingRef( 54 | std::vector frameHessians); 55 | 56 | void makeK( 57 | CalibHessian* HCalib); 58 | 59 | bool debugPrint, debugPlot; 60 | 61 | Mat33f K[PYR_LEVELS]; 62 | Mat33f Ki[PYR_LEVELS]; 63 | float fx[PYR_LEVELS]; 64 | float fy[PYR_LEVELS]; 65 | float fxi[PYR_LEVELS]; 66 | float fyi[PYR_LEVELS]; 67 | float cx[PYR_LEVELS]; 68 | float cy[PYR_LEVELS]; 69 | float cxi[PYR_LEVELS]; 70 | float cyi[PYR_LEVELS]; 71 | int w[PYR_LEVELS]; 72 | int h[PYR_LEVELS]; 73 | 74 | FrameHessian* lastRef; 75 | AffLight lastRef_aff_g2l; 76 | FrameHessian* newFrame; 77 | int refFrameID; 78 | 79 | // act as pure ouptut 80 | Vec5 lastResiduals; 81 | Vec3 lastFlowIndicators; 82 | double firstCoarseRMSE; 83 | private: 84 | 85 | 86 | void makeCoarseDepthL0(std::vector frameHessians); 87 | float* idepth[PYR_LEVELS]; 88 | float* weightSums[PYR_LEVELS]; 89 | float* weightSums_bak[PYR_LEVELS]; 90 | 91 | 92 | Vec6 calcResAndGS(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH); 93 | Vec6 calcRes(int lvl, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH); 94 | void calcGSSSE(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l); 95 | void calcGS(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l); 96 | 97 | // pc buffers 98 | float* pc_u[PYR_LEVELS]; 99 | float* pc_v[PYR_LEVELS]; 100 | float* pc_idepth[PYR_LEVELS]; 101 | float* pc_color[PYR_LEVELS]; 102 | int pc_n[PYR_LEVELS]; 103 | 104 | // warped buffers 105 | float* buf_warped_idepth; 106 | float* buf_warped_u; 107 | float* buf_warped_v; 108 | float* buf_warped_dx; 109 | float* buf_warped_dy; 110 | float* buf_warped_residual; 111 | float* buf_warped_weight; 112 | float* buf_warped_refColor; 113 | int buf_warped_n; 114 | 115 | 116 | std::vector ptrToDelete; 117 | 118 | 119 | Accumulator9 acc; 120 | }; 121 | 122 | 123 | class CoarseDistanceMap { 124 | public: 125 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 126 | 127 | CoarseDistanceMap(int w, int h); 128 | ~CoarseDistanceMap(); 129 | 130 | void makeDistanceMap( 131 | std::vector frameHessians, 132 | FrameHessian* frame); 133 | 134 | void makeInlierVotes( 135 | std::vector frameHessians); 136 | 137 | void makeK( CalibHessian* HCalib); 138 | 139 | 140 | float* fwdWarpedIDDistFinal; 141 | 142 | Mat33f K[PYR_LEVELS]; 143 | Mat33f Ki[PYR_LEVELS]; 144 | float fx[PYR_LEVELS]; 145 | float fy[PYR_LEVELS]; 146 | float fxi[PYR_LEVELS]; 147 | float fyi[PYR_LEVELS]; 148 | float cx[PYR_LEVELS]; 149 | float cy[PYR_LEVELS]; 150 | float cxi[PYR_LEVELS]; 151 | float cyi[PYR_LEVELS]; 152 | int w[PYR_LEVELS]; 153 | int h[PYR_LEVELS]; 154 | 155 | void addIntoDistFinal(int u, int v); 156 | 157 | 158 | private: 159 | 160 | PointFrameResidual** coarseProjectionGrid; 161 | int* coarseProjectionGridNum; 162 | Eigen::Vector2i* bfsList1; 163 | Eigen::Vector2i* bfsList2; 164 | 165 | void growDistBFS(int bfsNum); 166 | }; 167 | 168 | } 169 | 170 | -------------------------------------------------------------------------------- /src/tracking/Config.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the EDS: Event-aided Direct Sparse Odometry 3 | * (https://rpg.ifi.uzh.ch/eds.html) 4 | * 5 | * Copyright (c) 2022 Javier Hidalgo-Carrió, Robotics and Perception 6 | * Group (RPG) University of Zurich. 7 | * 8 | * EDS is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, version 3. 11 | * 12 | * EDS is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #ifndef _EDS_TRACKING_CONFIG_HPP_ 22 | #define _EDS_TRACKING_CONFIG_HPP_ 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include // std::setprecision() 33 | #include 34 | 35 | namespace eds { namespace tracking{ 36 | 37 | enum LOSS_FUNCTION{NONE, HUBER, CAUCHY}; 38 | enum LINEAR_SOLVER_TYPE{DENSE_QR, DENSE_SCHUR, SPARSE_SCHUR, SPARSE_NORMAL_CHOLESKY}; 39 | enum BOOTSTRAP_TYPE{EIGHT_POINTS, MiDAS}; 40 | 41 | struct SolverOptions 42 | { 43 | LINEAR_SOLVER_TYPE linear_solver_type; 44 | int num_threads; 45 | std::vector max_num_iterations; 46 | double function_tolerance; 47 | bool minimizer_progress_to_stdout; 48 | }; 49 | 50 | struct Config 51 | { 52 | double percent_points; 53 | std::string type; 54 | LOSS_FUNCTION loss_type; 55 | std::vector loss_params; 56 | SolverOptions options; 57 | BOOTSTRAP_TYPE bootstrap; 58 | }; 59 | 60 | struct TrackerInfo 61 | { 62 | base::Time time; 63 | double meas_time_us; 64 | uint32_t num_points; 65 | int num_iterations; 66 | double time_seconds; 67 | uint8_t success; 68 | }; 69 | 70 | inline ::eds::tracking::LOSS_FUNCTION selectLoss(const std::string &loss_name) 71 | { 72 | if (loss_name.compare("Huber") == 0) 73 | return eds::tracking::HUBER; 74 | else if (loss_name.compare("Cauchy") == 0) 75 | return eds::tracking::CAUCHY; 76 | else 77 | return eds::tracking::NONE; 78 | }; 79 | 80 | inline ::eds::tracking::LINEAR_SOLVER_TYPE selectSolver(const std::string &solver_name) 81 | { 82 | if (solver_name.compare("DENSE_QR") == 0) 83 | return eds::tracking::DENSE_QR; 84 | else if (solver_name.compare("DENSE_SCHUR") == 0) 85 | return eds::tracking::DENSE_SCHUR; 86 | else if (solver_name.compare("SPARSE_SCHUR") == 0) 87 | return eds::tracking::SPARSE_SCHUR; 88 | else 89 | return eds::tracking::SPARSE_NORMAL_CHOLESKY; 90 | }; 91 | 92 | inline ::eds::tracking::Config readTrackingConfig(YAML::Node config) 93 | { 94 | ::eds::tracking::Config tracker_config; 95 | 96 | /** Number of points per frame **/ 97 | tracker_config.percent_points = config["percent_points"].as(); 98 | /** Config for tracker type (only ceres) **/ 99 | tracker_config.type = config["type"].as(); 100 | 101 | if (config["bootstrapping"]) 102 | { 103 | std::string bootstrapping = config["bootstrapping"].as(); 104 | if (bootstrapping.compare("EIGHT_POINTS") == 0) 105 | tracker_config.bootstrap = eds::tracking::EIGHT_POINTS; 106 | else if (bootstrapping.compare("MiDAS") == 0) 107 | tracker_config.bootstrap = eds::tracking::MiDAS; 108 | else 109 | tracker_config.bootstrap = eds::tracking::EIGHT_POINTS; 110 | } 111 | else 112 | tracker_config.bootstrap = eds::tracking::EIGHT_POINTS; 113 | 114 | /** Config the loss **/ 115 | YAML::Node tracker_loss = config["loss_function"]; 116 | std::string loss_name = tracker_loss["type"].as(); 117 | tracker_config.loss_type = eds::tracking::selectLoss(tracker_loss["type"].as()); 118 | tracker_config.loss_params = tracker_loss["param"].as< std::vector >(); 119 | 120 | /** Config for ceres options **/ 121 | YAML::Node tracker_options = config["options"]; 122 | tracker_config.options.linear_solver_type = eds::tracking::selectSolver(tracker_options["solver_type"].as()); 123 | tracker_config.options.num_threads = tracker_options["num_threads"].as(); 124 | tracker_config.options.max_num_iterations = tracker_options["max_num_iterations"].as< std::vector > (); 125 | tracker_config.options.function_tolerance = tracker_options["function_tolerance"].as(); 126 | tracker_config.options.minimizer_progress_to_stdout = tracker_options["minimizer_progress_to_stdout"].as(); 127 | 128 | return tracker_config; 129 | }; 130 | 131 | } // tracking namespace 132 | } // end namespace 133 | 134 | #endif -------------------------------------------------------------------------------- /src/tracking/EventFrame.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the EDS: Event-aided Direct Sparse Odometry 3 | * (https://rpg.ifi.uzh.ch/eds.html) 4 | * 5 | * Copyright (c) 2022 Javier Hidalgo-Carrió, Robotics and Perception 6 | * Group (RPG) University of Zurich. 7 | * 8 | * EDS is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, version 3. 11 | * 12 | * EDS is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #ifndef _EDS_EVENT_FRAME_HPP_ 22 | #define _EDS_EVENT_FRAME_HPP_ 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | namespace eds { 30 | namespace tracking { 31 | class EventFrame 32 | { 33 | public: 34 | static constexpr float log_eps = 1e-06; 35 | public: 36 | /* unique id **/ 37 | uint64_t idx; 38 | /** Desired Image dimension **/ 39 | uint16_t height, width; 40 | /** Time stamps **/ 41 | base::Time first_time, last_time, time, delta_time; 42 | /** Undistortion Maps (inverse and forward mapping) **/ 43 | cv::Mat mapx, fwd_mapx, mapy, fwd_mapy; 44 | /** Rescale out img order: (W x H) like in cv::Size**/ 45 | std::array out_scale; 46 | /** Events Coordinates and normalize coord **/ 47 | std::vector coord, undist_coord; 48 | /** Events polarities **/ 49 | std::vector pol; 50 | /** Distortion model information **/ 51 | std::string distortion_model; 52 | /** Intrisic and rectification matrices **/ 53 | cv::Mat K, D, K_ref, R_rect; 54 | /** Event Frame pose **/ 55 | ::base::Affine3d T_w_ef; 56 | /** Event frame (integration of events) no normalized **/ 57 | std::vector frame; 58 | /** Normalized event frame in std vector for optimization **/ 59 | std::vector< std::vector > event_frame; // event_frame = frame / norm 60 | /** Norm of the event frame **/ 61 | std::vector norm; 62 | 63 | public: 64 | /** @brief Default constructor **/ 65 | EventFrame(const ::eds::calib::Camera &cam, const ::eds::calib::Camera &newcam, const std::string &distortion_model="radtan"); 66 | 67 | EventFrame(const uint64_t &idx, const std::vector &events, 68 | const ::eds::calib::CameraInfo &cam_info, const int &num_levels = 1, 69 | const ::base::Affine3d &T=::base::Affine3d::Identity(), 70 | const cv::Size &out_size = cv::Size(0, 0)); 71 | 72 | /** @brief Default constructor **/ 73 | EventFrame(const uint64_t &idx, const std::vector &events, const uint16_t height, const uint16_t width, 74 | cv::Mat &K, cv::Mat &D, cv::Mat &R_rect, cv::Mat &P, const std::string distortion_model="radtan", const int &num_levels = 1, 75 | const ::base::Affine3d &T=::base::Affine3d::Identity(), const cv::Size &out_size = cv::Size(0, 0)); 76 | 77 | /** @brief Insert new Eventframe **/ 78 | void create(const uint64_t &idx, const std::vector &events, 79 | const ::eds::calib::CameraInfo &cam_info, const int &num_levels = 1, 80 | const ::base::Affine3d &T=::base::Affine3d::Identity(), 81 | const cv::Size &out_size = cv::Size(0, 0)); 82 | 83 | void create(const uint64_t &idx, const std::vector &events, 84 | const uint16_t height, const uint16_t width, const int &num_levels = 1, 85 | const ::base::Affine3d &T=::base::Affine3d::Identity(), const cv::Size &out_size = cv::Size(0, 0)); 86 | 87 | void clear(); 88 | 89 | cv::Mat viz(size_t id=0, bool color = false); 90 | 91 | cv::Mat getEventFrame(const size_t &id=0); 92 | 93 | cv::Mat getEventFrameViz(const size_t &id=0, bool color = false); 94 | 95 | void setPose(const ::base::Transform3d& pose); 96 | 97 | ::base::Transform3d& getPose(); 98 | 99 | ::base::Matrix4d getPoseMatrix(); 100 | 101 | std::pair getTransQuater(); 102 | 103 | cv::Mat epilinesViz(const std::vector &coord, const cv::Mat &F, const size_t &skip_amount = 10.0); 104 | 105 | cv::Mat pyramidViz(const bool &color = false); 106 | 107 | }; 108 | 109 | } //tracking namespace 110 | } // end namespace 111 | 112 | #endif // _EDS_EVENT_FRAME_HPP_ -------------------------------------------------------------------------------- /src/tracking/HessianBlocks.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | 26 | #include "eds/tracking/HessianBlocks.h" 27 | #include "eds/utils/FrameShell.h" 28 | #include "eds/tracking/ImmaturePoint.h" 29 | #include "eds/bundles/EnergyFunctionalStructs.h" 30 | 31 | namespace dso 32 | { 33 | 34 | int FrameHessian::instanceCounter=0; 35 | int PointHessian::instanceCounter=0; 36 | int CalibHessian::instanceCounter=0; 37 | 38 | PointHessian::PointHessian(const ImmaturePoint* const rawPoint, CalibHessian* Hcalib) 39 | { 40 | instanceCounter++; 41 | host = rawPoint->host; 42 | hasDepthPrior=false; 43 | 44 | idepth_hessian=0; 45 | maxRelBaseline=0; 46 | numGoodResiduals=0; 47 | 48 | // set static values & initialization. 49 | u = rawPoint->u; 50 | v = rawPoint->v; 51 | assert(std::isfinite(rawPoint->idepth_max)); 52 | //idepth_init = rawPoint->idepth_GT; 53 | 54 | my_type = rawPoint->my_type; 55 | 56 | setIdepthScaled((rawPoint->idepth_max + rawPoint->idepth_min)*0.5); 57 | setPointStatus(PointHessian::INACTIVE); 58 | 59 | int n = patternNum; 60 | memcpy(color, rawPoint->color, sizeof(float)*n); 61 | 62 | if (rawPoint->red) 63 | memcpy(red, rawPoint->red, sizeof(float)*n); 64 | if (rawPoint->green) 65 | memcpy(green, rawPoint->green, sizeof(float)*n); 66 | if (rawPoint->blue) 67 | memcpy(blue, rawPoint->blue, sizeof(float)*n); 68 | 69 | memcpy(weights, rawPoint->weights, sizeof(float)*n); 70 | energyTH = rawPoint->energyTH; 71 | 72 | efPoint=0; 73 | 74 | 75 | } 76 | 77 | 78 | void PointHessian::release() 79 | { 80 | for(unsigned int i=0;i().squaredNorm() < 1e-20); 88 | 89 | this->state_zero = state_zero; 90 | 91 | 92 | for(int i=0;i<6;i++) 93 | { 94 | Vec6 eps; eps.setZero(); eps[i] = 1e-3; 95 | SE3 EepsP = Sophus::SE3::exp(eps); 96 | SE3 EepsM = Sophus::SE3::exp(-eps); 97 | SE3 w2c_leftEps_P_x0 = (get_worldToCam_evalPT() * EepsP) * get_worldToCam_evalPT().inverse(); 98 | SE3 w2c_leftEps_M_x0 = (get_worldToCam_evalPT() * EepsM) * get_worldToCam_evalPT().inverse(); 99 | nullspaces_pose.col(i) = (w2c_leftEps_P_x0.log() - w2c_leftEps_M_x0.log())/(2e-3); 100 | } 101 | //nullspaces_pose.topRows<3>() *= SCALE_XI_TRANS_INVERSE; 102 | //nullspaces_pose.bottomRows<3>() *= SCALE_XI_ROT_INVERSE; 103 | 104 | // scale change 105 | SE3 w2c_leftEps_P_x0 = (get_worldToCam_evalPT()); 106 | w2c_leftEps_P_x0.translation() *= 1.00001; 107 | w2c_leftEps_P_x0 = w2c_leftEps_P_x0 * get_worldToCam_evalPT().inverse(); 108 | SE3 w2c_leftEps_M_x0 = (get_worldToCam_evalPT()); 109 | w2c_leftEps_M_x0.translation() /= 1.00001; 110 | w2c_leftEps_M_x0 = w2c_leftEps_M_x0 * get_worldToCam_evalPT().inverse(); 111 | nullspaces_scale = (w2c_leftEps_P_x0.log() - w2c_leftEps_M_x0.log())/(2e-3); 112 | 113 | 114 | nullspaces_affine.setZero(); 115 | nullspaces_affine.topLeftCorner<2,1>() = Vec2(1,0); 116 | assert(ab_exposure > 0); 117 | nullspaces_affine.topRightCorner<2,1>() = Vec2(0, expf(aff_g2l_0().a)*ab_exposure); 118 | }; 119 | 120 | 121 | 122 | void FrameHessian::release() 123 | { 124 | // DELETE POINT 125 | // DELETE RESIDUAL 126 | for(unsigned int i=0;i0) 163 | { 164 | int lvlm1 = lvl-1; 165 | int wlm1 = wG[lvlm1]; 166 | Eigen::Vector3f* dI_lm = dIp[lvlm1]; 167 | 168 | 169 | 170 | for(int y=0;ygetBGradOnly((float)(dI_l[idx][0])); 198 | dabs_l[idx] *= gw*gw; // convert to gradient of original color space (before removing response). 199 | } 200 | } 201 | } 202 | } 203 | 204 | void FrameFramePrecalc::set(FrameHessian* host, FrameHessian* target, CalibHessian* HCalib ) 205 | { 206 | this->host = host; 207 | this->target = target; 208 | 209 | SE3 leftToLeft_0 = target->get_worldToCam_evalPT() * host->get_worldToCam_evalPT().inverse(); 210 | PRE_RTll_0 = (leftToLeft_0.rotationMatrix()).cast(); 211 | PRE_tTll_0 = (leftToLeft_0.translation()).cast(); 212 | 213 | 214 | 215 | SE3 leftToLeft = target->PRE_worldToCam * host->PRE_camToWorld; 216 | PRE_RTll = (leftToLeft.rotationMatrix()).cast(); 217 | PRE_tTll = (leftToLeft.translation()).cast(); 218 | distanceLL = leftToLeft.translation().norm(); 219 | 220 | 221 | Mat33f K = Mat33f::Zero(); 222 | K(0,0) = HCalib->fxl(); 223 | K(1,1) = HCalib->fyl(); 224 | K(0,2) = HCalib->cxl(); 225 | K(1,2) = HCalib->cyl(); 226 | K(2,2) = 1; 227 | PRE_KRKiTll = K * PRE_RTll * K.inverse(); 228 | PRE_RKiTll = PRE_RTll * K.inverse(); 229 | PRE_KtTll = K * PRE_tTll; 230 | 231 | 232 | PRE_aff_mode = AffLight::fromToVecExposure(host->ab_exposure, target->ab_exposure, host->aff_g2l(), target->aff_g2l()).cast(); 233 | PRE_b0_mode = host->aff_g2l_0().b; 234 | } 235 | 236 | } 237 | 238 | -------------------------------------------------------------------------------- /src/tracking/ImmaturePoint.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | 28 | #include "eds/utils/NumType.h" 29 | #include "eds/tracking/Residuals.h" 30 | #include "eds/tracking/HessianBlocks.h" 31 | 32 | namespace dso 33 | { 34 | 35 | 36 | struct ImmaturePointTemporaryResidual 37 | { 38 | public: 39 | ResState state_state; 40 | double state_energy; 41 | ResState state_NewState; 42 | double state_NewEnergy; 43 | FrameHessian* target; 44 | }; 45 | 46 | 47 | enum ImmaturePointStatus { 48 | IPS_GOOD=0, // traced well and good 49 | IPS_OOB, // OOB: end tracking & marginalize! 50 | IPS_OUTLIER, // energy too high: if happens again: outlier! 51 | IPS_SKIPPED, // traced well and good (but not actually traced). 52 | IPS_BADCONDITION, // not traced because of bad condition. 53 | IPS_UNINITIALIZED}; // not even traced once. 54 | 55 | 56 | class ImmaturePoint 57 | { 58 | public: 59 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 60 | // static values 61 | float color[MAX_RES_PER_POINT]; 62 | float red[MAX_RES_PER_POINT]; 63 | float green[MAX_RES_PER_POINT]; 64 | float blue[MAX_RES_PER_POINT]; 65 | float weights[MAX_RES_PER_POINT]; 66 | 67 | Mat22f gradH; 68 | Vec2f gradH_ev; 69 | Mat22f gradH_eig; 70 | float energyTH; 71 | float u,v; 72 | FrameHessian* host; 73 | int idxInImmaturePoints; 74 | 75 | float quality; 76 | 77 | float my_type; 78 | 79 | float idepth_min; 80 | float idepth_max; 81 | ImmaturePoint(int u_, int v_, FrameHessian* host_, float type, CalibHessian* HCalib, 82 | const unsigned char *red=nullptr, const unsigned char *green=nullptr, const unsigned char *blue=nullptr); 83 | ImmaturePoint(int u_, int v_, FrameHessian* host_, float type, float idepth, double distance, CalibHessian* HCalib, 84 | const unsigned char *red=nullptr, const unsigned char *green=nullptr, const unsigned char *blue=nullptr); 85 | ~ImmaturePoint(); 86 | 87 | ImmaturePointStatus traceOn(FrameHessian* frame, const Mat33f &hostToFrame_KRKi, const Vec3f &hostToFrame_Kt, const Vec2f &hostToFrame_affine, CalibHessian* HCalib, bool debugPrint=false); 88 | 89 | ImmaturePointStatus lastTraceStatus; 90 | Vec2f lastTraceUV; 91 | float lastTracePixelInterval; 92 | 93 | float idepth_GT; 94 | 95 | double linearizeResidual( 96 | CalibHessian * HCalib, const float outlierTHSlack, 97 | ImmaturePointTemporaryResidual* tmpRes, 98 | float &Hdd, float &bd, 99 | float idepth); 100 | float getdPixdd( 101 | CalibHessian * HCalib, 102 | ImmaturePointTemporaryResidual* tmpRes, 103 | float idepth); 104 | 105 | float calcResidual( 106 | CalibHessian * HCalib, const float outlierTHSlack, 107 | ImmaturePointTemporaryResidual* tmpRes, 108 | float idepth); 109 | 110 | private: 111 | }; 112 | 113 | } 114 | 115 | -------------------------------------------------------------------------------- /src/tracking/PhotometricError.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the EDS: Event-aided Direct Sparse Odometry 3 | * (https://rpg.ifi.uzh.ch/eds.html) 4 | * 5 | * Copyright (c) 2022 Javier Hidalgo-Carrió, Robotics and Perception 6 | * Group (RPG) University of Zurich. 7 | * 8 | * EDS is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, version 3. 11 | * 12 | * EDS is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #ifndef _EDS_PHOTOMETRIC_ERROR_HPP_ 22 | #define _EDS_PHOTOMETRIC_ERROR_HPP_ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | namespace eds { namespace tracking { 31 | 32 | struct UnitNormVectorAddition 33 | { 34 | 35 | template 36 | bool operator()(const Scalar* x, const Scalar* delta, Scalar* x_plus_delta) const { 37 | 38 | Scalar sum = Scalar(0); 39 | for (size_t i = 0; i < 6; i++) { 40 | Scalar s = x[i] + delta[i]; 41 | sum += s * s; 42 | x_plus_delta[i] = s; 43 | } 44 | 45 | 46 | sum = Scalar(1)/ceres::sqrt(sum); 47 | 48 | for (size_t i = 0; i < 6; i++) { 49 | x_plus_delta[i] *= sum; 50 | } 51 | 52 | return true; 53 | } 54 | }; 55 | 56 | struct PhotometricError 57 | { 58 | PhotometricError(const std::vector *grad, 59 | const std::vector *norm_coord, 60 | const std::vector *idp, 61 | const std::vector *weights, 62 | const std::vector *event_frame, 63 | const int &height, const int &width, 64 | const double &fx, const double &fy, 65 | const double &cx, const double &cy, 66 | const int &start_element, const int &num_elements) 67 | :height(height), width(width), fx(fx), fy(fy), cx(cx), cy(cy) 68 | { 69 | /** Sanity checks **/ 70 | assert(grad->size() == norm_coord->size()); 71 | assert(norm_coord->size() == idp->size()); 72 | assert(idp->size() == weights->size()); 73 | assert(event_frame->size() == (size_t)(height * width)); 74 | 75 | this->start = start_element; 76 | this->n_points = num_elements; 77 | 78 | /** Get the parameters **/ 79 | this->grad = grad; 80 | this->norm_coord = norm_coord; 81 | this->idp = idp; 82 | this->weights = weights; 83 | this->event_frame = event_frame; 84 | 85 | //std::cout<<"[PHOTO_ERROR] height: "<n_points<grad))<<"] size:"<grad->size()<norm_coord))<<"] size:"<norm_coord->size()<idp))<<"] size: "<idp->size()<size()<event_frame))<<"] size: "<event_frame->size()<start; 97 | for (int i=0; ieps); 101 | p[0] = (*norm_coord)[idx].x*p[2]; 102 | p[1] = (*norm_coord)[idx].y*p[2]; 103 | 104 | kp.push_back(p); 105 | idx++; 106 | } 107 | assert(kp.size() == n_points); 108 | 109 | /** Create the grid for the event frame interpolate **/ 110 | event_grid.reset(new ceres::Grid2D (this->event_frame->data(), 0, height, 0, width)); 111 | event_grid_interp.reset(new ceres::BiCubicInterpolator< ceres::Grid2D > (*event_grid)); 112 | } 113 | 114 | template inline 115 | void compute_flow(const T &xp, const T &yp, const T* vx, const T &idp, T *result) const 116 | { 117 | result[0] = (-idp*vx[0]) + (xp*idp*vx[2]) 118 | + (xp*yp*vx[3]) - (T(1.0)+ceres::pow(xp, 2))*vx[4] + (yp*vx[5]); 119 | 120 | result[1] = (-idp*vx[1]) + (yp*idp*vx[2]) 121 | + (T(1.0)+ceres::pow(yp, 2))*vx[3] - (xp*yp*vx[4]) - (xp*vx[5]); 122 | } 123 | 124 | template 125 | bool operator()(const T* px, const T* qx, const T* vx, T* residual) const 126 | { 127 | /** Get current pose and quaternion **/ 128 | Eigen::Map> p_x(px); 129 | Eigen::Map> q_x(qx); 130 | 131 | /** Compute the model for the active points **/ 132 | T model_norm_sq(1e-03); 133 | int idx = this->start; 134 | for (int i=0; in_points; i++) 135 | { 136 | /** Flow in the key frame **/ 137 | Eigen::Matrix flow; compute_flow(T((*norm_coord)[idx].x), T((*norm_coord)[idx].y), vx, T((*idp)[idx]), flow.data()); // 2 x 1 flow 138 | //flow[2] = T(0.0); 139 | 140 | /** Contra rotate the flow: flow in keyframe **/ 141 | //flow = q_x.toRotationMatrix().transpose() * flow; 142 | 143 | /** Model **/ 144 | residual[i] = -((*grad)[idx].x * flow[0] + (*grad)[idx].y * flow[1]); 145 | 146 | /** Get the normalization for later **/ 147 | model_norm_sq += ceres::pow(residual[i], 2); 148 | idx++; 149 | } 150 | 151 | /** Compute the residuals: model - event frame **/ 152 | T model_norm = ceres::sqrt(model_norm_sq); 153 | idx = this->start; 154 | for (int i=0; in_points; i++) 155 | { 156 | /** Get the point in the keyframe **/ 157 | Eigen::Matrix point; 158 | point[0] = T(kp[i][0]); point[1] = T(kp[i][1]); point[2] = T(kp[i][2]); 159 | //std::cout<<"["< p; 163 | p = q_x.toRotationMatrix() * point + p_x; 164 | //std::cout<<"p: "<Evaluate(yp, xp, &event_brightness); 173 | residual[i] = T((*weights)[idx]) * ((residual[i]/model_norm) - event_brightness); 174 | //std::cout<<"["< *grad, 187 | const std::vector *norm_coord, 188 | const std::vector *idp, 189 | const std::vector *weights, 190 | const std::vector *event_frame, 191 | const int &height, const int &width, 192 | const double &fx, const double &fy, 193 | const double &cx, const double &cy, 194 | const int &start_element, const int &num_elements) 195 | { 196 | PhotometricError* functor = new PhotometricError(grad, norm_coord, idp, weights, event_frame, height, width, fx, fy, cx, cy, start_element, num_elements); 197 | return new ceres::AutoDiffCostFunction(functor, num_elements); 198 | } 199 | 200 | static constexpr double eps = 1e-05; 201 | 202 | int start; // start of points 203 | int n_points; // number of active points 204 | int height, width; // height and width of the image 205 | double fx, fy, cx, cy; // intrinsics 206 | std::vector< ::eds::mapping::Point3d > kp; // 3D points [x, y , z] 207 | const std::vector *grad; // N x 2 img gradient [\Nabla x, \Nabla y] 208 | const std::vector *norm_coord; // N x 2 normalized keyframe coord [X, Y] 209 | const std::vector *idp; // N x 1 inverse depth 210 | const std::vector *weights; // N x 1 weight points 211 | const std::vector *event_frame; // H x W event frame with the brightness change 212 | std::unique_ptr< ceres::Grid2D > event_grid; 213 | std::unique_ptr< ceres::BiCubicInterpolator< ceres::Grid2D > > event_grid_interp; 214 | }; 215 | 216 | } //tracking namespace 217 | } // end namespace 218 | 219 | #endif // _EDS_PHOTOMETRIC_ERROR_HPP_ -------------------------------------------------------------------------------- /src/tracking/PhotometricErrorNC.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the EDS: Event-aided Direct Sparse Odometry 3 | * (https://rpg.ifi.uzh.ch/eds.html) 4 | * 5 | * Copyright (c) 2022 Javier Hidalgo-Carrió, Robotics and Perception 6 | * Group (RPG) University of Zurich. 7 | * 8 | * EDS is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, version 3. 11 | * 12 | * EDS is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #ifndef _EDS_PHOTOMETRIC_ERROR_HPP_ 22 | #define _EDS_PHOTOMETRIC_ERROR_HPP_ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | namespace eds { namespace tracking { 31 | 32 | struct UnitNormVectorAddition 33 | { 34 | 35 | template 36 | bool operator()(const Scalar* x, const Scalar* delta, Scalar* x_plus_delta) const { 37 | 38 | Scalar sum = Scalar(0); 39 | for (size_t i = 0; i < 6; i++) { 40 | Scalar s = x[i] + delta[i]; 41 | sum += s * s; 42 | x_plus_delta[i] = s; 43 | } 44 | 45 | 46 | sum = Scalar(1)/ceres::sqrt(sum); 47 | 48 | for (size_t i = 0; i < 6; i++) { 49 | x_plus_delta[i] *= sum; 50 | } 51 | 52 | return true; 53 | } 54 | }; 55 | 56 | struct PhotometricErrorNC 57 | { 58 | PhotometricErrorNC(const std::vector *grad, 59 | const std::vector *norm_coord, 60 | const std::vector *idp, 61 | const std::vector *weights, 62 | const std::vector *event_frame, 63 | const int &height, const int &width, 64 | const double &fx, const double &fy, 65 | const double &cx, const double &cy, 66 | const int &start_element, const int &num_elements) 67 | :height(height), width(width), fx(fx), fy(fy), cx(cx), cy(cy) 68 | { 69 | /** Sanity checks **/ 70 | assert(grad->size() == norm_coord->size()); 71 | assert(norm_coord->size() == idp->size()); 72 | assert(idp->size() == weights->size()); 73 | assert(event_frame->size() == (size_t)(height * width)); 74 | 75 | this->start = start_element; 76 | this->n_points = num_elements; 77 | 78 | /** Get the parameters **/ 79 | this->grad = grad; 80 | this->norm_coord = norm_coord; 81 | this->idp = idp; 82 | this->weights = weights; 83 | this->event_frame = event_frame; 84 | 85 | //std::cout<<"[PHOTO_ERROR] height: "<n_points<grad))<<"] size:"<grad->size()<norm_coord))<<"] size:"<norm_coord->size()<idp))<<"] size: "<idp->size()<size()<event_frame))<<"] size: "<event_frame->size()<start; 97 | for (int i=0; ieps); 101 | p[0] = (*norm_coord)[idx].x*p[2]; 102 | p[1] = (*norm_coord)[idx].y*p[2]; 103 | 104 | kp.push_back(p); 105 | idx++; 106 | } 107 | assert(kp.size() == n_points); 108 | 109 | /** Create the grid for the event frame interpolate **/ 110 | event_grid.reset(new ceres::Grid2D (this->event_frame->data(), 0, height, 0, width)); 111 | event_grid_interp.reset(new ceres::BiCubicInterpolator< ceres::Grid2D > (*event_grid)); 112 | } 113 | 114 | template inline 115 | void compute_flow(const T &xp, const T &yp, const T* vx, const T &idp, T *result) const 116 | { 117 | result[0] = (-idp*vx[0]) + (xp*idp*vx[2]) 118 | + (xp*yp*vx[3]) - (T(1.0)+ceres::pow(xp, 2))*vx[4] + (yp*vx[5]); 119 | 120 | result[1] = (-idp*vx[1]) + (yp*idp*vx[2]) 121 | + (T(1.0)+ceres::pow(yp, 2))*vx[3] - (xp*yp*vx[4]) - (xp*vx[5]); 122 | } 123 | 124 | template 125 | bool operator()(const T* px, const T* qx, const T* vx, T* residual) const 126 | { 127 | /** Get current pose and quaternion **/ 128 | Eigen::Map> p_x(px); 129 | Eigen::Map> q_x(qx); 130 | 131 | /** Compute the model for the active points **/ 132 | T model_norm_sq(1e-03); 133 | int idx = this->start; 134 | for (int i=0; in_points; i++) 135 | { 136 | /** Flow in the key frame **/ 137 | Eigen::Matrix flow; compute_flow(T((*norm_coord)[idx].x), T((*norm_coord)[idx].y), vx, T((*idp)[idx]), flow.data()); // 2 x 1 flow 138 | //flow[2] = T(0.0); 139 | 140 | /** Contra rotate the flow: flow in keyframe **/ 141 | //flow = q_x.toRotationMatrix().transpose() * flow; 142 | 143 | /** Model **/ 144 | residual[i] = -((*grad)[idx].x * flow[0] + (*grad)[idx].y * flow[1]); 145 | 146 | /** Get the normalization for later **/ 147 | model_norm_sq += ceres::pow(residual[i], 2); 148 | idx++; 149 | } 150 | 151 | T meas_norm_sq(1e-03); 152 | std::vector event_points_brightness; 153 | for (int i=0; in_points; i++) 154 | { 155 | /** Get the point in the keyframe **/ 156 | Eigen::Matrix point; 157 | point[0] = T(kp[i][0]); point[1] = T(kp[i][1]); point[2] = T(kp[i][2]); 158 | //std::cout<<"["< p; 162 | p = q_x.toRotationMatrix() * point + p_x; 163 | //std::cout<<"p: "<Evaluate(yp, xp, &event_brightness); 172 | event_points_brightness.push_back(event_brightness); 173 | 174 | /** Get the normalization for later **/ 175 | meas_norm_sq += ceres::pow(event_brightness, 2); 176 | } 177 | 178 | /** Compute the residuals: model - event frame (sparse points) **/ 179 | T model_norm = ceres::sqrt(model_norm_sq); 180 | T meas_norm = ceres::sqrt(meas_norm_sq); 181 | idx = this->start; 182 | for (int i=0; in_points; i++) 183 | { 184 | residual[i] = T((*weights)[idx]) * ((residual[i]/model_norm) - (event_points_brightness[i]/meas_norm)); 185 | idx++; 186 | } 187 | 188 | //std::cout << "[CERES] Press Enter to Continue"; 189 | //std::cin.ignore(); 190 | //std::cout<<"*****"< *grad, 199 | const std::vector *norm_coord, 200 | const std::vector *idp, 201 | const std::vector *weights, 202 | const std::vector *event_frame, 203 | const int &height, const int &width, 204 | const double &fx, const double &fy, 205 | const double &cx, const double &cy, 206 | const int &start_element, const int &num_elements) 207 | { 208 | PhotometricErrorNC* functor = new PhotometricErrorNC(grad, norm_coord, idp, weights, event_frame, height, width, fx, fy, cx, cy, start_element, num_elements); 209 | return new ceres::AutoDiffCostFunction(functor, num_elements); 210 | } 211 | 212 | static constexpr double eps = 1e-05; 213 | 214 | int start; // start of points 215 | int n_points; // number of active points 216 | int height, width; // height and width of the image 217 | double fx, fy, cx, cy; // intrinsics 218 | std::vector< ::eds::mapping::Point3d > kp; // 3D points [x, y , z] 219 | const std::vector *grad; // N x 2 img gradient [\Nabla x, \Nabla y] 220 | const std::vector *norm_coord; // N x 2 normalized keyframe coord [X, Y] 221 | const std::vector *idp; // N x 1 inverse depth 222 | const std::vector *weights; // N x 1 weight points 223 | const std::vector *event_frame; // H x W event frame with the brightness change 224 | std::unique_ptr< ceres::Grid2D > event_grid; 225 | std::unique_ptr< ceres::BiCubicInterpolator< ceres::Grid2D > > event_grid_interp; 226 | }; 227 | 228 | } //tracking namespace 229 | } // end namespace 230 | 231 | #endif // _EDS_PHOTOMETRIC_ERROR_HPP_ 232 | -------------------------------------------------------------------------------- /src/tracking/ResidualProjections.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | #include "eds/utils/NumType.h" 28 | #include "eds/tracking/HessianBlocks.h" 29 | #include "eds/utils/settings.h" 30 | 31 | namespace dso 32 | { 33 | 34 | 35 | EIGEN_STRONG_INLINE float derive_idepth( 36 | const Vec3f &t, const float &u, const float &v, 37 | const int &dx, const int &dy, const float &dxInterp, 38 | const float &dyInterp, const float &drescale) 39 | { 40 | return (dxInterp*drescale * (t[0]-t[2]*u) 41 | + dyInterp*drescale * (t[1]-t[2]*v))*SCALE_IDEPTH; 42 | } 43 | 44 | 45 | 46 | EIGEN_STRONG_INLINE bool projectPoint( 47 | const float &u_pt,const float &v_pt, 48 | const float &idepth, 49 | const Mat33f &KRKi, const Vec3f &Kt, 50 | float &Ku, float &Kv) 51 | { 52 | Vec3f ptp = KRKi * Vec3f(u_pt,v_pt, 1) + Kt*idepth; 53 | Ku = ptp[0] / ptp[2]; 54 | Kv = ptp[1] / ptp[2]; 55 | return Ku>1.1f && Kv>1.1f && Kucxl())*HCalib->fxli(), 71 | (v_pt+dy-HCalib->cyl())*HCalib->fyli(), 72 | 1); 73 | 74 | Vec3f ptp = R * KliP + t*idepth; 75 | drescale = 1.0f/ptp[2]; 76 | new_idepth = idepth*drescale; 77 | 78 | if(!(drescale>0)) return false; 79 | 80 | u = ptp[0] * drescale; 81 | v = ptp[1] * drescale; 82 | Ku = u*HCalib->fxl() + HCalib->cxl(); 83 | Kv = v*HCalib->fyl() + HCalib->cyl(); 84 | 85 | return Ku>1.1f && Kv>1.1f && Ku, 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | 28 | #include "eds/utils/globalCalib.h" 29 | #include "eds/utils/NumType.h" 30 | #include "eds/utils/globalFuncs.h" 31 | #include "eds/bundles/RawResidualJacobian.h" 32 | 33 | #include "vector" 34 | #include 35 | #include 36 | 37 | namespace dso 38 | { 39 | class PointHessian; 40 | class FrameHessian; 41 | class CalibHessian; 42 | 43 | class EFResidual; 44 | 45 | 46 | enum ResLocation {ACTIVE=0, LINEARIZED, MARGINALIZED, NONE}; 47 | enum ResState {IN=0, OOB, OUTLIER}; 48 | 49 | struct FullJacRowT 50 | { 51 | Eigen::Vector2f projectedTo[MAX_RES_PER_POINT]; 52 | }; 53 | 54 | class PointFrameResidual 55 | { 56 | public: 57 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 58 | 59 | EFResidual* efResidual; 60 | 61 | static int instanceCounter; 62 | 63 | 64 | ResState state_state; 65 | double state_energy; 66 | ResState state_NewState; 67 | double state_NewEnergy; 68 | double state_NewEnergyWithOutlier; 69 | 70 | 71 | void setState(ResState s) {state_state = s;} 72 | 73 | 74 | PointHessian* point; 75 | FrameHessian* host; 76 | FrameHessian* target; 77 | RawResidualJacobian* J; 78 | 79 | 80 | bool isNew; 81 | 82 | 83 | Eigen::Vector2f projectedTo[MAX_RES_PER_POINT]; 84 | Vec3f centerProjectedTo; 85 | 86 | ~PointFrameResidual(); 87 | PointFrameResidual(); 88 | PointFrameResidual(PointHessian* point_, FrameHessian* host_, FrameHessian* target_); 89 | double linearize(CalibHessian* HCalib); 90 | 91 | 92 | void resetOOB() 93 | { 94 | state_NewEnergy = state_energy = 0; 95 | state_NewState = ResState::OUTLIER; 96 | 97 | setState(ResState::IN); 98 | }; 99 | void applyRes( bool copyJacobians); 100 | 101 | void debugPlot(); 102 | 103 | void printRows(std::vector &v, VecX &r, int nFrames, int nPoints, int M, int res); 104 | }; 105 | } 106 | 107 | -------------------------------------------------------------------------------- /src/tracking/Tracker.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the EDS: Event-aided Direct Sparse Odometry 3 | * (https://rpg.ifi.uzh.ch/eds.html) 4 | * 5 | * Copyright (c) 2022 Javier Hidalgo-Carrió, Robotics and Perception 6 | * Group (RPG) University of Zurich. 7 | * 8 | * EDS is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, version 3. 11 | * 12 | * EDS is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #ifndef _EDS_TRACKER_HPP_ 22 | #define _EDS_TRACKER_HPP_ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | namespace eds { namespace tracking{ 33 | 34 | enum LOSS_PARAM_METHOD{CONSTANT, MAD, STD}; 35 | 36 | class Tracker 37 | { 38 | public: 39 | /** Configuration **/ 40 | ::eds::tracking::Config config; 41 | 42 | private: 43 | /** Pointer to KeyFrame **/ 44 | std::shared_ptr kf; 45 | 46 | /** Optimization parameters **/ 47 | Eigen::Vector3d px; 48 | Eigen::Quaterniond qx; 49 | Eigen::Matrix vx; 50 | 51 | /** Status Information **/ 52 | eds::tracking::TrackerInfo info; 53 | 54 | /** Vector of the last N poses **/ 55 | std::vector poses; 56 | 57 | /** Squared Norm Mean Flow **/ 58 | double squared_norm_flow; 59 | 60 | public: 61 | /** @brief Default constructor */ 62 | Tracker(std::shared_ptr kf, const eds::tracking::Config &config); 63 | 64 | /** @brief Default constructor */ 65 | Tracker(const eds::tracking::Config &config); 66 | 67 | void reset(std::shared_ptr kf, const Eigen::Vector3d &px, const Eigen::Quaterniond &qx, const bool &keep_velo = true); 68 | 69 | void reset(std::shared_ptr kf, const Eigen::Vector3d &px, const Eigen::Quaterniond &qx, const base::Vector6d &velo); 70 | 71 | void set(const base::Transform3d &T_kf_ef); 72 | 73 | void optimize(const int &id, const std::vector *event_frame, ::base::Transform3d &T_kf_ef, 74 | const Eigen::Vector3d &px, const Eigen::Quaterniond &qx, 75 | const eds::tracking::LOSS_PARAM_METHOD loss_param_method); 76 | 77 | void optimize(const int &id, const std::vector *event_frame, ::base::Transform3d &T_kf_ef, 78 | const Eigen::Matrix &vx, const eds::tracking::LOSS_PARAM_METHOD loss_param_method); 79 | 80 | bool optimize(const int &id, const std::vector *event_frame, ::base::Transform3d &T_kf_ef, 81 | const eds::tracking::LOSS_PARAM_METHOD loss_param_method = eds::tracking::LOSS_PARAM_METHOD::MAD); 82 | 83 | ::base::Transform3d getTransform(); 84 | 85 | ::base::Transform3d getTransform(bool &result); 86 | 87 | Eigen::Matrix& getVelocity(); 88 | 89 | const Eigen::Vector3d linearVelocity(); 90 | 91 | const Eigen::Vector3d angularVelocity(); 92 | 93 | std::vector getLossParams(eds::tracking::LOSS_PARAM_METHOD method=CONSTANT); 94 | 95 | /** Get warpped active points coordinates (point in event frame) **/ 96 | std::vector getCoord(const bool &delete_out_point = false); 97 | 98 | void trackPoints(const cv::Mat &event_frame, const uint16_t &patch_radius = 7); 99 | 100 | void trackPointsPyr(const cv::Mat &event_frame, const size_t num_level = 3); 101 | 102 | std::vector trackPointsAlongEpiline(const cv::Mat &event_frame, const uint16_t &patch_radius = 7, 103 | const int &border_type = cv::BORDER_DEFAULT, const uint8_t &border_value = 255); 104 | 105 | cv::Mat getEMatrix(); 106 | 107 | cv::Mat getFMatrix(); 108 | 109 | ::eds::tracking::TrackerInfo getInfo(); 110 | 111 | bool getFilteredPose(eds::SE3 &pose, const size_t &mean_filter_size = 3); 112 | 113 | bool needNewKeyframe(const double &weight_factor = 0.03); 114 | }; 115 | 116 | } //tracking namespace 117 | } // end namespace 118 | 119 | #endif // _EDS_TRACKER_HPP_ 120 | -------------------------------------------------------------------------------- /src/tracking/Types.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the EDS: Event-aided Direct Sparse Odometry 3 | * (https://rpg.ifi.uzh.ch/eds.html) 4 | * 5 | * Copyright (c) 2022 Javier Hidalgo-Carrió, Robotics and Perception 6 | * Group (RPG) University of Zurich. 7 | * 8 | * EDS is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, version 3. 11 | * 12 | * EDS is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #ifndef _EDS_TRACKING_TYPES_HPP_ 22 | #define _EDS_TRACKING_TYPES_HPP_ 23 | 24 | #include 25 | 26 | /** Sophus **/ 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | 38 | namespace eds { namespace tracking { 39 | 40 | struct KFImagePyr 41 | { 42 | size_t w;// image width 43 | size_t h;// image height 44 | double fx, fy, cx, cy; // instrinsics 45 | cv::Mat img; // undistort image 46 | std::vector coord;// coordinates of points 47 | std::vector norm_coord;// normalized coordinates of points 48 | std::vector grad;// x-y gradient of the points 49 | std::vector idp;// inverse depth of the points 50 | }; 51 | 52 | struct ManageKFImagePyr 53 | { 54 | uint8_t num_level; //number of pyramid levels 55 | std::vector<::eds::tracking::KFImagePyr> image; // the pyramed images 56 | }; 57 | 58 | struct EventImagePyr 59 | { 60 | size_t w;// image width 61 | size_t h;// image height 62 | double fx, fy, cx, cy;// instrinsics 63 | cv::Mat frame;// Event frame (integration of events) no normalized 64 | std::vector event_frame;// event_frame = frame / norm 65 | double norm; // norm of the event frame 66 | }; 67 | 68 | struct ManageEventImagePyr 69 | { 70 | uint8_t num_level; //number of pyramid levels 71 | std::vector<::eds::tracking::EventImagePyr> image; 72 | }; 73 | 74 | } // tracking namespace 75 | 76 | typedef Sophus::SE3d SE3; 77 | typedef Sophus::Sim3d Sim3; 78 | typedef Sophus::SO3d SO3; 79 | 80 | /** SE3 Moving window **/ 81 | template 82 | class SE3MW 83 | { 84 | private: 85 | typedef typename std::array vector_type; 86 | typedef typename vector_type::iterator iterator; 87 | typedef typename vector_type::const_iterator const_iterator; 88 | 89 | public: 90 | vector_type data; 91 | size_t head, tail; 92 | 93 | public: 94 | SE3MW() { for(auto &it : data) it = SE3(); head = tail = 0;}; 95 | 96 | void advance() 97 | { 98 | /** Advance indexes **/ 99 | if (++(tail) == N) 100 | { 101 | tail = 0; 102 | } 103 | 104 | if (++(head) == N) 105 | { 106 | head = 0; 107 | } 108 | 109 | /** When full **/ 110 | if (head == tail) 111 | { 112 | if (++(tail) == N) 113 | { 114 | tail = 0; 115 | } 116 | } 117 | }; 118 | 119 | void push(const SE3 &se3) 120 | { 121 | data[head] = se3; 122 | advance(); 123 | }; 124 | 125 | SE3 mean() 126 | { 127 | int number = 0; 128 | base::Vector6d mean_log = base::Vector6d::Zero(); 129 | for (size_t i=tail; i != head; i = (i+1)%N) 130 | { 131 | mean_log += data[i].log(); 132 | number++; 133 | } 134 | //std::cout<<"number: "< SE3MW10; 146 | typedef SE3MW<20> SE3MW20; 147 | typedef SE3MW<50> SE3MW50; 148 | 149 | } // end namespace 150 | #endif // _EDS_TRACKING_TYPES_HPP_ 151 | 152 | -------------------------------------------------------------------------------- /src/utils/Calib.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the EDS: Event-aided Direct Sparse Odometry 3 | * (https://rpg.ifi.uzh.ch/eds.html) 4 | * 5 | * Copyright (c) 2022 Javier Hidalgo-Carrió, Robotics and Perception 6 | * Group (RPG) University of Zurich. 7 | * 8 | * EDS is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, version 3. 11 | * 12 | * EDS is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #ifndef _EDS_CALIB_HPP_ 22 | #define _EDS_CALIB_HPP_ 23 | 24 | /** Yaml **/ 25 | #include 26 | 27 | /** Opencv **/ 28 | #include 29 | 30 | /** Base types **/ 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | namespace eds { namespace calib { 37 | 38 | struct CameraInfo 39 | { 40 | uint16_t height;// input image height 41 | uint16_t width;// input image width 42 | uint16_t out_height;// output image height 43 | uint16_t out_width;// output image width 44 | std::string distortion_model; // distortion model name 45 | std::vector D; // distortion coefficients 46 | std::vector intrinsics; // fx, fy, cx, cy intrinsics 47 | std::vector R; //3x3 row-major rectification matrix 48 | /* this matrix specifies the intrinsic (camera) matrix 49 | of the processed (rectified) image. That is, the left 3x3 portion 50 | is the normal camera intrinsic matrix for the rectified image. **/ 51 | std::vector P; //3x4 row major 52 | std::vector T_cam_imu; //4x4 row-major p_cam = T_cam_imu * p_imu 53 | bool flip; //vertical flip when the image is comming from a beamsplitter 54 | 55 | void toDSOFormat(const std::string &filename="/tmp/dso_camera.txt"); 56 | }; 57 | 58 | struct Baseline 59 | { 60 | base::Vector3d translation; 61 | base::Quaterniond rotation; 62 | }; 63 | 64 | struct DualCamera 65 | { 66 | ::eds::calib::CameraInfo cam0; //rgb camera 67 | ::eds::calib::CameraInfo cam1; //event camera 68 | ::eds::calib::Baseline extrinsics; // Camera extrinsics 69 | }; 70 | 71 | /** Camera calibration **/ 72 | struct Camera 73 | { 74 | cv::Size size; 75 | cv::Size out_size; 76 | cv::Mat K, D, R; 77 | cv::Mat mapx, mapy; 78 | std::string distortion_model; // distortion model name 79 | 80 | Camera(){}; 81 | Camera(const ::eds::calib::CameraInfo &cam_info, const base::Quaterniond &rotation = base::Quaterniond::Identity()); 82 | void toDSOFormat(const std::string &filename="/tmp/dso_camera.txt"); 83 | void undistort(const cv::Mat &input, cv::Mat &output) 84 | { 85 | remap(input, output, mapx, mapy, cv::INTER_CUBIC); 86 | } 87 | double fx(){return K.at(0,0);} 88 | double fy(){return K.at(1,1);} 89 | double cx(){return K.at(0,2);} 90 | double cy(){return K.at(1,2);} 91 | std::vector intrinsics(){return {fx(), fy(), cx(), cy()};} 92 | }; 93 | 94 | ::eds::calib::CameraInfo readCameraCalib(YAML::Node cam_calib); 95 | ::eds::calib::DualCamera readDualCalibration(YAML::Node cam_calib); 96 | ::eds::calib::Camera setNewCamera(const ::eds::calib::Camera &cam0, const ::eds::calib::Camera &cam1); 97 | ::eds::calib::Camera setNewCamera(const ::eds::calib::Camera &cam0, const ::eds::calib::Camera &cam1, const cv::Size out_size); 98 | void getMapping(::eds::calib::Camera &cam0, ::eds::calib::Camera &cam1, ::eds::calib::Camera &newcam); 99 | void getEventUndistCoord (const ::eds::calib::Camera &event_camera, const ::eds::calib::Camera &new_camera, std::vector &undist_coord); 100 | 101 | } //calib namespace 102 | } // end namespace 103 | 104 | #endif // _EDS_CALIB_HPP_ -------------------------------------------------------------------------------- /src/utils/Colormap.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the EDS: Event-aided Direct Sparse Odometry 3 | * (https://rpg.ifi.uzh.ch/eds.html) 4 | * 5 | * Copyright (c) 2022 Javier Hidalgo-Carrió, Robotics and Perception 6 | * Group (RPG) University of Zurich. 7 | * 8 | * EDS is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, version 3. 11 | * 12 | * EDS is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #include "Colormap.hpp" 22 | 23 | namespace eds { namespace utils { 24 | 25 | void ColorMap::operator()(cv::InputArray _src, cv::OutputArray _dst) const 26 | { 27 | if(_lut.total() != 256) 28 | CV_Error(cv::Error::StsAssert, "cv::LUT only supports tables of size 256."); 29 | cv::Mat src = _src.getMat(); 30 | if(src.type() != CV_8UC1 && src.type() != CV_8UC3) 31 | CV_Error(cv::Error::StsBadArg, "cv::ColorMap only supports source images of type CV_8UC1 or CV_8UC3"); 32 | // Turn into a BGR matrix into its grayscale representation. 33 | if(src.type() == CV_8UC3) 34 | cvtColor(src.clone(), src, cv::COLOR_BGR2GRAY); 35 | cv::cvtColor(src.clone(), src, cv::COLOR_GRAY2BGR); 36 | // Apply the ColorMap. 37 | cv::LUT(src, _lut, _dst); 38 | } 39 | 40 | cv::Mat ColorMap::linear_colormap(cv::InputArray X, 41 | cv::InputArray r, cv::InputArray g, cv::InputArray b, 42 | cv::InputArray xi) { 43 | cv::Mat lut, lut8; 44 | cv::Mat planes[] = { 45 | interp1(X, b, xi), 46 | interp1(X, g, xi), 47 | interp1(X, r, xi)}; 48 | merge(planes, 3, lut); 49 | lut.convertTo(lut8, CV_8U, 255.); 50 | return lut8; 51 | } 52 | 53 | void BlueWhiteRed::init(int n) 54 | { 55 | 56 | static const float r[] = {0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1.}; 57 | static const float g[] = {0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.}; 58 | static const float b[] = {1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.85714286, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.57142857, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0.28571429, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.}; 59 | cv::Mat X = linspace(0,1,128); 60 | this->_lut = eds::utils::ColorMap::linear_colormap(X, 61 | cv::Mat(128,1, CV_32FC1, (void*)r).clone(), // red 62 | cv::Mat(128,1, CV_32FC1, (void*)g).clone(), // green 63 | cv::Mat(128,1, CV_32FC1, (void*)b).clone(), // blue 64 | n); // number of sample points 65 | } 66 | 67 | void BlueWhiteBlack::init(int n) 68 | { 69 | 70 | static const float r[] = {0., 0.03149606, 0.06299213, 0.09448819, 0.12598425, 0.15748031, 0.18897638, 0.22047244, 0.2519685 , 0.28346457, 0.31496063, 0.34645669, 0.37795276, 0.40944882, 0.44094488, 0.47244094, 0.50393701, 0.53543307, 0.56692913, 0.5984252 , 0.62992126, 0.66929134, 0.7007874 , 0.73228346, 0.76377953, 0.79527559, 0.82677165, 0.85826772, 0.88976378, 0.92125984, 0.95275591, 0.98425197, 0.98425197, 0.95275591, 0.92125984, 0.88976378, 0.85826772, 0.82677165, 0.79527559, 0.76377953, 0.73228346, 0.7007874 , 0.66141732, 0.62992126, 0.5984252 , 0.56692913, 0.53543307, 0.50393701, 0.47244094, 0.44094488, 0.40944882, 0.37795276, 0.34645669, 0.31496063, 0.28346457, 0.2519685 , 0.22047244, 0.18897638, 0.15748031, 0.12598425, 0.09448819, 0.06299213, 0.03149606, 0.0}; 71 | static const float g[] = {0., 0.03149606, 0.06299213, 0.09448819, 0.12598425, 0.15748031, 0.18897638, 0.22047244, 0.2519685 , 0.28346457, 0.31496063, 0.34645669, 0.37795276, 0.40944882, 0.44094488, 0.47244094, 0.50393701, 0.53543307, 0.56692913, 0.5984252 , 0.62992126, 0.66929134, 0.7007874 , 0.73228346, 0.76377953, 0.79527559, 0.82677165, 0.85826772, 0.88976378, 0.92125984, 0.95275591, 0.98425197, 0.98425197, 0.95275591, 0.92125984, 0.88976378, 0.85826772, 0.82677165, 0.79527559, 0.76377953, 0.73228346, 0.7007874 , 0.66141732, 0.62992126, 0.5984252 , 0.56692913, 0.53543307, 0.50393701, 0.47244094, 0.44094488, 0.40944882, 0.37795276, 0.34645669, 0.31496063, 0.28346457, 0.2519685 , 0.22047244, 0.18897638, 0.15748031, 0.12598425, 0.09448819, 0.06299213, 0.03149606, 0.0}; 72 | static const float b[] = {0., 0.03149606, 0.06299213, 0.09448819, 0.12598425, 0.15748031, 0.18897638, 0.22047244, 0.2519685 , 0.28346457, 0.31496063, 0.34645669, 0.37795276, 0.40944882, 0.44094488, 0.47244094, 0.50393701, 0.53543307, 0.56692913, 0.5984252 , 0.62992126, 0.66929134, 0.7007874 , 0.73228346, 0.76377953, 0.79527559, 0.82677165, 0.85826772, 0.88976378, 0.92125984, 0.95275591, 0.98425197, 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1.}; 73 | cv::Mat X = linspace(0,1,64); 74 | this->_lut = eds::utils::ColorMap::linear_colormap(X, 75 | cv::Mat(64,1, CV_32FC1, (void*)r).clone(), // red 76 | cv::Mat(64,1, CV_32FC1, (void*)g).clone(), // green 77 | cv::Mat(64,1, CV_32FC1, (void*)b).clone(), // blue 78 | n); // number of sample points 79 | } 80 | 81 | void GreenWhiteRed::init(int n) 82 | { 83 | 84 | static const float r[] = {1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 0.98425197, 0.95275591, 0.92125984, 0.88976378, 0.85826772, 0.82677165, 0.79527559, 0.76377953, 0.73228346, 0.7007874 , 0.66141732, 0.62992126, 0.5984252 , 0.56692913, 0.53543307, 0.50393701, 0.47244094, 0.44094488, 0.40944882, 0.37795276, 0.34645669, 0.31496063, 0.28346457, 0.2519685 , 0.22047244, 0.18897638, 0.15748031, 0.12598425, 0.09448819, 0.06299213, 0.03149606, 0.}; 85 | static const float g[] = {0., 0.03149606, 0.06299213, 0.09448819, 0.12598425, 0.15748031, 0.18897638, 0.22047244, 0.2519685 , 0.28346457, 0.31496063, 0.34645669, 0.37795276, 0.40944882, 0.44094488, 0.47244094, 0.50393701, 0.53543307, 0.56692913, 0.5984252 , 0.62992126, 0.66929134, 0.7007874 , 0.73228346, 0.76377953, 0.79527559, 0.82677165, 0.85826772, 0.88976378, 0.92125984, 0.95275591, 0.98425197, 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1.}; 86 | static const float b[] = {0., 0.03149606, 0.06299213, 0.09448819, 0.12598425, 0.15748031, 0.18897638, 0.22047244, 0.2519685 , 0.28346457, 0.31496063, 0.34645669, 0.37795276, 0.40944882, 0.44094488, 0.47244094, 0.50393701, 0.53543307, 0.56692913, 0.5984252 , 0.62992126, 0.66929134, 0.7007874 , 0.73228346, 0.76377953, 0.79527559, 0.82677165, 0.85826772, 0.88976378, 0.92125984, 0.95275591, 0.98425197, 0.98425197, 0.95275591, 0.92125984, 0.88976378, 0.85826772, 0.82677165, 0.79527559, 0.76377953, 0.73228346, 0.7007874 , 0.66141732, 0.62992126, 0.5984252 , 0.56692913, 0.53543307, 0.50393701, 0.47244094, 0.44094488, 0.40944882, 0.37795276, 0.34645669, 0.31496063, 0.28346457, 0.2519685 , 0.22047244, 0.18897638, 0.15748031, 0.12598425, 0.09448819, 0.06299213, 0.03149606, 0.}; 87 | cv::Mat X = linspace(0,1,64); 88 | this->_lut = eds::utils::ColorMap::linear_colormap(X, 89 | cv::Mat(64,1, CV_32FC1, (void*)r).clone(), // red 90 | cv::Mat(64,1, CV_32FC1, (void*)g).clone(), // green 91 | cv::Mat(64,1, CV_32FC1, (void*)b).clone(), // blue 92 | n); // number of sample points 93 | } 94 | }} //end namespace eds::utils -------------------------------------------------------------------------------- /src/utils/Colormap.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the EDS: Event-aided Direct Sparse Odometry 3 | * (https://rpg.ifi.uzh.ch/eds.html) 4 | * 5 | * Copyright (c) 2022 Javier Hidalgo-Carrió, Robotics and Perception 6 | * Group (RPG) University of Zurich. 7 | * 8 | * EDS is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, version 3. 11 | * 12 | * EDS is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | 22 | #ifndef _EDS_COLORMAP_HPP_ 23 | #define _EDS_COLORMAP_HPP_ 24 | 25 | #include 26 | 27 | namespace eds { namespace utils { 28 | 29 | class ColorMap 30 | { 31 | 32 | protected: 33 | cv::Mat _lut; 34 | 35 | public: 36 | virtual ~ColorMap() {} 37 | 38 | // Applies the colormap on a given image. 39 | // 40 | // This function expects BGR-aligned data of type CV_8UC1 or CV_8UC3. 41 | // Throws an error for wrong-aligned lookup table, which must be 42 | // of size 256 in the latest OpenCV release (2.3.1). 43 | void operator()(cv::InputArray src, cv::OutputArray dst) const; 44 | 45 | // Setup base map to interpolate from. 46 | //not used: virtual void init(int n) = 0; 47 | 48 | // Interpolates from a base colormap. 49 | static cv::Mat linear_colormap(cv::InputArray X, 50 | cv::InputArray r, cv::InputArray g, cv::InputArray b, 51 | int n) { 52 | return linear_colormap(X,r,g,b,linspace(0,1,n)); 53 | } 54 | 55 | // Interpolates from a base colormap. 56 | static cv::Mat linear_colormap(cv::InputArray X, 57 | cv::InputArray r, cv::InputArray g, cv::InputArray b, 58 | float begin, float end, float n) { 59 | return linear_colormap(X,r,g,b,linspace(begin,end, cvRound(n))); 60 | } 61 | 62 | // Interpolates from a base colormap. 63 | static cv::Mat linear_colormap(cv::InputArray X, 64 | cv::InputArray r, cv::InputArray g, cv::InputArray b, 65 | cv::InputArray xi); 66 | }; 67 | 68 | 69 | 70 | class BlueWhiteRed : public ColorMap 71 | { 72 | public: 73 | BlueWhiteRed():ColorMap() { 74 | init(256); 75 | } 76 | BlueWhiteRed(int n) : ColorMap() { 77 | init(n); 78 | } 79 | void init(int n); 80 | }; 81 | 82 | class BlueWhiteBlack : public ColorMap 83 | { 84 | public: 85 | BlueWhiteBlack():ColorMap() { 86 | init(256); 87 | } 88 | BlueWhiteBlack(int n) : ColorMap() { 89 | init(n); 90 | } 91 | void init(int n); 92 | }; 93 | 94 | class GreenWhiteRed : public ColorMap 95 | { 96 | public: 97 | GreenWhiteRed():ColorMap() { 98 | init(256); 99 | } 100 | GreenWhiteRed(int n) : ColorMap() { 101 | init(n); 102 | } 103 | void init(int n); 104 | }; 105 | 106 | } //utils namespace 107 | } // end namespace 108 | 109 | #endif // _EDS_COLORMAP_HPP_ -------------------------------------------------------------------------------- /src/utils/Config.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the EDS: Event-aided Direct Sparse Odometry 3 | * (https://rpg.ifi.uzh.ch/eds.html) 4 | * 5 | * Copyright (c) 2022 Javier Hidalgo-Carrió, Robotics and Perception 6 | * Group (RPG) University of Zurich. 7 | * 8 | * EDS is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, version 3. 11 | * 12 | * EDS is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #ifndef _EDS_UTILS_CONFIG_HPP_ 22 | #define _EDS_UTILS_CONFIG_HPP_ 23 | 24 | #include 25 | #include 26 | 27 | namespace eds { namespace recorder{ 28 | 29 | struct Visual 30 | { 31 | bool model; 32 | bool events; 33 | bool depth; 34 | bool flow; 35 | bool sigma; 36 | bool weights; 37 | bool dsi; 38 | bool map; 39 | }; 40 | 41 | struct Config 42 | { 43 | std::string output_folder; 44 | std::string poses_filename; 45 | std::string velos_filename; 46 | Visual viz; 47 | }; 48 | 49 | inline ::eds::recorder::Config readRecorderConfig(YAML::Node config) 50 | { 51 | ::eds::recorder::Config recorder_config; 52 | 53 | return recorder_config; 54 | }; 55 | 56 | } //recorder namespace 57 | } // end namespace 58 | #endif -------------------------------------------------------------------------------- /src/utils/FrameShell.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO 3 | * 4 | * This file is modified and part of the EDS 5 | * (https://rpg.ifi.uzh.ch/eds.html) 6 | * 7 | * Copyright 2016 Technical University of Munich and Intel. 8 | * Developed by Jakob Engel , 9 | * for more information see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above website. 12 | * 13 | * DSO is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSO is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSO. If not, see . 25 | */ 26 | 27 | 28 | #pragma once 29 | 30 | #include "eds/utils/NumType.h" 31 | #include "algorithm" 32 | 33 | namespace dso 34 | { 35 | 36 | 37 | class FrameShell 38 | { 39 | public: 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 41 | int id; // INTERNAL ID, starting at zero. 42 | int incoming_id; // ID passed into DSO 43 | double timestamp; // timestamp passed into DSO. 44 | 45 | // set once after tracking 46 | SE3 camToTrackingRef; 47 | FrameShell* trackingRef; 48 | 49 | // constantly adapted. 50 | SE3 camToWorld; // Write: TRACKING, while frame is still fresh; MAPPING: only when locked [shellPoseMutex]. 51 | AffLight aff_g2l; 52 | bool poseValid; 53 | 54 | // statisitcs 55 | int statistics_outlierResOnThis; 56 | int statistics_goodResOnThis; 57 | int marginalizedAt; 58 | double movedByOpt; 59 | 60 | 61 | inline FrameShell() 62 | { 63 | id=0; 64 | poseValid=true; 65 | camToWorld = SE3(); 66 | timestamp=0; 67 | marginalizedAt=-1; 68 | movedByOpt=0; 69 | statistics_outlierResOnThis=statistics_goodResOnThis=0; 70 | trackingRef=0; 71 | camToTrackingRef = SE3(); 72 | } 73 | }; 74 | 75 | 76 | } 77 | 78 | -------------------------------------------------------------------------------- /src/utils/ImageAndExposure.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * This file is modified and part of the EDS 5 | * (https://rpg.ifi.uzh.ch/eds.html) 6 | * 7 | * Copyright 2016 Technical University of Munich and Intel. 8 | * Developed by Jakob Engel , 9 | * for more information see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above website. 12 | * 13 | * DSO is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSO is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSO. If not, see . 25 | */ 26 | 27 | 28 | #pragma once 29 | #include 30 | #include 31 | 32 | 33 | namespace dso 34 | { 35 | 36 | 37 | class ImageAndExposure 38 | { 39 | public: 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 41 | float* image; // irradiance. between 0 and 256 42 | int w,h; // width and height; 43 | double timestamp; 44 | float exposure_time; // exposure time in ms. 45 | inline ImageAndExposure(int w_, int h_, double timestamp_=0) : w(w_), h(h_), timestamp(timestamp_) 46 | { 47 | image = new float[w*h]; 48 | exposure_time=1; 49 | } 50 | inline ~ImageAndExposure() 51 | { 52 | delete[] image; 53 | } 54 | 55 | inline void copyMetaTo(ImageAndExposure &other) 56 | { 57 | other.exposure_time = exposure_time; 58 | } 59 | 60 | inline ImageAndExposure* getDeepCopy() 61 | { 62 | ImageAndExposure* img = new ImageAndExposure(w,h,timestamp); 63 | img->exposure_time = exposure_time; 64 | memcpy(img->image, image, w*h*sizeof(float)); 65 | return img; 66 | } 67 | }; 68 | 69 | 70 | } 71 | -------------------------------------------------------------------------------- /src/utils/IndexThreadReduce.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * This file is modified and part of the EDS 5 | * (https://rpg.ifi.uzh.ch/eds.html) 6 | * 7 | * Copyright 2016 Technical University of Munich and Intel. 8 | * Developed by Jakob Engel , 9 | * for more information see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above website. 12 | * 13 | * DSO is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSO is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSO. If not, see . 25 | */ 26 | 27 | 28 | 29 | #pragma once 30 | #include "eds/utils/settings.h" 31 | #include "boost/thread.hpp" 32 | #include 33 | #include 34 | 35 | 36 | 37 | namespace dso 38 | { 39 | 40 | template 41 | class IndexThreadReduce 42 | { 43 | 44 | public: 45 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 46 | 47 | inline IndexThreadReduce() 48 | { 49 | nextIndex = 0; 50 | maxIndex = 0; 51 | stepSize = 1; 52 | callPerIndex = boost::bind(&IndexThreadReduce::callPerIndexDefault, this, _1, _2, _3, _4); 53 | 54 | running = true; 55 | for(int i=0;i callPerIndex, int first, int end, int stepSize = 0) 80 | { 81 | 82 | stats={}; 83 | 84 | 85 | 86 | if(stepSize == 0) 87 | stepSize = ((end-first)+NUM_THREADS-1)/NUM_THREADS; 88 | 89 | 90 | 91 | boost::unique_lock lock(exMutex); 92 | 93 | // save 94 | this->callPerIndex = callPerIndex; 95 | nextIndex = first; 96 | maxIndex = end; 97 | this->stepSize = stepSize; 98 | 99 | // go worker threads! 100 | for(int i=0;icallPerIndex = boost::bind(&IndexThreadReduce::callPerIndexDefault, this, _1, _2, _3, _4); 131 | 132 | //printf("reduce done (all threads finished)\n"); 133 | } 134 | 135 | Running stats; 136 | 137 | private: 138 | boost::thread workerThreads[NUM_THREADS]; 139 | bool isDone[NUM_THREADS]; 140 | bool gotOne[NUM_THREADS]; 141 | 142 | boost::mutex exMutex; 143 | boost::condition_variable todo_signal; 144 | boost::condition_variable done_signal; 145 | 146 | int nextIndex; 147 | int maxIndex; 148 | int stepSize; 149 | 150 | bool running; 151 | 152 | boost::function callPerIndex; 153 | 154 | void callPerIndexDefault(int i, int j,Running* k, int tid) 155 | { 156 | printf("ERROR: should never be called....\n"); 157 | assert(false); 158 | } 159 | 160 | void workerLoop(int idx) 161 | { 162 | boost::unique_lock lock(exMutex); 163 | 164 | while(running) 165 | { 166 | // try to get something to do. 167 | int todo = 0; 168 | bool gotSomething = false; 169 | if(nextIndex < maxIndex) 170 | { 171 | // got something! 172 | todo = nextIndex; 173 | nextIndex+=stepSize; 174 | gotSomething = true; 175 | } 176 | 177 | // if got something: do it (unlock in the meantime) 178 | if(gotSomething) 179 | { 180 | lock.unlock(); 181 | 182 | assert(callPerIndex != 0); 183 | 184 | Running s = {}; 185 | callPerIndex(todo, std::min(todo+stepSize, maxIndex), &s, idx); 186 | gotOne[idx] = true; 187 | lock.lock(); 188 | stats += s; 189 | } 190 | 191 | // otherwise wait on signal, releasing lock in the meantime. 192 | else 193 | { 194 | if(!gotOne[idx]) 195 | { 196 | lock.unlock(); 197 | assert(callPerIndex != 0); 198 | Running s = {}; 199 | callPerIndex(0, 0, &s, idx); 200 | gotOne[idx] = true; 201 | lock.lock(); 202 | stats += s; 203 | } 204 | isDone[idx] = true; 205 | //printf("worker %d waiting..\n", idx); 206 | done_signal.notify_all(); 207 | todo_signal.wait(lock); 208 | } 209 | } 210 | } 211 | }; 212 | } 213 | -------------------------------------------------------------------------------- /src/utils/KDTree.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the EDS: Event-aided Direct Sparse Odometry 3 | * (https://rpg.ifi.uzh.ch/eds.html) 4 | * 5 | * This is taken from https://github.com/gishi523/kd-tree 6 | * 7 | */ 8 | 9 | #ifndef __EDS_KDTREE_H__ 10 | #define __EDS_KDTREE_H__ 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | namespace eds { namespace mapping { 20 | /** @brief k-d tree class. 21 | */ 22 | template 23 | class KDTree 24 | { 25 | public: 26 | /** @brief The constructors. 27 | */ 28 | KDTree() : root_(nullptr) {}; 29 | KDTree(const std::vector& points) : root_(nullptr) { build(points); } 30 | template KDTree(Iter &begin, Iter &end) 31 | : root_(nullptr) { build(begin, end); } 32 | 33 | /** @brief The destructor. 34 | */ 35 | ~KDTree() { clear(); } 36 | 37 | /** @brief Re-builds k-d tree. 38 | */ 39 | void build(const std::vector& points) 40 | { 41 | clear(); 42 | 43 | points_ = points; 44 | 45 | std::vector indices(points.size()); 46 | std::iota(std::begin(indices), std::end(indices), 0); 47 | 48 | root_ = buildRecursive(indices.data(), (int)points.size(), 0); 49 | } 50 | 51 | /** @brief Re-builds k-d tree. 52 | */ 53 | template void build(Iter& begin, Iter &end) 54 | { 55 | clear(); 56 | points_.resize(std::distance(begin, end)); 57 | std::copy(begin, end, points_.begin()); 58 | 59 | std::vector indices(points_.size()); 60 | std::iota(std::begin(indices), std::end(indices), 0); 61 | 62 | root_ = buildRecursive(indices.data(), (int)points_.size(), 0); 63 | } 64 | 65 | /** @brief Clears k-d tree. 66 | */ 67 | void clear() 68 | { 69 | clearRecursive(root_); 70 | root_ = nullptr; 71 | points_.clear(); 72 | } 73 | 74 | /** @brief NUmber of points. 75 | */ 76 | size_t size() 77 | { 78 | return points_.size(); 79 | } 80 | 81 | /** @brief Validates k-d tree. 82 | */ 83 | bool validate() const 84 | { 85 | try 86 | { 87 | validateRecursive(root_, 0); 88 | } 89 | catch (const Exception&) 90 | { 91 | return false; 92 | } 93 | 94 | return true; 95 | } 96 | 97 | /** @brief Searches the nearest neighbor. 98 | */ 99 | int nnSearch(const PointT& query, double* minDist = nullptr) const 100 | { 101 | int guess; 102 | double _minDist = std::numeric_limits::max(); 103 | 104 | nnSearchRecursive(query, root_, &guess, &_minDist); 105 | 106 | if (minDist) 107 | *minDist = _minDist; 108 | 109 | return guess; 110 | } 111 | 112 | /** @brief Searches k-nearest neighbors. 113 | */ 114 | std::vector knnSearch(const PointT& query, int k) const 115 | { 116 | KnnQueue queue(k); 117 | knnSearchRecursive(query, root_, queue, k); 118 | 119 | std::vector indices(queue.size()); 120 | for (size_t i = 0; i < queue.size(); i++) 121 | indices[i] = queue[i].second; 122 | 123 | return indices; 124 | } 125 | 126 | /** @brief Searches neighbors within radius. 127 | */ 128 | std::vector radiusSearch(const PointT& query, double radius) const 129 | { 130 | std::vector indices; 131 | radiusSearchRecursive(query, root_, indices, radius); 132 | return indices; 133 | } 134 | 135 | private: 136 | 137 | /** @brief k-d tree node. 138 | */ 139 | struct Node 140 | { 141 | int idx; //!< index to the original point 142 | Node* next[2]; //!< pointers to the child nodes 143 | int axis; //!< dimension's axis 144 | 145 | Node() : idx(-1), axis(-1) { next[0] = next[1] = nullptr; } 146 | }; 147 | 148 | /** @brief k-d tree exception. 149 | */ 150 | class Exception : public std::exception { using std::exception::exception; }; 151 | 152 | /** @brief Bounded priority queue. 153 | */ 154 | template > 155 | class BoundedPriorityQueue 156 | { 157 | public: 158 | 159 | BoundedPriorityQueue() = delete; 160 | BoundedPriorityQueue(size_t bound) : bound_(bound) { elements_.reserve(bound + 1); }; 161 | 162 | void push(const T& val) 163 | { 164 | auto it = std::find_if(std::begin(elements_), std::end(elements_), 165 | [&](const T& element){ return Compare()(val, element); }); 166 | elements_.insert(it, val); 167 | 168 | if (elements_.size() > bound_) 169 | elements_.resize(bound_); 170 | } 171 | 172 | const T& back() const { return elements_.back(); }; 173 | const T& operator[](size_t index) const { return elements_[index]; } 174 | size_t size() const { return elements_.size(); } 175 | 176 | private: 177 | size_t bound_; 178 | std::vector elements_; 179 | }; 180 | 181 | /** @brief Priority queue of pair. 182 | */ 183 | using KnnQueue = BoundedPriorityQueue>; 184 | 185 | /** @brief Builds k-d tree recursively. 186 | */ 187 | Node* buildRecursive(int* indices, int npoints, int depth) 188 | { 189 | if (npoints <= 0) 190 | return nullptr; 191 | 192 | const int axis = depth % PointT::DIM; 193 | const int mid = (npoints - 1) / 2; 194 | 195 | std::nth_element(indices, indices + mid, indices + npoints, [&](int lhs, int rhs) 196 | { 197 | return points_[lhs][axis] < points_[rhs][axis]; 198 | }); 199 | 200 | Node* node = new Node(); 201 | node->idx = indices[mid]; 202 | node->axis = axis; 203 | 204 | node->next[0] = buildRecursive(indices, mid, depth + 1); 205 | node->next[1] = buildRecursive(indices + mid + 1, npoints - mid - 1, depth + 1); 206 | 207 | return node; 208 | } 209 | 210 | /** @brief Clears k-d tree recursively. 211 | */ 212 | void clearRecursive(Node* node) 213 | { 214 | if (node == nullptr) 215 | return; 216 | 217 | if (node->next[0]) 218 | clearRecursive(node->next[0]); 219 | 220 | if (node->next[1]) 221 | clearRecursive(node->next[1]); 222 | 223 | delete node; 224 | } 225 | 226 | /** @brief Validates k-d tree recursively. 227 | */ 228 | void validateRecursive(const Node* node, int depth) const 229 | { 230 | if (node == nullptr) 231 | return; 232 | 233 | const int axis = node->axis; 234 | const Node* node0 = node->next[0]; 235 | const Node* node1 = node->next[1]; 236 | 237 | if (node0 && node1) 238 | { 239 | if (points_[node->idx][axis] < points_[node0->idx][axis]) 240 | throw Exception(); 241 | 242 | if (points_[node->idx][axis] > points_[node1->idx][axis]) 243 | throw Exception(); 244 | } 245 | 246 | if (node0) 247 | validateRecursive(node0, depth + 1); 248 | 249 | if (node1) 250 | validateRecursive(node1, depth + 1); 251 | } 252 | 253 | static double distance(const PointT& p, const PointT& q) 254 | { 255 | double dist = 0; 256 | for (size_t i = 0; i < PointT::DIM; i++) 257 | dist += (p[i] - q[i]) * (p[i] - q[i]); 258 | return sqrt(dist); 259 | } 260 | 261 | /** @brief Searches the nearest neighbor recursively. 262 | */ 263 | void nnSearchRecursive(const PointT& query, const Node* node, int *guess, double *minDist) const 264 | { 265 | if (node == nullptr) 266 | return; 267 | 268 | const PointT& train = points_[node->idx]; 269 | 270 | const double dist = distance(query, train); 271 | if (dist < *minDist) 272 | { 273 | *minDist = dist; 274 | *guess = node->idx; 275 | } 276 | 277 | const int axis = node->axis; 278 | const int dir = query[axis] < train[axis] ? 0 : 1; 279 | nnSearchRecursive(query, node->next[dir], guess, minDist); 280 | 281 | const double diff = fabs(query[axis] - train[axis]); 282 | if (diff < *minDist) 283 | nnSearchRecursive(query, node->next[!dir], guess, minDist); 284 | } 285 | 286 | /** @brief Searches k-nearest neighbors recursively. 287 | */ 288 | void knnSearchRecursive(const PointT& query, const Node* node, KnnQueue& queue, int k) const 289 | { 290 | if (node == nullptr) 291 | return; 292 | 293 | const PointT& train = points_[node->idx]; 294 | 295 | const double dist = distance(query, train); 296 | queue.push(std::make_pair(dist, node->idx)); 297 | 298 | const int axis = node->axis; 299 | const int dir = query[axis] < train[axis] ? 0 : 1; 300 | knnSearchRecursive(query, node->next[dir], queue, k); 301 | 302 | const double diff = fabs(query[axis] - train[axis]); 303 | if ((int)queue.size() < k || diff < queue.back().first) 304 | knnSearchRecursive(query, node->next[!dir], queue, k); 305 | } 306 | 307 | /** @brief Searches neighbors within radius. 308 | */ 309 | void radiusSearchRecursive(const PointT& query, const Node* node, std::vector& indices, double radius) const 310 | { 311 | if (node == nullptr) 312 | return; 313 | 314 | const PointT& train = points_[node->idx]; 315 | 316 | const double dist = distance(query, train); 317 | if (dist < radius) 318 | indices.push_back(node->idx); 319 | 320 | const int axis = node->axis; 321 | const int dir = query[axis] < train[axis] ? 0 : 1; 322 | radiusSearchRecursive(query, node->next[dir], indices, radius); 323 | 324 | const double diff = fabs(query[axis] - train[axis]); 325 | if (diff < radius) 326 | radiusSearchRecursive(query, node->next[!dir], indices, radius); 327 | } 328 | 329 | Node* root_; //!< root node 330 | std::vector points_; //!< points 331 | }; 332 | } //utils namespace 333 | } // end namespace 334 | 335 | #endif // !__KDTREE_H__ 336 | -------------------------------------------------------------------------------- /src/utils/MinimalImage.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | #include "eds/utils/NumType.h" 28 | #include "algorithm" 29 | 30 | namespace dso 31 | { 32 | 33 | template 34 | class MinimalImage 35 | { 36 | public: 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 38 | int w; 39 | int h; 40 | T* data; 41 | 42 | /* 43 | * creates minimal image with own memory 44 | */ 45 | inline MinimalImage(int w_, int h_) : w(w_), h(h_) 46 | { 47 | data = new T[w*h]; 48 | ownData=true; 49 | } 50 | 51 | /* 52 | * creates minimal image wrapping around existing memory 53 | */ 54 | inline MinimalImage(int w_, int h_, T* data_) : w(w_), h(h_) 55 | { 56 | data = data_; 57 | ownData=false; 58 | } 59 | 60 | inline ~MinimalImage() 61 | { 62 | if(ownData) delete [] data; 63 | } 64 | 65 | inline MinimalImage* getClone() 66 | { 67 | MinimalImage* clone = new MinimalImage(w,h); 68 | memcpy(clone->data, data, sizeof(T)*w*h); 69 | return clone; 70 | } 71 | 72 | 73 | inline T& at(int x, int y) {return data[(int)x+((int)y)*w];} 74 | inline T& at(int i) {return data[i];} 75 | 76 | inline void setBlack() 77 | { 78 | memset(data, 0, sizeof(T)*w*h); 79 | } 80 | 81 | inline void setConst(T val) 82 | { 83 | for(int i=0;i Vec3b; 134 | typedef MinimalImage MinimalImageF; 135 | typedef MinimalImage MinimalImageF3; 136 | typedef MinimalImage MinimalImageB; 137 | typedef MinimalImage MinimalImageB3; 138 | typedef MinimalImage MinimalImageB16; 139 | 140 | } 141 | 142 | -------------------------------------------------------------------------------- /src/utils/NumType.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | #include "Eigen/Core" 28 | #include "eds/sophus/sim3.hpp" 29 | #include "eds/sophus/se3.hpp" 30 | #include 31 | 32 | 33 | namespace dso 34 | { 35 | 36 | // CAMERA MODEL TO USE 37 | 38 | 39 | #define SSEE(val,idx) (*(((float*)&val)+idx)) 40 | 41 | 42 | #define MAX_RES_PER_POINT 8 43 | #define NUM_THREADS 6 44 | 45 | 46 | #define todouble(x) (x).cast() 47 | 48 | 49 | typedef Sophus::SE3d SE3; 50 | typedef Sophus::Sim3d Sim3; 51 | typedef Sophus::SO3d SO3; 52 | 53 | 54 | 55 | #define CPARS 4 56 | 57 | 58 | typedef Eigen::Matrix MatXX; 59 | typedef Eigen::Matrix MatCC; 60 | #define MatToDynamic(x) MatXX(x) 61 | 62 | 63 | 64 | typedef Eigen::Matrix MatC10; 65 | typedef Eigen::Matrix Mat1010; 66 | typedef Eigen::Matrix Mat1313; 67 | 68 | typedef Eigen::Matrix Mat810; 69 | typedef Eigen::Matrix Mat83; 70 | typedef Eigen::Matrix Mat66; 71 | typedef Eigen::Matrix Mat53; 72 | typedef Eigen::Matrix Mat43; 73 | typedef Eigen::Matrix Mat42; 74 | typedef Eigen::Matrix Mat33; 75 | typedef Eigen::Matrix Mat22; 76 | typedef Eigen::Matrix Mat8C; 77 | typedef Eigen::Matrix MatC8; 78 | typedef Eigen::Matrix Mat8Cf; 79 | typedef Eigen::Matrix MatC8f; 80 | 81 | typedef Eigen::Matrix Mat88; 82 | typedef Eigen::Matrix Mat77; 83 | 84 | typedef Eigen::Matrix VecC; 85 | typedef Eigen::Matrix VecCf; 86 | typedef Eigen::Matrix Vec13; 87 | typedef Eigen::Matrix Vec10; 88 | typedef Eigen::Matrix Vec9; 89 | typedef Eigen::Matrix Vec8; 90 | typedef Eigen::Matrix Vec7; 91 | typedef Eigen::Matrix Vec6; 92 | typedef Eigen::Matrix Vec5; 93 | typedef Eigen::Matrix Vec4; 94 | typedef Eigen::Matrix Vec3; 95 | typedef Eigen::Matrix Vec2; 96 | typedef Eigen::Matrix VecX; 97 | 98 | typedef Eigen::Matrix Mat33f; 99 | typedef Eigen::Matrix Mat103f; 100 | typedef Eigen::Matrix Mat22f; 101 | typedef Eigen::Matrix Vec3f; 102 | typedef Eigen::Matrix Vec2f; 103 | typedef Eigen::Matrix Vec6f; 104 | 105 | 106 | 107 | typedef Eigen::Matrix Mat49; 108 | typedef Eigen::Matrix Mat89; 109 | 110 | typedef Eigen::Matrix Mat94; 111 | typedef Eigen::Matrix Mat98; 112 | 113 | typedef Eigen::Matrix Mat81; 114 | typedef Eigen::Matrix Mat18; 115 | typedef Eigen::Matrix Mat91; 116 | typedef Eigen::Matrix Mat19; 117 | 118 | 119 | typedef Eigen::Matrix Mat84; 120 | typedef Eigen::Matrix Mat48; 121 | typedef Eigen::Matrix Mat44; 122 | 123 | 124 | typedef Eigen::Matrix VecNRf; 125 | typedef Eigen::Matrix Vec12f; 126 | typedef Eigen::Matrix Mat18f; 127 | typedef Eigen::Matrix Mat66f; 128 | typedef Eigen::Matrix Mat88f; 129 | typedef Eigen::Matrix Mat84f; 130 | typedef Eigen::Matrix Vec8f; 131 | typedef Eigen::Matrix Vec10f; 132 | typedef Eigen::Matrix Mat66f; 133 | typedef Eigen::Matrix Vec4f; 134 | typedef Eigen::Matrix Mat44f; 135 | typedef Eigen::Matrix Mat1212f; 136 | typedef Eigen::Matrix Vec12f; 137 | typedef Eigen::Matrix Mat1313f; 138 | typedef Eigen::Matrix Mat1010f; 139 | typedef Eigen::Matrix Vec13f; 140 | typedef Eigen::Matrix Mat99f; 141 | typedef Eigen::Matrix Vec9f; 142 | 143 | typedef Eigen::Matrix Mat42f; 144 | typedef Eigen::Matrix Mat62f; 145 | typedef Eigen::Matrix Mat12f; 146 | 147 | typedef Eigen::Matrix VecXf; 148 | typedef Eigen::Matrix MatXXf; 149 | 150 | 151 | typedef Eigen::Matrix MatPCPC; 152 | typedef Eigen::Matrix MatPCPCf; 153 | typedef Eigen::Matrix VecPC; 154 | typedef Eigen::Matrix VecPCf; 155 | 156 | typedef Eigen::Matrix Mat1414f; 157 | typedef Eigen::Matrix Vec14f; 158 | typedef Eigen::Matrix Mat1414; 159 | typedef Eigen::Matrix Vec14; 160 | 161 | 162 | 163 | 164 | 165 | 166 | // transforms points from one frame to another. 167 | struct AffLight 168 | { 169 | AffLight(double a_, double b_) : a(a_), b(b_) {}; 170 | AffLight() : a(0), b(0) {}; 171 | 172 | // Affine Parameters: 173 | double a,b; // I_frame = exp(a)*I_global + b. // I_global = exp(-a)*(I_frame - b). 174 | 175 | static Vec2 fromToVecExposure(float exposureF, float exposureT, AffLight g2F, AffLight g2T) 176 | { 177 | if(exposureF==0 || exposureT==0) 178 | { 179 | exposureT = exposureF = 1; 180 | //printf("got exposure value of 0! please choose the correct model.\n"); 181 | //assert(setting_brightnessTransferFunc < 2); 182 | } 183 | 184 | double a = exp(g2T.a-g2F.a) * exposureT / exposureF; 185 | double b = g2T.b - a*g2F.b; 186 | return Vec2(a,b); 187 | } 188 | 189 | Vec2 vec() 190 | { 191 | return Vec2(a,b); 192 | } 193 | }; 194 | 195 | inline base::Transform3d SE3ToBaseTransform(const SE3 &se3, const base::Transform3d &pre_trans = ::base::Transform3d::Identity()) 196 | { 197 | base::Quaterniond q; 198 | q.x() = se3.so3().unit_quaternion().x(); 199 | q.y() = se3.so3().unit_quaternion().y(); 200 | q.z() = se3.so3().unit_quaternion().z(); 201 | q.w() = se3.so3().unit_quaternion().w(); 202 | base::Transform3d pose(q); 203 | pose.translation() = se3.translation().transpose(); 204 | return pre_trans * pose; 205 | }; 206 | 207 | inline SE3 BaseTransformToSE3(const base::Transform3d &pose) 208 | { 209 | SE3 se3(pose.rotation(), pose.translation()); 210 | return se3; 211 | }; 212 | 213 | } -------------------------------------------------------------------------------- /src/utils/Transforms.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the EDS: Event-aided Direct Sparse Odometry 3 | * (https://rpg.ifi.uzh.ch/eds.html) 4 | * 5 | * Copyright (c) 2022 Javier Hidalgo-Carrió, Robotics and Perception 6 | * Group (RPG) University of Zurich. 7 | * 8 | * EDS is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, version 3. 11 | * 12 | * EDS is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | 22 | 23 | #ifndef _EDS_TRANSFORMS_HPP_ 24 | #define _EDS_TRANSFORMS_HPP_ 25 | 26 | /** Base types **/ 27 | #include 28 | #include 29 | #include 30 | 31 | namespace eds { namespace transforms { 32 | 33 | inline Eigen::Matrix3d axis_angle(Eigen::Vector3d &axis, double &theta) 34 | { 35 | Eigen::Matrix3d R; 36 | if (theta*theta > 1e-06) 37 | { 38 | double wx = axis[0]; double wy = axis[1]; double wz = axis[2]; 39 | double costheta = cos(theta); double sintheta = sin(theta); 40 | 41 | double c_1 = 1.0 - costheta; 42 | double wx_sintheta = wx * sintheta; 43 | double wy_sintheta = wy * sintheta; 44 | double wz_sintheta = wz * sintheta; 45 | double C00 = c_1 * wx * wx; 46 | double C01 = c_1 * wx * wy; 47 | double C02 = c_1 * wx * wz; 48 | double C11 = c_1 * wy * wy; 49 | double C12 = c_1 * wy * wz; 50 | double C22 = c_1 * wz * wz; 51 | 52 | R(0,0) = costheta + C00; 53 | R(1,0) = wz_sintheta + C01; 54 | R(2,0) = -wy_sintheta + C02; 55 | R(0,1) = -wz_sintheta + C01; 56 | R(1,1) = costheta + C11; 57 | R(2,1) = wx_sintheta + C12; 58 | R(0,2) = wy_sintheta + C02; 59 | R(1,2) = -wx_sintheta + C12; 60 | R(2,2) = costheta + C22; 61 | return R; 62 | } 63 | else 64 | { 65 | Eigen::Vector3d rotvec = axis*theta; 66 | R(0,0) = 1.0; 67 | R(1,0) = rotvec[2]; 68 | R(2,0) = -rotvec[1]; 69 | R(0,1) = -rotvec[2]; 70 | R(1,1) = 1.0; 71 | R(2,1) = rotvec[0]; 72 | R(0,2) = rotvec[1]; 73 | R(1,2) = -rotvec[0]; 74 | R(2,2) = 1.0; 75 | return R; 76 | 77 | } 78 | }; 79 | 80 | } //transforms namespace 81 | } // end namespace 82 | 83 | #endif 84 | 85 | -------------------------------------------------------------------------------- /src/utils/Undistort.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | #include "eds/utils/ImageAndExposure.h" 28 | #include "eds/utils/MinimalImage.h" 29 | #include "eds/utils/NumType.h" 30 | #include "Eigen/Core" 31 | 32 | 33 | 34 | 35 | 36 | namespace dso 37 | { 38 | 39 | 40 | class PhotometricUndistorter 41 | { 42 | public: 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 44 | PhotometricUndistorter(std::string file, std::string noiseImage, std::string vignetteImage, int w_, int h_); 45 | ~PhotometricUndistorter(); 46 | 47 | // removes readout noise, and converts to irradiance. 48 | // affine normalizes values to 0 <= I < 256. 49 | // raw irradiance = a*I + b. 50 | // output will be written in [output]. 51 | template void processFrame(T* image_in, float exposure_time, float factor=1); 52 | void unMapFloatImage(float* image); 53 | 54 | ImageAndExposure* output; 55 | 56 | float* getG() {if(!valid) return 0; else return G;}; 57 | private: 58 | float G[256*256]; 59 | int GDepth; 60 | float* vignetteMap; 61 | float* vignetteMapInv; 62 | int w,h; 63 | bool valid; 64 | }; 65 | 66 | 67 | class Undistort 68 | { 69 | public: 70 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 71 | virtual ~Undistort(); 72 | 73 | virtual void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const = 0; 74 | 75 | 76 | inline const Mat33 getK() const {return K;}; 77 | inline const Eigen::Vector2i getSize() const {return Eigen::Vector2i(w,h);}; 78 | inline const VecX getOriginalParameter() const {return parsOrg;}; 79 | inline const Eigen::Vector2i getOriginalSize() {return Eigen::Vector2i(wOrg,hOrg);}; 80 | inline bool isValid() {return valid;}; 81 | 82 | template 83 | ImageAndExposure* undistort(const MinimalImage* image_raw, float exposure=0, double timestamp=0, float factor=1) const; 84 | static Undistort* getUndistorterForFile(std::string configFilename, std::string gammaFilename, std::string vignetteFilename); 85 | 86 | void loadPhotometricCalibration(std::string file, std::string noiseImage, std::string vignetteImage); 87 | 88 | PhotometricUndistorter* photometricUndist; 89 | 90 | protected: 91 | int w, h, wOrg, hOrg, wUp, hUp; 92 | int upsampleUndistFactor; 93 | Mat33 K; 94 | VecX parsOrg; 95 | bool valid; 96 | bool passthrough; 97 | 98 | float* remapX; 99 | float* remapY; 100 | 101 | void applyBlurNoise(float* img) const; 102 | 103 | void makeOptimalK_crop(); 104 | void makeOptimalK_full(); 105 | 106 | void readFromFile(const char* configFileName, int nPars, std::string prefix = ""); 107 | }; 108 | 109 | class UndistortFOV : public Undistort 110 | { 111 | public: 112 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 113 | 114 | UndistortFOV(const char* configFileName, bool noprefix); 115 | ~UndistortFOV(); 116 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; 117 | 118 | }; 119 | 120 | class UndistortRadTan : public Undistort 121 | { 122 | public: 123 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 124 | UndistortRadTan(const char* configFileName, bool noprefix); 125 | ~UndistortRadTan(); 126 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; 127 | 128 | }; 129 | 130 | class UndistortEquidistant : public Undistort 131 | { 132 | public: 133 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 134 | UndistortEquidistant(const char* configFileName, bool noprefix); 135 | ~UndistortEquidistant(); 136 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; 137 | 138 | }; 139 | 140 | class UndistortPinhole : public Undistort 141 | { 142 | public: 143 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 144 | UndistortPinhole(const char* configFileName, bool noprefix); 145 | ~UndistortPinhole(); 146 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; 147 | 148 | private: 149 | float inputCalibration[8]; 150 | }; 151 | 152 | class UndistortKB : public Undistort 153 | { 154 | public: 155 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 156 | UndistortKB(const char* configFileName, bool noprefix); 157 | ~UndistortKB(); 158 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; 159 | 160 | }; 161 | 162 | } 163 | 164 | -------------------------------------------------------------------------------- /src/utils/globalCalib.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * This file is modified and part of the EDS 5 | * (https://rpg.ifi.uzh.ch/eds.html) 6 | * 7 | * Copyright 2016 Technical University of Munich and Intel. 8 | * Developed by Jakob Engel , 9 | * for more information see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above website. 12 | * 13 | * DSO is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSO is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSO. If not, see . 25 | */ 26 | 27 | 28 | 29 | #include "eds/utils/globalCalib.h" 30 | #include "stdio.h" 31 | #include 32 | 33 | namespace dso 34 | { 35 | int wG[PYR_LEVELS], hG[PYR_LEVELS]; 36 | float fxG[PYR_LEVELS], fyG[PYR_LEVELS], 37 | cxG[PYR_LEVELS], cyG[PYR_LEVELS]; 38 | 39 | float fxiG[PYR_LEVELS], fyiG[PYR_LEVELS], 40 | cxiG[PYR_LEVELS], cyiG[PYR_LEVELS]; 41 | 42 | Eigen::Matrix3f KG[PYR_LEVELS], KiG[PYR_LEVELS]; 43 | 44 | 45 | float wM3G; 46 | float hM3G; 47 | 48 | void setGlobalCalib(int w, int h,const Eigen::Matrix3f &K) 49 | { 50 | int wlvl=w; 51 | int hlvl=h; 52 | pyrLevelsUsed=1; 53 | while(wlvl%2==0 && hlvl%2==0 && wlvl*hlvl > 5000 && pyrLevelsUsed < PYR_LEVELS) 54 | { 55 | wlvl /=2; 56 | hlvl /=2; 57 | pyrLevelsUsed++; 58 | } 59 | printf("using pyramid levels 0 to %d. coarsest resolution: %d x %d!\n", 60 | pyrLevelsUsed-1, wlvl, hlvl); 61 | if(wlvl>100 && hlvl > 100) 62 | { 63 | printf("\n\n===============WARNING!===================\n " 64 | "using not enough pyramid levels.\n" 65 | "Consider scaling to a resolution that is a multiple of a power of 2.\n"); 66 | } 67 | if(pyrLevelsUsed < 3) 68 | { 69 | printf("\n\n===============WARNING!===================\n " 70 | "I need higher resolution.\n" 71 | "I will probably segfault.\n"); 72 | } 73 | 74 | wM3G = w-3; 75 | hM3G = h-3; 76 | 77 | wG[0] = w; 78 | hG[0] = h; 79 | KG[0] = K; 80 | fxG[0] = K(0,0); 81 | fyG[0] = K(1,1); 82 | cxG[0] = K(0,2); 83 | cyG[0] = K(1,2); 84 | KiG[0] = KG[0].inverse(); 85 | fxiG[0] = KiG[0](0,0); 86 | fyiG[0] = KiG[0](1,1); 87 | cxiG[0] = KiG[0](0,2); 88 | cyiG[0] = KiG[0](1,2); 89 | 90 | for (int level = 1; level < pyrLevelsUsed; ++ level) 91 | { 92 | wG[level] = w >> level; 93 | hG[level] = h >> level; 94 | 95 | fxG[level] = fxG[level-1] * 0.5; 96 | fyG[level] = fyG[level-1] * 0.5; 97 | cxG[level] = (cxG[0] + 0.5) / ((int)1<, 9 | * for more information see . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above website. 12 | * 13 | * DSO is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * DSO is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with DSO. If not, see . 25 | */ 26 | 27 | 28 | 29 | #pragma once 30 | #include "eds/utils/settings.h" 31 | #include "eds/utils/NumType.h" 32 | 33 | namespace dso 34 | { 35 | extern int wG[PYR_LEVELS], hG[PYR_LEVELS]; 36 | extern float fxG[PYR_LEVELS], fyG[PYR_LEVELS], 37 | cxG[PYR_LEVELS], cyG[PYR_LEVELS]; 38 | 39 | extern float fxiG[PYR_LEVELS], fyiG[PYR_LEVELS], 40 | cxiG[PYR_LEVELS], cyiG[PYR_LEVELS]; 41 | 42 | extern Eigen::Matrix3f KG[PYR_LEVELS],KiG[PYR_LEVELS]; 43 | 44 | extern float wM3G; 45 | extern float hM3G; 46 | 47 | void setGlobalCalib(int w, int h, const Eigen::Matrix3f &K ); 48 | } 49 | -------------------------------------------------------------------------------- /src/utils/settings.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | 26 | #pragma once 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | 33 | namespace dso 34 | { 35 | #define SOLVER_SVD (int)1 36 | #define SOLVER_ORTHOGONALIZE_SYSTEM (int)2 37 | #define SOLVER_ORTHOGONALIZE_POINTMARG (int)4 38 | #define SOLVER_ORTHOGONALIZE_FULL (int)8 39 | #define SOLVER_SVD_CUT7 (int)16 40 | #define SOLVER_REMOVE_POSEPRIOR (int)32 41 | #define SOLVER_USE_GN (int)64 42 | #define SOLVER_FIX_LAMBDA (int)128 43 | #define SOLVER_ORTHOGONALIZE_X (int)256 44 | #define SOLVER_MOMENTUM (int)512 45 | #define SOLVER_STEPMOMENTUM (int)1024 46 | #define SOLVER_ORTHOGONALIZE_X_LATER (int)2048 47 | 48 | 49 | // ============== PARAMETERS TO BE DECIDED ON COMPILE TIME ================= 50 | #define PYR_LEVELS 6 51 | extern int pyrLevelsUsed; 52 | 53 | 54 | 55 | extern float setting_keyframesPerSecond; 56 | extern bool setting_realTimeMaxKF; 57 | extern float setting_maxShiftWeightT; 58 | extern float setting_maxShiftWeightR; 59 | extern float setting_maxShiftWeightRT; 60 | extern float setting_maxAffineWeight; 61 | extern float setting_kfGlobalWeight; 62 | 63 | 64 | 65 | extern float setting_idepthFixPrior; 66 | extern float setting_idepthFixPriorMargFac; 67 | extern float setting_initialRotPrior; 68 | extern float setting_initialTransPrior; 69 | extern float setting_initialAffBPrior; 70 | extern float setting_initialAffAPrior; 71 | extern float setting_initialCalibHessian; 72 | 73 | extern int setting_solverMode; 74 | extern double setting_solverModeDelta; 75 | 76 | 77 | extern float setting_minIdepthH_act; 78 | extern float setting_minIdepthH_marg; 79 | 80 | 81 | 82 | extern float setting_maxIdepth; 83 | extern float setting_maxPixSearch; 84 | extern float setting_desiredImmatureDensity; // done 85 | extern float setting_desiredPointDensity; // done 86 | extern float setting_minPointsRemaining; 87 | extern float setting_maxLogAffFacInWindow; 88 | extern int setting_minFrames; 89 | extern int setting_maxFrames; 90 | extern int setting_minFrameAge; 91 | extern int setting_maxOptIterations; 92 | extern int setting_minOptIterations; 93 | extern float setting_thOptIterations; 94 | extern float setting_outlierTH; 95 | extern float setting_outlierTHSumComponent; 96 | 97 | 98 | 99 | extern int setting_pattern; 100 | extern float setting_margWeightFac; 101 | extern int setting_GNItsOnPointActivation; 102 | 103 | 104 | extern float setting_minTraceQuality; 105 | extern int setting_minTraceTestRadius; 106 | extern float setting_reTrackThreshold; 107 | 108 | 109 | extern int setting_minGoodActiveResForMarg; 110 | extern int setting_minGoodResForMarg; 111 | extern int setting_minInlierVotesForMarg; 112 | 113 | 114 | 115 | 116 | extern int setting_photometricCalibration; 117 | extern bool setting_useExposure; 118 | extern float setting_affineOptModeA; 119 | extern float setting_affineOptModeB; 120 | extern int setting_gammaWeightsPixelSelect; 121 | 122 | 123 | 124 | extern bool setting_forceAceptStep; 125 | 126 | 127 | 128 | extern float setting_huberTH; 129 | 130 | 131 | extern bool setting_logStuff; 132 | extern float benchmarkSetting_fxfyfac; 133 | extern int benchmarkSetting_width; 134 | extern int benchmarkSetting_height; 135 | extern float benchmark_varNoise; 136 | extern float benchmark_varBlurNoise; 137 | extern int benchmark_noiseGridsize; 138 | extern float benchmark_initializerSlackFactor; 139 | 140 | extern float setting_frameEnergyTHConstWeight; 141 | extern float setting_frameEnergyTHN; 142 | 143 | extern float setting_frameEnergyTHFacMedian; 144 | extern float setting_overallEnergyTHWeight; 145 | extern float setting_coarseCutoffTH; 146 | 147 | extern float setting_minGradHistCut; 148 | extern float setting_minGradHistAdd; 149 | extern float setting_gradDownweightPerLevel; 150 | extern bool setting_selectDirectionDistribution; 151 | 152 | 153 | 154 | extern float setting_trace_stepsize; 155 | extern int setting_trace_GNIterations; 156 | extern float setting_trace_GNThreshold; 157 | extern float setting_trace_extraSlackOnTH; 158 | extern float setting_trace_slackInterval; 159 | extern float setting_trace_minImprovementFactor; 160 | 161 | 162 | extern bool setting_render_displayCoarseTrackingFull; 163 | extern bool setting_render_renderWindowFrames; 164 | extern bool setting_render_plotTrackingFull; 165 | extern bool setting_render_display3D; 166 | extern bool setting_render_displayResidual; 167 | extern bool setting_render_displayVideo; 168 | extern bool setting_render_displayDepth; 169 | 170 | extern bool setting_fullResetRequested; 171 | 172 | extern bool setting_debugout_runquiet; 173 | 174 | extern bool disableAllDisplay; 175 | extern bool disableReconfigure; 176 | 177 | 178 | extern bool setting_onlyLogKFPoses; 179 | 180 | 181 | 182 | 183 | extern bool debugSaveImages; 184 | 185 | 186 | extern int sparsityFactor; 187 | extern bool goStepByStep; 188 | extern bool plotStereoImages; 189 | extern bool multiThreading; 190 | 191 | extern float freeDebugParam1; 192 | extern float freeDebugParam2; 193 | extern float freeDebugParam3; 194 | extern float freeDebugParam4; 195 | extern float freeDebugParam5; 196 | 197 | 198 | void handleKey(char k); 199 | 200 | 201 | 202 | 203 | extern int staticPattern[10][40][2]; 204 | extern int staticPatternNum[10]; 205 | extern int staticPatternPadding[10]; 206 | 207 | 208 | 209 | 210 | //#define patternNum staticPatternNum[setting_pattern] 211 | //#define patternP staticPattern[setting_pattern] 212 | //#define patternPadding staticPatternPadding[setting_pattern] 213 | 214 | // 215 | #define patternNum 8 216 | #define patternP staticPattern[8] 217 | #define patternPadding 2 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | } 232 | --------------------------------------------------------------------------------