├── .gitignore ├── README.md ├── Software_License_Agreement_TUB_dvs_mcemvs.pdf ├── cartesian3dgrid ├── CMakeLists.txt ├── include │ └── cartesian3dgrid │ │ ├── cartesian3dgrid.h │ │ └── gaussianiir3d.h ├── package.xml └── src │ ├── cartesian3dgrid.cpp │ ├── cartesian3dgrid_IO.cpp │ ├── cartesian3dgrid_filter.cpp │ └── gaussianiir3d.cpp ├── data └── DSEC │ ├── interlaken_00-odometry │ └── pose.bag │ ├── zurich_city_02-odometry │ └── pose.bag │ └── zurich_city_04-odometry │ └── pose.bag ├── dependencies.yaml ├── docs ├── 2022_MCEVMS_poster.pdf ├── block_all.png ├── datasets.md ├── evaluation.md ├── examples.md ├── installation.md ├── mcemvs_thumbnail.jpg └── running.md └── mapper_emvs_stereo ├── CMakeLists.txt ├── cfg ├── DSEC │ ├── interlaken_00_b_2 │ │ └── dsec.conf │ └── zurich_04_a_full │ │ └── dsec.conf ├── evimo2 │ └── evimo2.conf ├── hkust │ └── hkust_lab.conf ├── rpg_eccv18 │ ├── boxes_edited │ │ ├── AtHc │ │ │ └── rpg_boxes_edited.conf │ │ ├── AtHc_shuffled │ │ │ └── rpg_boxes_edited.conf │ │ ├── alg1 │ │ │ └── rpg_boxes_edited.conf │ │ └── fusionfuncs │ │ │ ├── alg1_AM │ │ │ └── rpg_boxes_edited.conf │ │ │ ├── alg1_GM │ │ │ └── rpg_boxes_edited.conf │ │ │ ├── alg1_HM │ │ │ └── rpg_boxes_edited.conf │ │ │ ├── alg1_RMS │ │ │ └── rpg_boxes_edited.conf │ │ │ ├── alg1_max │ │ │ └── rpg_boxes_edited.conf │ │ │ └── alg1_min │ │ │ └── rpg_boxes_edited.conf │ ├── monitor_edited │ │ ├── AtHc │ │ │ └── rpg_monitor_edited_fixedts.conf │ │ ├── AtHc_shuffled │ │ │ └── rpg_monitor_edited_fixedts.conf │ │ ├── alg1 │ │ │ └── rpg_monitor_edited_fixedts.conf │ │ └── fusionfuncs │ │ │ ├── alg1_AM │ │ │ └── rpg_monitor_edited_fixedts.conf │ │ │ ├── alg1_GM │ │ │ └── rpg_monitor_edited_fixedts.conf │ │ │ ├── alg1_HM │ │ │ └── rpg_monitor_edited_fixedts.conf │ │ │ ├── alg1_RMS │ │ │ └── rpg_monitor_edited_fixedts.conf │ │ │ ├── alg1_max │ │ │ └── rpg_monitor_edited_fixedts.conf │ │ │ └── alg1_min │ │ │ └── rpg_monitor_edited_fixedts.conf │ └── reader │ │ └── alg1 │ │ └── rpg_reader_fixedts.conf ├── tumvie │ ├── bike-easy-3_full │ │ └── tum-vie.conf │ ├── bike-easy-4_full │ │ └── tum-vie.conf │ ├── bike-easy-5_full │ │ └── tum-vie.conf │ ├── desk2_full │ │ └── tum-vie.conf │ ├── mocap-1d-trans_full │ │ └── tum-vie.conf │ ├── mocap-3d-trans_full │ │ └── tum-vie.conf │ ├── running-easy-0_full │ │ └── tum-vie.conf │ ├── running-easy-1_full │ │ └── tum-vie.conf │ └── skate-easy-1_full │ │ └── tum-vie.conf └── upenn_mvsec │ ├── mvsec_flying1_full │ ├── AcAt │ │ └── flying1.conf │ ├── AcHt │ │ └── flying1.conf │ ├── AtHc │ │ └── flying1.conf │ ├── AtHc_shuffled │ │ └── flying1.conf │ ├── HcHt │ │ └── flying1.conf │ └── alg1 │ │ └── flying1.conf │ ├── mvsec_flying2_full │ ├── AcAt │ │ └── flying2.conf │ ├── AcHt │ │ └── flying2.conf │ ├── AtHc │ │ └── flying2.conf │ ├── AtHc_shuffled │ │ └── flying2.conf │ ├── HcHt │ │ └── flying2.conf │ └── norm_max_c14 │ │ └── flying2.conf │ └── mvsec_flying3_full │ ├── AcAt │ └── flying3.conf │ ├── AcHt │ └── flying3.conf │ ├── AtHc │ └── flying3.conf │ ├── AtHc_shuffled │ └── flying3.conf │ ├── HcHt │ └── flying3.conf │ └── norm_max_c14 │ └── flying3.conf ├── include └── mapper_emvs_stereo │ ├── calib.hpp │ ├── data_loading.hpp │ ├── depth_vector.hpp │ ├── geometry_utils.hpp │ ├── mapper_emvs_stereo.hpp │ ├── median_filtering.hpp │ ├── process1.hpp │ ├── process2.hpp │ ├── process5.hpp │ ├── trajectory.hpp │ └── utils.hpp ├── launch └── rpg_calib_info.launch ├── package.xml ├── scripts ├── depth_metrics.py ├── evaluate_mcemvs_dsec.py ├── mocap_txt2bag.py ├── precision_completeness.py ├── visualize_dsi_slices.py ├── visualize_dsi_volume.py └── visualize_pointcloud.py └── src ├── calib.cpp ├── data_loading.cpp ├── main.cpp ├── mapper_emvs_stereo.cpp ├── median_filtering.cpp ├── process1.cpp ├── process2.cpp ├── process5.cpp └── utils.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | *.user 2 | mapper_emvs_stereo/experiments/ 3 | mapper_emvs_stereo/*.png 4 | mapper_emvs_stereo/*.npy 5 | mapper_emvs_stereo/*.pcd 6 | .idea 7 | 8 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MC-EMVS: Multi-Camera Event-based Multi-View Stereo 2 | This is the code for the journal paper [**Multi-Event-Camera Depth Estimation and Outlier Rejection by Refocused Events Fusion**](https://doi.org/10.1002/aisy.202200221), also known as **MC-EMVS: Multi-Camera Event-based Multi-View Stereo**, 3 | by [Suman Ghosh](https://www.linkedin.com/in/suman-ghosh-a8762576/) and [Guillermo Gallego](https://sites.google.com/view/guillermogallego), published at Advanced Intelligent Systems. 4 |

5 | 6 | [Paper](https://arxiv.org/pdf/2207.10494) | [Video](https://youtu.be/o7Bxg9XlHmg) | [Poster](/docs/2022_MCEVMS_poster.pdf) 7 |

8 | 9 | [![Multi-Event-Camera Depth Estimation and Outlier Rejection by Refocused Events Fusion](docs/mcemvs_thumbnail.jpg)](https://youtu.be/o7Bxg9XlHmg) 10 | 11 | If you use this work in your research, please cite it as follows: 12 | 13 | ```bibtex 14 | @article{Ghosh22aisy, 15 | author = {Ghosh, Suman and Gallego, Guillermo}, 16 | title = {Multi-Event-Camera Depth Estimation and Outlier Rejection by Refocused Events Fusion}, 17 | journal = {Advanced Intelligent Systems}, 18 | year = {2022}, 19 | doi = {10.1002/aisy.202200221} 20 | } 21 | ``` 22 | 23 | ## Data Processing Pipeline 24 | 25 | 26 | ![pipeline](docs/block_all.png) 27 | 28 | ### Input 29 | * Events from multiple cameras 30 | * Pose of camera rig 31 | * Camera calibration (intrinsic, extrinsic, hand-eye) parameters 32 | 33 | ### Output 34 | * Depth map 35 | * Confidence map 36 | * Point cloud 37 | * Intermediate ray density maps / Disparity Space Images (DSI) 38 | 39 | ## Code 40 | * [Installation instructions](docs/installation.md) 41 | * [Running the code with various configurations](docs/running.md) 42 | * [Datasets used](docs/datasets.md) 43 | * [Running Examples](docs/examples.md) 44 | * [Evaluation scripts](docs/evaluation.md) 45 | 46 | 47 | ## License 48 | 49 | The license is available [here](Software_License_Agreement_TUB_dvs_mcemvs.pdf). 50 | 51 | Follow-up works 52 | ------- 53 | * **[Event-based Stereo Depth Estimation: A Survey](https://arxiv.org/pdf/2409.17680)** 54 | * **[ES-PTAM: Event-based Stereo Parallel Tracking and Mapping](https://github.com/tub-rip/ES-PTAM)** 55 | 56 | Additional Resources on Event-based Vision 57 | ------- 58 | * [Research page (TU Berlin RIP lab)](https://sites.google.com/view/guillermogallego/research/event-based-vision) 59 | * [Course at TU Berlin](https://sites.google.com/view/guillermogallego/teaching/event-based-robot-vision) 60 | * [Event-based Vision: A Survey](http://rpg.ifi.uzh.ch/docs/EventVisionSurvey.pdf) 61 | * [List of Resources](https://github.com/uzh-rpg/event-based_vision_resources) 62 | -------------------------------------------------------------------------------- /Software_License_Agreement_TUB_dvs_mcemvs.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tub-rip/dvs_mcemvs/9ae7a374c72f2653c42cd36d663698eee8cfe509/Software_License_Agreement_TUB_dvs_mcemvs.pdf -------------------------------------------------------------------------------- /cartesian3dgrid/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(cartesian3dgrid) 2 | set(CMAKE_CXX_STANDARD 14) 3 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 4 | cmake_minimum_required(VERSION 2.8.3) 5 | 6 | find_package(catkin_simple REQUIRED) 7 | catkin_simple(ALL_DEPS_REQUIRED) 8 | 9 | set(CMAKE_BUILD_TYPE RelWithDebInfo) # Release, RelWithDebInfo 10 | set(CMAKE_CXX_FLAGS "-O3 ${CMAKE_CXX_FLAGS} -fopenmp") 11 | 12 | file(GLOB SOURCE_FILES src/cartesian3dgrid.cpp 13 | src/cartesian3dgrid_IO.cpp) 14 | 15 | cs_add_library(${PROJECT_NAME} ${SOURCE_FILES} ${HEADER_FILES}) 16 | target_link_libraries(${PROJECT_NAME}) 17 | 18 | #target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) 19 | 20 | cs_export() 21 | -------------------------------------------------------------------------------- /cartesian3dgrid/include/cartesian3dgrid/cartesian3dgrid.h: -------------------------------------------------------------------------------- 1 | /* 2 | * \file cartesian3dgrid.h 3 | * \brief header file for handling 3d volume 4 | * \author (1) Suman Ghosh 5 | * \date 2022-09-01 6 | * \author (2) Guillermo Gallego 7 | * \date 2022-09-01 8 | * Copyright/Rights of Use: 9 | * 2022, Technische Universität Berlin 10 | * Prof. Guillermo Gallego 11 | * Robotic Interactive Perception 12 | * Marchstrasse 23, Sekr. MAR 5-5 13 | * 10587 Berlin, Germany 14 | */ 15 | 16 | #pragma once 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | class Grid3D 23 | { 24 | public: 25 | Grid3D(); 26 | Grid3D(const unsigned int dimX, const unsigned int dimY, const unsigned int dimZ); 27 | ~Grid3D(); 28 | 29 | void allocate(const unsigned int dimX, const unsigned int dimY, const unsigned int dimZ); 30 | void deallocate(); 31 | 32 | void printInfo() const; 33 | 34 | // The 3D data is stored in a 1D array that is ordered such that 35 | // volume[x + dimX*(y + dimY*z)] = data value at (x,y,z). 36 | 37 | // Accessing elements of the grid: 38 | 39 | // At integer location 40 | inline float getGridValueAt(const unsigned int ix, const unsigned int iy, const unsigned int iz) const 41 | { 42 | return data_array_.at(ix + size_[0]*(iy + size_[1]*iz)); 43 | } 44 | 45 | inline float getGridValueAt(const unsigned int p) const 46 | { 47 | return data_array_.at(p); 48 | } 49 | 50 | inline void accumulateGridValueAt(const unsigned int p, const float fval) 51 | { 52 | data_array_.at(p) += fval; 53 | } 54 | 55 | inline void setGridValueAt(const unsigned int p, const float fval) 56 | { 57 | data_array_.at(p) = fval; 58 | } 59 | 60 | // At floating point location within a Z slice. Bilinear interpolation or voting. 61 | inline void accumulateGridValueAt(const float x_f, const float y_f, float* grid); 62 | 63 | // FIXME: The following voxel-wise operations do not use parallelization yet 64 | inline void addTwoGrids(const Grid3D grid2) 65 | { 66 | for (int p = 0; p < numCells_; p++) 67 | { 68 | data_array_.at(p) += grid2.data_array_.at(p); 69 | } 70 | } 71 | 72 | inline void addInverseOfTwoGrids(const Grid3D grid2, const float eps=1e-2) 73 | { 74 | for (int p = 0; p < numCells_; p++) 75 | { 76 | data_array_.at(p) = data_array_.at(p) + 1.0f/(eps + grid2.data_array_.at(p)); 77 | } 78 | } 79 | 80 | inline void computeHMfromSumOfInv(int n) 81 | { 82 | for (int p = 0; p < numCells_; p++) 83 | { 84 | data_array_.at(p) = (float)n /(data_array_.at(p)); 85 | } 86 | } 87 | inline void computeAMfromSum(int n) 88 | { 89 | for (int p = 0; p < numCells_; p++) 90 | { 91 | data_array_.at(p) = data_array_.at(p)/(float)n; 92 | } 93 | } 94 | 95 | inline void subtractTwoGrids(const Grid3D grid2) 96 | { 97 | for (int p = 0; p < numCells_; p++) 98 | { 99 | data_array_.at(p) -= grid2.data_array_.at(p); 100 | } 101 | } 102 | 103 | inline void ratioTwoGrids(const Grid3D grid2, const float eps=1e-1) 104 | { 105 | for (int p = 0; p < numCells_; p++) 106 | { 107 | data_array_.at(p) /= fabs(grid2.data_array_.at(p)) + eps; 108 | } 109 | } 110 | 111 | inline void minTwoGrids(const Grid3D grid2) 112 | { 113 | for (int p = 0; p < numCells_; p++) 114 | { 115 | data_array_.at(p) = std::min(data_array_.at(p), grid2.data_array_.at(p)); 116 | } 117 | } 118 | 119 | inline void harmonicMeanTwoGrids(const Grid3D grid2, const float eps=1e-1) 120 | { 121 | for (int p = 0; p < numCells_; p++) 122 | { 123 | float prod = data_array_.at(p) * grid2.data_array_.at(p); 124 | float sum = data_array_.at(p) + grid2.data_array_.at(p); 125 | data_array_.at(p) = 2*prod / (sum + eps); 126 | } 127 | } 128 | 129 | // allows for recursive application by stating number of maps to fuse 130 | inline void harmonicMeanTwoGrids(const Grid3D grid2, int n, const float eps=1e-1) 131 | { 132 | for (int p = 0; p < numCells_; p++) 133 | { 134 | float a = data_array_.at(p)/(float)(n-1); 135 | float prod = a * grid2.data_array_.at(p); 136 | float sum = a + grid2.data_array_.at(p); 137 | data_array_.at(p) = n*prod / (sum + eps); 138 | } 139 | } 140 | 141 | inline void rmsTwoGrids(const Grid3D grid2) 142 | { 143 | for (int p = 0; p < numCells_; p++) 144 | { 145 | float ms = 0.5*(pow(data_array_.at(p), 2) + pow(grid2.data_array_.at(p), 2)); 146 | data_array_.at(p) = sqrt(ms); 147 | } 148 | } 149 | 150 | inline void geometricMeanTwoGrids(const Grid3D grid2) 151 | { 152 | for (int p = 0; p < numCells_; p++) 153 | { 154 | data_array_.at(p) = std::sqrt(data_array_.at(p) * grid2.data_array_.at(p)); 155 | } 156 | } 157 | 158 | inline void arithmeticMeanTwoGrids(const Grid3D grid2) 159 | { 160 | for (int p = 0; p < numCells_; p++) 161 | { 162 | data_array_.at(p) = 0.5 * (data_array_.at(p) + grid2.data_array_.at(p)); 163 | } 164 | } 165 | 166 | inline void quadraticMeanTwoGrids(const Grid3D grid2) 167 | { 168 | for (int p = 0; p < numCells_; p++) 169 | { 170 | float u = data_array_.at(p); 171 | float v = grid2.data_array_.at(p); 172 | data_array_.at(p) = std::sqrt( 0.5 * (u*u + v*v) ); 173 | } 174 | } 175 | 176 | inline void cubicMeanTwoGrids(const Grid3D grid2) 177 | { 178 | for (int p = 0; p < numCells_; p++) 179 | { 180 | float u = data_array_.at(p); 181 | float v = grid2.data_array_.at(p); 182 | data_array_.at(p) = std::cbrt( 0.5 * (u*u*u + v*v*v) ); 183 | } 184 | } 185 | 186 | inline void maxTwoGrids(const Grid3D grid2) 187 | { 188 | for (int p = 0; p < numCells_; p++) 189 | { 190 | data_array_.at(p) = std::max(data_array_.at(p), grid2.data_array_.at(p)); 191 | } 192 | } 193 | 194 | 195 | inline void accumulateZSliceAt(const unsigned int iz, const cv::Mat& img) 196 | { 197 | for(unsigned int iy=0; iy < img.rows; iy++) 198 | { 199 | for(unsigned int ix=0; ix < img.cols; ix++) 200 | { 201 | data_array_.at(ix + size_[0]*(iy + size_[1]*iz)) += img.at(iy,ix); 202 | } 203 | } 204 | } 205 | 206 | cv::Mat getSlice(const unsigned int sliceIdx, const unsigned int dimIdx) const; 207 | void collapseMaxZSlice(cv::Mat* max_val, cv::Mat* max_pos) const; 208 | void collapseMinZSlice(cv::Mat* min_val, cv::Mat* min_pos) const; 209 | 210 | void collapseZSliceByGradMag(cv::Mat* confidence, cv::Mat* depth_cell_indices, int half_patchsize=1); 211 | void collapseZSliceByLaplacianMag(cv::Mat* confidence, cv::Mat* depth_cell_indices); 212 | void collapseZSliceByDoG(cv::Mat* confidence, cv::Mat* depth_cell_indices); 213 | void collapseZSliceByLocalVar(cv::Mat* confidence, cv::Mat* depth_cell_indices); 214 | void collapseZSliceByLocalMeanSquare(cv::Mat* confidence, cv::Mat* depth_cell_indices); 215 | 216 | void computeLocalFocusInPlace(int focus_method); 217 | void computeLocalMeanSquareInPlace(); 218 | void computeLocalVarInPlace(); 219 | 220 | // Statistics 221 | double computeMeanSquare() const; 222 | void getMinMax(float* min_val, float* max_val, unsigned long* min_pos=NULL, unsigned long* max_pos=NULL) const; 223 | 224 | void resetGrid(); 225 | 226 | // Output 227 | int writeGridNpy(const char szFilename[]) const; 228 | void imwriteSlices(const char prefix[], const unsigned int dimIdx, bool normalize_by_minmax=true) const; 229 | 230 | void getDimensions(int* dimX, int* dimY, int* dimZ) const 231 | { 232 | *dimX = size_[0]; 233 | *dimY = size_[1]; 234 | *dimZ = size_[2]; 235 | } 236 | 237 | float* getPointerToSlice(int layer) 238 | { 239 | return &data_array_.data()[layer * size_[0] * size_[1]]; 240 | } 241 | 242 | private: 243 | std::vector data_array_; 244 | unsigned int numCells_; 245 | unsigned int size_[3]; 246 | 247 | }; 248 | 249 | 250 | // Function implementation 251 | 252 | // Bilinear voting within a Z-slice, if point (x,y) is given as float 253 | inline void Grid3D::accumulateGridValueAt(const float x_f, const float y_f, float* grid) 254 | { 255 | if (x_f >= 0.f && y_f >= 0.f) 256 | { 257 | const int x = x_f, y = y_f; 258 | if (x+1 < size_[0] && 259 | y+1 < size_[1]) 260 | { 261 | float* g = grid + x + y * size_[0]; 262 | const float fx = x_f - x, 263 | fy = y_f - y, 264 | fx1 = 1.f - fx, 265 | fy1 = 1.f - fy; 266 | 267 | g[0] += fx1*fy1; 268 | g[1] += fx*fy1; 269 | g[size_[0]] += fx1*fy; 270 | g[size_[0]+1] += fx*fy; 271 | } 272 | } 273 | } 274 | -------------------------------------------------------------------------------- /cartesian3dgrid/include/cartesian3dgrid/gaussianiir3d.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "cartesian3dgrid.h" 3 | 4 | /** 5 | * \file gaussianiir3d.h 6 | * \brief Fast 3D Gaussian convolution IIR approximation 7 | * \author Pascal Getreuer 8 | * 9 | * Copyright (c) 2011, Pascal Getreuer 10 | * All rights reserved. 11 | * 12 | * This program is free software: you can redistribute it and/or modify it 13 | * under, at your option, the terms of the GNU General Public License as 14 | * published by the Free Software Foundation, either version 3 of the 15 | * License, or (at your option) any later version, or the terms of the 16 | * simplified BSD license. 17 | * 18 | * You should have received a copy of these licenses along with this program. 19 | * If not, see and 20 | * . 21 | */ 22 | 23 | void gaussianiir3d(float *volume, long width, long height, long depth, float sigma, int numsteps); 24 | 25 | void gaussianiir3d(float *volume, long width, long height, long depth, float sigma_x, float sigma_y, float sigma_z, int numsteps); 26 | 27 | void smoothInPlace(const float sigma=1.f); 28 | void smoothInPlaceIIR3D(const float sigma_x=1.f, const float sigma_y=1.f, const float sigma_z=1.f); 29 | void laplacianInPlace(); 30 | -------------------------------------------------------------------------------- /cartesian3dgrid/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | cartesian3dgrid 4 | 0.0.0 5 | Cartesian 3D grid 6 | Guillermo Gallego 7 | 8 | see LICENSE 9 | 10 | catkin 11 | catkin_simple 12 | 13 | cv_bridge 14 | cnpy_catkin 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /cartesian3dgrid/src/cartesian3dgrid.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * \file cartesian3dgrid.cpp 3 | * \brief utility functions for handling 3D volumes 4 | * \author (1) Suman Ghosh 5 | * \date 2022-09-01 6 | * \author (2) Guillermo Gallego 7 | * \date 2022-09-01 8 | * Copyright/Rights of Use: 9 | * 2022, Technische Universität Berlin 10 | * Prof. Guillermo Gallego 11 | * Robotic Interactive Perception 12 | * Marchstrasse 23, Sekr. MAR 5-5 13 | * 10587 Berlin, Germany 14 | */ 15 | 16 | #include "../include/cartesian3dgrid/cartesian3dgrid.h" 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | 24 | Grid3D::Grid3D() 25 | { 26 | deallocate(); 27 | } 28 | 29 | 30 | Grid3D::Grid3D(const unsigned int dimX, const unsigned int dimY, const unsigned int dimZ) 31 | { 32 | deallocate(); 33 | allocate(dimX,dimY,dimZ); 34 | } 35 | 36 | 37 | void Grid3D::allocate(const unsigned int dimX, const unsigned int dimY, const unsigned int dimZ) 38 | { 39 | size_[0] = dimX; 40 | size_[1] = dimY; 41 | size_[2] = dimZ; 42 | numCells_ = size_[0]*size_[1]*size_[2]; 43 | data_array_.resize(numCells_); 44 | resetGrid(); 45 | } 46 | 47 | 48 | Grid3D::~Grid3D() 49 | { 50 | deallocate(); 51 | } 52 | 53 | 54 | void Grid3D::deallocate() 55 | { 56 | size_[0] = size_[1] = size_[2] = 0; 57 | data_array_.clear(); 58 | } 59 | 60 | 61 | void Grid3D::printInfo() const 62 | { 63 | std::cout << "Grid3D Dimensions: " << "(" << size_[0] << "," << size_[1] << "," << size_[2] << ")" << std::endl; 64 | std::cout << "Grid3D Data_array size: " << data_array_.size() << std::endl; 65 | } 66 | 67 | void Grid3D::resetGrid() 68 | { 69 | std::fill(data_array_.begin(), data_array_.end(), 0.f); 70 | } 71 | 72 | cv::Mat Grid3D::getSlice(const unsigned int sliceIdx, const unsigned int dimIdx) const 73 | { 74 | cv::Mat slice; 75 | 76 | // simplest way 77 | if (dimIdx==0) 78 | { 79 | // X-slice 80 | unsigned int u_size = size_[2], // Z acts as X-axis 81 | v_size = size_[1]; 82 | slice = cv::Mat(v_size,u_size,CV_32FC1); 83 | for(unsigned int u=0; u(v,u) = getGridValueAt(sliceIdx,v,u); 86 | } 87 | else if (dimIdx==1) 88 | { 89 | // Y-slice 90 | unsigned int u_size = size_[2], // Z acts as X-axis 91 | v_size = size_[0]; 92 | slice = cv::Mat(v_size,u_size,CV_32FC1); 93 | for(unsigned int u=0; u(v,u) = getGridValueAt(v,sliceIdx,u); 96 | } 97 | else if (dimIdx==2) 98 | { 99 | // Z-slice 100 | unsigned int u_size = size_[0], 101 | v_size = size_[1]; 102 | slice = cv::Mat(v_size,u_size,CV_32FC1); // (y,x) as in images 103 | for(unsigned int v=0; v(v,u) = getGridValueAt(u,v,sliceIdx); 106 | } 107 | else 108 | { 109 | std::cout << "ERROR. dimIdx should be 0, 1 or 2" << std::endl; 110 | } 111 | 112 | return slice; 113 | } 114 | 115 | void Grid3D::collapseMaxZSlice(cv::Mat* max_val, cv::Mat* max_pos_idx) const 116 | { 117 | // Z-slice 118 | unsigned int u_size = size_[0], v_size = size_[1], w_size = size_[2]; 119 | *max_val = cv::Mat(v_size,u_size, CV_32FC1); // (y,x) as in images 120 | *max_pos_idx = cv::Mat(v_size,u_size, CV_8U); // WARNING: Max 256 depth layers 121 | 122 | std::vector grid_vals_vec(w_size); 123 | for(unsigned int v=0; v(v,u) = *max; 134 | (*max_pos_idx).at(v,u) = std::distance(std::begin(grid_vals_vec), max); 135 | } 136 | } 137 | } 138 | 139 | void Grid3D::collapseMinZSlice(cv::Mat* max_val, cv::Mat* max_pos_idx) const 140 | { 141 | // Z-slice 142 | unsigned int u_size = size_[0], v_size = size_[1], w_size = size_[2]; 143 | *max_val = cv::Mat(v_size,u_size, CV_32FC1); // (y,x) as in images 144 | *max_pos_idx = cv::Mat(v_size,u_size, CV_8U); // WARNING: Max 256 depth layers 145 | 146 | std::vector grid_vals_vec(w_size); 147 | for(unsigned int v=0; v(v,u) = *max; 158 | (*max_pos_idx).at(v,u) = std::distance(std::begin(grid_vals_vec), max); 159 | } 160 | } 161 | } 162 | 163 | 164 | double Grid3D::computeMeanSquare() const 165 | { 166 | double result = 0.; 167 | for (int i=0; i(y,x); // Causes double edges 224 | 225 | cv::Mat patch = grad_mag(cv::Rect(x-half_patchsize, y-half_patchsize, patch_size, patch_size)); //.mul(gaussian_kernel_2d); 226 | float focus = cv::mean(patch)[0]; 227 | 228 | // Select maximum focus value along the optical ray 229 | if(focus > confidence->at(y,x)) 230 | { 231 | confidence->at(y,x) = focus; // max focus along optical ray 232 | depth_cell_indices->at(y,x) = k; 233 | } 234 | } 235 | } 236 | } 237 | 238 | // Return gradient magnitude instead of its square 239 | cv::sqrt(*confidence, *confidence); 240 | } 241 | 242 | 243 | void Grid3D::collapseZSliceByLaplacianMag(cv::Mat* confidence, cv::Mat* depth_cell_indices) 244 | { 245 | // Z-slice 246 | unsigned int u_size = size_[0], v_size = size_[1], w_size = size_[2]; 247 | *confidence = cv::Mat::zeros(v_size,u_size, CV_32FC1); // (y,x) as in images 248 | *depth_cell_indices = cv::Mat::zeros(v_size,u_size, CV_8U); // WARNING: Max 256 depth layers 249 | 250 | for(unsigned int k=0; k(y,x); 268 | 269 | // Select maximum focus value along the optical ray 270 | if(focus > confidence->at(y,x)) 271 | { 272 | confidence->at(y,x) = focus; // max focus along optical ray 273 | depth_cell_indices->at(y,x) = k; 274 | } 275 | } 276 | } 277 | } 278 | 279 | // Return gradient magnitude instead of its square 280 | cv::sqrt(*confidence, *confidence); 281 | } 282 | 283 | 284 | void Grid3D::collapseZSliceByDoG(cv::Mat* confidence, cv::Mat* depth_cell_indices) 285 | { 286 | // Z-slice 287 | unsigned int u_size = size_[0], v_size = size_[1], w_size = size_[2]; 288 | *confidence = cv::Mat::zeros(v_size,u_size, CV_32FC1); // (y,x) as in images 289 | *depth_cell_indices = cv::Mat::zeros(v_size,u_size, CV_8U); // WARNING: Max 256 depth layers 290 | 291 | // Compute DoG filtered image 292 | const double sigma = 0.5; // 1.0 293 | 294 | //const double sigma2 = sigma * 3.0; // DoG 295 | const double sigma2 = sigma * 1.6; // LoG 296 | 297 | for(unsigned int k=0; k(y,x)); 317 | 318 | // Select maximum focus value along the optical ray 319 | if(focus > confidence->at(y,x)) 320 | { 321 | confidence->at(y,x) = focus; // max focus along optical ray 322 | depth_cell_indices->at(y,x) = k; 323 | } 324 | } 325 | } 326 | } 327 | } 328 | 329 | 330 | void Grid3D::collapseZSliceByLocalVar(cv::Mat* confidence, cv::Mat* depth_cell_indices) 331 | { 332 | // Z-slice 333 | unsigned int u_size = size_[0], v_size = size_[1], w_size = size_[2]; 334 | *confidence = cv::Mat::zeros(v_size,u_size, CV_32FC1); // (y,x) as in images 335 | *depth_cell_indices = cv::Mat::zeros(v_size,u_size, CV_8U); // WARNING: Max 256 depth layers 336 | 337 | const double sigma = 0.5; 338 | 339 | for(unsigned int k=0; k(y,x)); 359 | 360 | // Select maximum focus value along the optical ray 361 | if(focus > confidence->at(y,x)) 362 | { 363 | confidence->at(y,x) = focus; // max focus along optical ray 364 | depth_cell_indices->at(y,x) = k; 365 | } 366 | } 367 | } 368 | } 369 | 370 | // Return std instead of variance 371 | //cv::sqrt(*confidence, *confidence); 372 | } 373 | 374 | 375 | void Grid3D::collapseZSliceByLocalMeanSquare(cv::Mat* confidence, cv::Mat* depth_cell_indices) 376 | { 377 | // Z-slice 378 | unsigned int u_size = size_[0], v_size = size_[1], w_size = size_[2]; 379 | *confidence = cv::Mat::zeros(v_size,u_size, CV_32FC1); // (y,x) as in images 380 | *depth_cell_indices = cv::Mat::zeros(v_size,u_size, CV_8U); // WARNING: Max 256 depth layers 381 | 382 | const double sigma = 0.5; 383 | 384 | for(unsigned int k=0; k(y,x); 401 | 402 | // Select maximum focus value along the optical ray 403 | if(focus > confidence->at(y,x)) 404 | { 405 | confidence->at(y,x) = focus; // max focus along optical ray 406 | depth_cell_indices->at(y,x) = k; 407 | } 408 | } 409 | } 410 | } 411 | 412 | // Return sqrt of MS 413 | //cv::sqrt(*confidence, *confidence); 414 | } 415 | 416 | 417 | void Grid3D::computeLocalFocusInPlace(int focus_method) 418 | { 419 | switch (focus_method) 420 | { 421 | case 1: 422 | computeLocalMeanSquareInPlace(); 423 | break; 424 | default: 425 | computeLocalVarInPlace(); 426 | break; 427 | } 428 | } 429 | 430 | 431 | void Grid3D::computeLocalVarInPlace() 432 | { 433 | // Z-slice 434 | unsigned int u_size = size_[0], v_size = size_[1], w_size = size_[2]; 435 | 436 | const double sigma = 0.5; 437 | 438 | #pragma omp parallel for 439 | for(unsigned int k=0; k 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | 28 | //--------------------- Output for Numpy ----------------------------------------- 29 | 30 | int Grid3D::writeGridNpy(const char filename[]) const 31 | { 32 | cnpy::npy_save(std::string(filename), 33 | &data_array_[0], 34 | {size_[2], size_[1], size_[0]}, "w"); 35 | return 0; 36 | } 37 | 38 | 39 | void Grid3D::imwriteSlices(const char prefix[], const unsigned int dimIdx, bool normalize_by_minmax) const 40 | { 41 | // Option: write images normalizing by the maximum value over grid 42 | float min, max, range; 43 | if (normalize_by_minmax) 44 | { 45 | auto it = std::minmax_element(std::begin(data_array_), std::end(data_array_)); 46 | min = *it.first; 47 | max = *it.second; 48 | range = max - min; 49 | //std::cout << "Min value " << min << std::endl; 50 | //std::cout << "Max value " << max << std::endl; 51 | } 52 | 53 | #pragma omp parallel for 54 | for (int idx_slice = 0; idx_slice < size_[dimIdx]; idx_slice++) 55 | { 56 | // Get slice 57 | cv::Mat slice_img = getSlice(idx_slice, dimIdx); 58 | 59 | // Write slice to disk 60 | std::stringstream ss; 61 | ss << std::string(prefix) << std::setfill('0') << std::setw(3) << idx_slice << ".png"; 62 | cv::Mat slice_u; 63 | if (normalize_by_minmax) 64 | { 65 | // All slices are normalized using the range 66 | slice_img = (slice_img - min) / range; 67 | slice_img.convertTo(slice_u,CV_8UC1,255); 68 | } 69 | else 70 | { 71 | // Each slice is normalized independently from the rest 72 | cv::normalize(slice_img, slice_u, 0, 255, cv::NORM_MINMAX, CV_8UC1); 73 | } 74 | imwrite(ss.str(),slice_u); 75 | } 76 | } 77 | -------------------------------------------------------------------------------- /cartesian3dgrid/src/cartesian3dgrid_filter.cpp: -------------------------------------------------------------------------------- 1 | #include "cartesian3dgrid/cartesian3dgrid.h" 2 | 3 | #include 4 | #include 5 | 6 | #include "cartesian3dgrid/gaussianiir3d.h" 7 | 8 | 9 | void Grid3D::smoothInPlaceIIR3D(const float sigma_x, const float sigma_y, const float sigma_z) 10 | { 11 | int numsteps = 3; 12 | gaussianiir3d(&(data_array_.at(0)), size_[0], size_[1], size_[2], sigma_x, sigma_y, sigma_z, numsteps); 13 | } 14 | 15 | 16 | /** 17 | * \brief Diffuse grid using the heat equation (isotropic Gaussian blur) 18 | */ 19 | void diffuse ( std::vector* grid, const unsigned int width, const unsigned int height, const unsigned int depth, 20 | const float sigma) 21 | { 22 | std::vector grid_new(width*height*depth,0.f); // temporary array 23 | 24 | const float dt_CFL = 1.f / 12.f; // 3D heat equation, explicit scheme stability condition 25 | const float t_final = 0.5f * sigma*sigma; 26 | const float dt = std::min( 0.5f*dt_CFL, 0.5f*t_final); 27 | unsigned int steps = std::ceil(t_final / dt); 28 | 29 | const int stride_x = 1, stride_y = width, stride_z = width*height; 30 | 31 | while ( steps-- ) 32 | { 33 | for (int iz=0, p=0; iz < depth; iz++) 34 | { 35 | for (int iy=0; iy < height; iy++) 36 | { 37 | for (int ix=0; ix < width; ix++, p++) 38 | { 39 | // Boundary conditions: homogeneous Neumann 40 | int dx_pos = stride_x; if (ix+1 >= width){dx_pos = 0;} 41 | int dx_neg = -stride_x; if (ix-1 < 0){dx_neg = 0;} 42 | int dy_pos = stride_y; if (iy+1 >= height){dy_pos = 0;} 43 | int dy_neg = -stride_y; if (iy-1 < 0){dy_neg = 0;} 44 | int dz_pos = stride_z; if (iz+1 >= depth){dz_pos = 0;} 45 | int dz_neg = -stride_z; if (iz-1 < 0){dz_neg = 0;} 46 | 47 | // Index of current point 48 | //int p = stride_x*ix + stride_y*iy + stride_z*iz; 49 | // Compute 3D Laplacian: sum of second order derivatives 50 | float grid_p = grid->at(p); 51 | float Dxx = grid->at(p+dx_pos) - 2*grid_p + grid->at(p+dx_neg); 52 | float Dyy = grid->at(p+dy_pos) - 2*grid_p + grid->at(p+dy_neg); 53 | float Dzz = grid->at(p+dz_pos) - 2*grid_p + grid->at(p+dz_neg); 54 | // Update current point with explicit Euler scheme. 55 | // Stability condition (CFL) is 1-6*dt > 0 56 | grid_new.at(p) = grid_p + dt * ( Dxx + Dyy + Dzz ); 57 | } 58 | } 59 | } 60 | grid->swap(grid_new); 61 | } 62 | } 63 | 64 | 65 | //void Grid3D::smoothInPlace(const float sigma_x, const float sigma_y, const float sigma_z) 66 | void Grid3D::smoothInPlace(const float sigma) 67 | { 68 | diffuse ( &data_array_, size_[0], size_[1], size_[2], sigma); 69 | } 70 | 71 | 72 | void laplacian3D ( std::vector* grid, const unsigned int width, const unsigned int height, const unsigned int depth) 73 | { 74 | std::vector grid_new(width*height*depth,0.f); // temporary array 75 | 76 | const int stride_x = 1, stride_y = width, stride_z = width*height; 77 | 78 | for (int iz=0, p=0; iz < depth; iz++) 79 | { 80 | for (int iy=0; iy < height; iy++) 81 | { 82 | for (int ix=0; ix < width; ix++, p++) 83 | { 84 | // Boundary conditions: homogeneous Neumann 85 | int dx_pos = stride_x; if (ix+1 >= width){dx_pos = 0;} 86 | int dx_neg = -stride_x; if (ix-1 < 0){dx_neg = 0;} 87 | int dy_pos = stride_y; if (iy+1 >= height){dy_pos = 0;} 88 | int dy_neg = -stride_y; if (iy-1 < 0){dy_neg = 0;} 89 | int dz_pos = stride_z; if (iz+1 >= depth){dz_pos = 0;} 90 | int dz_neg = -stride_z; if (iz-1 < 0){dz_neg = 0;} 91 | 92 | // Index of current point 93 | //int p = stride_x*ix + stride_y*iy + stride_z*iz; 94 | // Compute 3D Laplacian: sum of second order derivatives 95 | float grid_p = grid->at(p); 96 | float Dxx = grid->at(p+dx_pos) - 2*grid_p + grid->at(p+dx_neg); 97 | float Dyy = grid->at(p+dy_pos) - 2*grid_p + grid->at(p+dy_neg); 98 | float Dzz = grid->at(p+dz_pos) - 2*grid_p + grid->at(p+dz_neg); 99 | grid_new.at(p) = Dxx + Dyy + Dzz; 100 | } 101 | } 102 | } 103 | grid->swap(grid_new); 104 | } 105 | 106 | 107 | void Grid3D::laplacianInPlace() 108 | { 109 | laplacian3D ( &data_array_, size_[0], size_[1], size_[2]); 110 | } 111 | 112 | 113 | double Grid3D::computeMoranIndexGaussianWeights(float sigma) const 114 | { 115 | if (sigma < 0.2) 116 | { 117 | // Need some "minimum" width to provide meaningful results 118 | sigma = 0.2; 119 | } 120 | 121 | // Standardized grid values 122 | double mean, stddev; 123 | computeMeanStd(&mean, &stddev); 124 | std::cout << "Grid3D: Mean = " << mean << " std = " << stddev << std::endl; 125 | 126 | float inv_stddev_f = 1.f / stddev; 127 | std::vector data_array_standardized(numCells_,0.f); 128 | for (int i=0; i data_array_standardized_smooth; 133 | data_array_standardized_smooth = data_array_standardized; // copy 134 | //diffuse(&data_array_standardized_smooth, size_[0], size_[1], size_[2], sigma); 135 | gaussianiir3d(&(data_array_standardized_smooth.at(0)), size_[0], size_[1], size_[2], sigma, sigma, sigma, 3); 136 | 137 | // Compute central weight of Gaussian kernel and its complement, the sum of weights of the neighbors 138 | const int half_size = 2 * std::ceil(3.f*sigma); // 2 to mitigate boundary issues 139 | const int kernel_size = 2*half_size + 1; 140 | std::vector weights(kernel_size*kernel_size*kernel_size, 0.f); 141 | unsigned int idx_center = half_size + kernel_size*(half_size + kernel_size*half_size); 142 | weights.at(idx_center) = 1.f; 143 | //diffuse(&weights, kernel_size, kernel_size, kernel_size, sigma); 144 | gaussianiir3d(&(weights.at(0)), kernel_size, kernel_size, kernel_size, sigma, sigma, sigma, 3); 145 | 146 | const float central_weight = weights.at(idx_center); 147 | /* 148 | //DEBUG 149 | std::cout << "sigma = " << sigma << std::endl; 150 | std::cout << "kernel_size = " << kernel_size << std::endl; 151 | std::cout << "central_weight = " << central_weight << std::endl; 152 | // print kernel 153 | std::cout << "weights" << std::endl; 154 | for (int iz=0; iz < kernel_size; iz++) 155 | { 156 | std::cout << "iz = " << iz << std::endl; 157 | for (int iy=0; iy < kernel_size; iy++) 158 | { 159 | for (int ix=0; ix < kernel_size; ix++) 160 | { 161 | int idx = ix + kernel_size*(iy + kernel_size*iz); 162 | std::cout << weights.at(idx) << " "; 163 | } 164 | std::cout << std::endl; 165 | } 166 | std::cout << std::endl; 167 | } 168 | */ 169 | 170 | // Compute sum of the weights (of the neighbors) 171 | const float sum_weights = 1.f - central_weight; 172 | /* 173 | //DEBUG 174 | weights.at(idx_center) = 0.f; // "remove" central point 175 | float sum_weights2 = 0.f; 176 | const int numCells = kernel_size*kernel_size*kernel_size; 177 | for (int i=0; i 0; x--) 86 | ptr[x - 1] += nu*ptr[x]; 87 | } 88 | } 89 | } 90 | 91 | /* Filter vertically along each column */ 92 | for(z = 0; z < depth; z++) 93 | { 94 | for(x = 0; x < width; x++) 95 | { 96 | for(step = 0; step < numsteps; step++) 97 | { 98 | ptr = volume + x + plane*z; 99 | ptr[0] *= boundaryscale; 100 | 101 | /* Filter downwards */ 102 | for(i = width; i < plane; i += width) 103 | ptr[i] += nu*ptr[i - width]; 104 | 105 | ptr[i = plane - width] *= boundaryscale; 106 | 107 | /* Filter upwards */ 108 | for(; i > 0; i -= width) 109 | ptr[i - width] += nu*ptr[i]; 110 | } 111 | } 112 | } 113 | 114 | /* Filter along z-dimension */ 115 | for(y = 0; y < height; y++) 116 | { 117 | for(x = 0; x < width; x++) 118 | { 119 | for(step = 0; step < numsteps; step++) 120 | { 121 | ptr = volume + x + width*y; 122 | ptr[0] *= boundaryscale; 123 | 124 | for(i = plane; i < numel; i += plane) 125 | ptr[i] += nu*ptr[i - plane]; 126 | 127 | ptr[i = numel - plane] *= boundaryscale; 128 | 129 | for(; i > 0; i -= plane) 130 | ptr[i - plane] += nu*ptr[i]; 131 | } 132 | } 133 | } 134 | 135 | for(i = 0; i < numel; i++) 136 | volume[i] *= postscale; 137 | 138 | return; 139 | } 140 | 141 | 142 | // Allow for a different sigma in each dimension 143 | void gaussianiir3d(float *volume, long width, long height, long depth, float sigma_x, float sigma_y, float sigma_z, int numsteps) 144 | { 145 | const long plane = width*height; 146 | const long numel = plane*depth; 147 | double lambda, dnu; 148 | float nu, boundaryscale, postscale = 1.f; 149 | float *ptr; 150 | long i, x, y, z; 151 | int step; 152 | 153 | if(sigma_x <= 0 || sigma_y <= 0 || sigma_z <= 0 || numsteps < 0) 154 | return; 155 | 156 | /* Filter horizontally along each row */ 157 | 158 | lambda = (sigma_x*sigma_x)/(2.0*numsteps); 159 | dnu = (1.0 + 2.0*lambda - sqrt(1.0 + 4.0*lambda))/(2.0*lambda); 160 | nu = (float)dnu; 161 | boundaryscale = (float)(1.0/(1.0 - dnu)); 162 | float postscale_x = (float)(pow(dnu/lambda,numsteps)); 163 | postscale *= postscale_x; 164 | 165 | for(z = 0; z < depth; z++) 166 | { 167 | for(y = 0; y < height; y++) 168 | { 169 | for(step = 0; step < numsteps; step++) 170 | { 171 | ptr = volume + width*(y + height*z); 172 | ptr[0] *= boundaryscale; 173 | 174 | /* Filter rightwards */ 175 | for(x = 1; x < width; x++) 176 | ptr[x] += nu*ptr[x - 1]; 177 | 178 | ptr[x = width - 1] *= boundaryscale; 179 | 180 | /* Filter leftwards */ 181 | for(; x > 0; x--) 182 | ptr[x - 1] += nu*ptr[x]; 183 | } 184 | } 185 | } 186 | 187 | /* Filter vertically along each column */ 188 | 189 | lambda = (sigma_y*sigma_y)/(2.0*numsteps); 190 | dnu = (1.0 + 2.0*lambda - sqrt(1.0 + 4.0*lambda))/(2.0*lambda); 191 | nu = (float)dnu; 192 | boundaryscale = (float)(1.0/(1.0 - dnu)); 193 | float postscale_y = (float)(pow(dnu/lambda,numsteps)); 194 | postscale *= postscale_y; 195 | 196 | for(z = 0; z < depth; z++) 197 | { 198 | for(x = 0; x < width; x++) 199 | { 200 | for(step = 0; step < numsteps; step++) 201 | { 202 | ptr = volume + x + plane*z; 203 | ptr[0] *= boundaryscale; 204 | 205 | /* Filter downwards */ 206 | for(i = width; i < plane; i += width) 207 | ptr[i] += nu*ptr[i - width]; 208 | 209 | ptr[i = plane - width] *= boundaryscale; 210 | 211 | /* Filter upwards */ 212 | for(; i > 0; i -= width) 213 | ptr[i - width] += nu*ptr[i]; 214 | } 215 | } 216 | } 217 | 218 | /* Filter along z-dimension */ 219 | 220 | lambda = (sigma_z*sigma_z)/(2.0*numsteps); 221 | dnu = (1.0 + 2.0*lambda - sqrt(1.0 + 4.0*lambda))/(2.0*lambda); 222 | nu = (float)dnu; 223 | boundaryscale = (float)(1.0/(1.0 - dnu)); 224 | float postscale_z = (float)(pow(dnu/lambda,numsteps)); 225 | postscale *= postscale_z; 226 | 227 | for(y = 0; y < height; y++) 228 | { 229 | for(x = 0; x < width; x++) 230 | { 231 | for(step = 0; step < numsteps; step++) 232 | { 233 | ptr = volume + x + width*y; 234 | ptr[0] *= boundaryscale; 235 | 236 | for(i = plane; i < numel; i += plane) 237 | ptr[i] += nu*ptr[i - plane]; 238 | 239 | ptr[i = numel - plane] *= boundaryscale; 240 | 241 | for(; i > 0; i -= plane) 242 | ptr[i - plane] += nu*ptr[i]; 243 | } 244 | } 245 | } 246 | 247 | for(i = 0; i < numel; i++) 248 | volume[i] *= postscale; 249 | 250 | return; 251 | } 252 | -------------------------------------------------------------------------------- /data/DSEC/interlaken_00-odometry/pose.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tub-rip/dvs_mcemvs/9ae7a374c72f2653c42cd36d663698eee8cfe509/data/DSEC/interlaken_00-odometry/pose.bag -------------------------------------------------------------------------------- /data/DSEC/zurich_city_02-odometry/pose.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tub-rip/dvs_mcemvs/9ae7a374c72f2653c42cd36d663698eee8cfe509/data/DSEC/zurich_city_02-odometry/pose.bag -------------------------------------------------------------------------------- /data/DSEC/zurich_city_04-odometry/pose.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tub-rip/dvs_mcemvs/9ae7a374c72f2653c42cd36d663698eee8cfe509/data/DSEC/zurich_city_04-odometry/pose.bag -------------------------------------------------------------------------------- /dependencies.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | catkin_simple: 3 | type: git 4 | url: https://github.com/catkin/catkin_simple.git 5 | version: master 6 | rpg_dvs_ros: 7 | type: git 8 | url: https://github.com/uzh-rpg/rpg_dvs_ros.git 9 | version: master 10 | gflags_catkin: 11 | type: git 12 | url: https://github.com/ethz-asl/gflags_catkin.git 13 | version: master 14 | glog_catkin: 15 | type: git 16 | url: https://github.com/ethz-asl/glog_catkin.git 17 | version: master 18 | minkindr: 19 | type: git 20 | url: https://github.com/ethz-asl/minkindr.git 21 | version: master 22 | eigen_catkin: 23 | type: git 24 | url: https://github.com/ethz-asl/eigen_catkin.git 25 | version: master 26 | eigen_checks: 27 | type: git 28 | url: https://github.com/ethz-asl/eigen_checks.git 29 | version: master 30 | cnpy: 31 | type: git 32 | url: git@github.com:uzh-rpg/cnpy_catkin.git 33 | version: master 34 | vicon: 35 | type: git 36 | url: https://github.com/KumarRobotics/vicon 37 | version: master 38 | 39 | -------------------------------------------------------------------------------- /docs/2022_MCEVMS_poster.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tub-rip/dvs_mcemvs/9ae7a374c72f2653c42cd36d663698eee8cfe509/docs/2022_MCEVMS_poster.pdf -------------------------------------------------------------------------------- /docs/block_all.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tub-rip/dvs_mcemvs/9ae7a374c72f2653c42cd36d663698eee8cfe509/docs/block_all.png -------------------------------------------------------------------------------- /docs/datasets.md: -------------------------------------------------------------------------------- 1 | ### Datasets 2 | - HKUST and edited RPG-ECCV 18 datasets [ESVO 2020](https://sites.google.com/view/esvo-project-page/home#h.tl1va3u667ae) 3 | - [RPG-ECCV 2018](http://rpg.ifi.uzh.ch/ECCV18_stereo_davis.html) 4 | - [MVSEC, RAL 2018](https://daniilidis-group.github.io/mvsec/) 5 | - [DSEC](https://dsec.ifi.uzh.ch/). Additionally, camera poses for some sequences are provided [in this directory](../data/DSEC). 6 | - [TUM-VIE](https://vision.in.tum.de/data/datasets/visual-inertial-event-dataset) 7 | 8 | Our code expects ROSBags for input events and poses. To convert event data from HDF5 files (as in DSEC, TUM-VIE) to ROSBags quickly, you may use [our events_h52bag C++ utility](https://github.com/tub-rip/events_h52bag). 9 | 10 | We suggest pre-processing the event data using the [DVS hot pixel filter](https://github.com/cedric-scheerlinck/dvs_tools/tree/master/dvs_hot_pixel_filter). It outputs bag files with a `.filtered` extension. 11 | -------------------------------------------------------------------------------- /docs/evaluation.md: -------------------------------------------------------------------------------- 1 | ### Evaluation scripts 2 | * **Visualization of the Disparity Space Images (DSIs)** can be done using the python scripts provided [here](https://github.com/tub-rip/dvs_mcemvs/blob/main/mapper_emvs_stereo/scripts). Please refer to the [the monocular package](https://github.com/uzh-rpg/rpg_emvs/blob/master/README.md#disparity-space-image-dsi) for documentation about using them. 3 | * We provide [an example script](https://github.com/tub-rip/dvs_mcemvs/blob/main/mapper_emvs_stereo/scripts/evaluate_mcemvs_dsec.py) for **computing the depth error metrics** for DSEC. Please modify the data paths before running. 4 | -------------------------------------------------------------------------------- /docs/examples.md: -------------------------------------------------------------------------------- 1 | ## Running Examples 2 | ### DSEC 3 | 4 | From the [DSEC dataset](https://dsec.ifi.uzh.ch/dsec-datasets/download/), download the following files: 5 | * [interlaken_00_b_events_left.zip](https://download.ifi.uzh.ch/rpg/DSEC/test/interlaken_00_b/interlaken_00_b_events_left.zip) 6 | * [interlaken_00_b_events_right.zip](https://download.ifi.uzh.ch/rpg/DSEC/test/interlaken_00_b/interlaken_00_b_events_right.zip) 7 | * [interlaken_00_b_calibration.zip](https://download.ifi.uzh.ch/rpg/DSEC/test/interlaken_00_b/interlaken_00_b_calibration.zip) for camera and hand-eye calibration. 8 | 9 | Camera poses obtained using LiDAR-IMU odometry are available [here](../data/DSEC/interlaken_00-odometry/pose.bag) in ROSBag format. Thanks to [Mathias Gehrig](https://magehrig.github.io/) for the data. 10 | 11 | Extract the zip files. Convert left and right events from h5 format to ROSBag. 12 | Clone and install [our h52bag converter](https://github.com/tub-rip/events_h52bag). Then, convert using: 13 | 14 | ./events_h52bag interlaken_00_b_events_left/events.h5 interlaken_00_b_events_left /dvs/left/events 480 640 15 | ./events_h52bag interlaken_00_b_events_right/events.h5 interlaken_00_b_events_right /dvs/right/events 480 640 16 | 17 | Since each h5 file is big (>500M events), this will generate multiple ROSBag files containing events from both cameras. In this example, we'll use the files `interlaken_00_b_events_left_1.bag` and `interlaken_00_b_events_right_1.bag`, which comprises a subset of the whole `interlaken_00_b` sequence. 18 | 19 | Set the correct path of the input events, poses and the unzipped calibration files by editing the configuration file `mapper_emvs_stereo/cfg/DSEC/interlaken_00_b_2/dsec.conf`. 20 | 21 | Finally, run mapper_emvs_stereo: 22 | 23 | roscd mapper_emvs_stereo 24 | cd cfg/DSEC/interlaken_00_b_2 25 | rosrun mapper_emvs_stereo run_emvs --flagfile=dsec.conf 26 | 27 | This will process the subsequence `interlaken_00_b_1` and generate a sequence of time-stamped output files (depth maps and confidence maps) in the current folder. 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 |
Depth mapConfidence map
39 | 40 | ### RPG_ECCV18_edited 41 | 42 | Download the [rpg_monitor_edited.bag](https://drive.google.com/file/d/1P8N3YfYnF5lgOgZGqkMU73otEnedztgy/view?usp=drive_web) file from the [ESVO 2020](https://sites.google.com/view/esvo-project-page/home#h.tl1va3u667ae), which contains stereo events and camera poses. 43 | 44 | Set the correct path of the filtered bag file in the `--bag_filename` parameter in the file `mapper_emvs_stereo/cfg/rpg_eccv18/monitor_edited/alg1/rpg_monitor_edited_fixedts.conf`. Make sure that the topic names are correct. Finally, run mapper_emvs_stereo 45 | 46 | roscd mapper_emvs_stereo 47 | cd cfg/rpg_eccv18/monitor_edited/alg1/ 48 | rosrun mapper_emvs_stereo run_emvs --flagfile=rpg_monitor_edited_fixedts.conf 49 | 50 | The output files will be saved in the current directory. 51 | The raw depth points are stored in `014.000000depth_points_fused_2.txt`file in the format `[row column depth]`. 52 | The color-coded inverse depth map is saved as `014.000000inv_depth_colored_dilated_fused_2.png`. 53 | The suffix `_2` denotes the fusion function used (Harmonic mean -HM- in this case). 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 |
Depth mapConfidence map
65 | 66 | 67 | ### TUM-VIE 68 | 69 | From the [TUM-VIE dataset](https://vision.in.tum.de/data/datasets/visual-inertial-event-dataset), download the following files: 70 | * [mocap-desk2-events_left.h5](https://tumevent-vi.vision.in.tum.de/mocap-desk2/mocap-desk2-events_left.h5) 71 | * [mocap-desk2-events_right.h5](https://tumevent-vi.vision.in.tum.de/mocap-desk2/mocap-desk2-events_right.h5) 72 | * [mocap-desk2-vi_gt_data.tar.gz](https://tumevent-vi.vision.in.tum.de/mocap-desk2/mocap-desk2-vi_gt_data.tar.gz) for camera poses. 73 | * [camera-calibrationA.json](https://tumevent-vi.vision.in.tum.de/camera-calibrationA.json) for camera and hand-eye calibration. 74 | 75 | 76 | Convert left and right events from h5 format to ROSBag. 77 | Clone and install [our h52bag converter](https://github.com/tub-rip/events_h52bag). Then, convert using: 78 | 79 | ./events_h52bag mocap-desk2-events_left.h5 mocap-desk2-events_left /dvs/left/events 720 1280 700000000 80 | ./events_h52bag mocap-desk2-events_right.h5 mocap-desk2-events_right /dvs/left/events 720 1280 700000000 81 | 82 | This should generate 2 bag files for the events, namely `mocap-desk2-events_left_0.bag` and `mocap-desk2-events_right_0.bag`. This was tested with 32GB RAM. If you run out of memory, use a lower number for `events_per_bag` instead `700000000`. This will split the output into multiple ROSBag files. 83 | 84 | Extract the contents of `mocap-desk2-vi_gt_data.tar.gz` into a folder `mocap-desk2-vi_gt_data`. Then, convert poses from `mocap_data.txt` to ROSBag using [this script](../mapper_emvs_stereo/scripts/mocap_txt2bag.py): 85 | 86 | python mapper_emvs_stereo/scripts/mocap_txt2bag.py --path_prefix mocap-desk2-vi_gt_data 87 | 88 | This should generate `pose.bag` as output inside the `mocap-desk2-vi_gt_data` folder. 89 | 90 | Set the correct path of the input events, poses and the calibration file by editing the configuration file `mapper_emvs_stereo/cfg/tumvie/desk2_full/tum-vie.conf`. 91 | 92 | Finally, run mapper_emvs_stereo: 93 | 94 | roscd mapper_emvs_stereo 95 | cd cfg/rpg_eccv18/tumvie/desk2_full 96 | rosrun mapper_emvs_stereo run_emvs --flagfile=tum-vie.conf 97 | 98 | This will process the whole desk2 sequence and generate a sequence of time-stamped output files (depth maps and confidence maps) in the current folder. 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 |
Depth mapConfidence map
110 | -------------------------------------------------------------------------------- /docs/installation.md: -------------------------------------------------------------------------------- 1 | ## Dependencies 2 | 3 | ### Install ROS Noetic (Ubuntu 20.04) 4 | 5 | ### Create a catkin workspace 6 | 7 | Create a catkin workspace (if there is none yet). For example, from your home folder: 8 | 9 | cd 10 | mkdir -p catkin_ws/src 11 | cd catkin_ws 12 | catkin config --init --mkdirs --extend /opt/ros/noetic --merge-devel --cmake-args -DCMAKE_BUILD_TYPE=Release 13 | 14 | The code has been tested on ROS Noetic. Depending on the ROS distribution you installed, you might have to use `kinetic` or `melodic` instead of `noetic` in the previous command. 15 | 16 | ### Add packages to the catkin workspace 17 | 18 | **Clone** this repository into the `src` folder of your catkin workspace. 19 | 20 | cd catkin_ws/src 21 | git clone git@github.com:tub-rip/dvs_mcemvs.git 22 | 23 | The catkin package dependencies are: 24 | - [catkin simple](https://github.com/catkin/catkin_simple) 25 | - ROS messages for DVS ([rpg_dvs_ros](https://github.com/uzh-rpg/rpg_dvs_ros)) 26 | - [Google Logging Library (glog)](https://github.com/catkin/catkin_simple.git) 27 | - [Gflags (formerly Google Commandline Flags)](https://github.com/ethz-asl/gflags_catkin) 28 | - [minkindr](https://github.com/ethz-asl/minkindr) (for dealing with poses). minkindr depends on [eigen_catkin](https://github.com/ethz-asl/eigen_catkin.git) and [eigen_checks](https://github.com/ethz-asl/eigen_checks.git), which are therefore also required. 29 | - [vicon](https://github.com/KumarRobotics/vicon) for reading poses in the form of vicon_msg 30 | 31 | The above dependencies are specified in the [dependencies.yaml](dependencies.yaml) file. They can be installed with the following commands from the `src` folder of your catkin workspace: 32 | 33 | cd catkin_ws/src 34 | sudo apt-get install python3-vcstool 35 | vcs-import < dvs_mcemvs/dependencies.yaml 36 | 37 | The previous command should clone the repositories into folders *catkin_simple*, *rpg_dvs_ros*, etc. inside the src/ folder of your catkin workspace, at the same level as this repository *dvs_mcemvs*. They should NOT be inside the *dvs_mcemvs* folder. 38 | 39 | Additional ROS tools needed (specified in the [package.xml](package.xml) file): 40 | 41 | sudo apt-get install ros-noetic-image-geometry 42 | sudo apt-get install ros-noetic-tf-conversions 43 | 44 | ## Compiling 45 | 46 | **Compile this package**: 47 | 48 | catkin build mapper_emvs_stereo 49 | 50 | After building, at least the first time, remember to run: 51 | 52 | source ~/catkin_ws/devel/setup.bash 53 | 54 | Sometimes (in case of strange errors) it is useful to delete the build folder before compilation: 55 | 56 | rm -rf build/mapper_emvs_stereo/ 57 | 58 | An alternative command to start from scratch (cleaning all catkin packages) is (to be used with *caution*): `catkin clean` 59 | -------------------------------------------------------------------------------- /docs/mcemvs_thumbnail.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tub-rip/dvs_mcemvs/9ae7a374c72f2653c42cd36d663698eee8cfe509/docs/mcemvs_thumbnail.jpg -------------------------------------------------------------------------------- /docs/running.md: -------------------------------------------------------------------------------- 1 | ## Running the code 2 | 3 | Running the code: 4 | 5 | rosrun mapper_emvs_stereo run_emvs --flagfile= 6 | 7 | The `` is used to define the set of parameters passed on the program. Examples of configuration files are provided in the `mapper_emvs_stereo/cfg` folder for different datasets and variants of the algorithm. 8 | 9 | ### Variants 10 | 11 | * **alg1**: Algorithm 1 in the paper. Only stereo fusion across cameras. This is the default variant if nothing is specified in the folder name. 12 | * **fusionfuncs**: Configurations for using different fusion functions for across-camera fusion -- minimum (min), Harmonic Mean (HM), Geometric Mean (GM), Arithmetic Mean (AM), Root Mean Square (RMS), maximum (max). 13 | * **alg2**: Algorithm 2 in the paper. Includes both temporal and stereo camera fusion. Different combinations of Arithmetic Mean (A) and Harmonic Mean (H) can be applied for temporal (t) and camera (c) fusion. For example, `AtHc` implies across-camera fusion happens first using Harmonic Mean, followed by temporal fusion using Arithmetic Mean. The order is important. 14 | * **shuffled**: Special case of algorithm 2, where the event sub-intervals of the stereo pair are shuffled before fusion. Denoted by `_shuffled` in the folder name suffix. 15 | * **full**: When a long data stream is broken into chunks and processed independently to produce a sequence of depth maps. Denoted by `_full` in the folder name. 16 | 17 | ### Configuration Parameters 18 | 19 | The main parameters of the `.conf` files are described below. Please take a look at `mapper_emvs_stereo/src/main.cpp` for a comprehensive list. 20 | 21 | * **bag_filename**: Path to single ROSBag file containing all events and camera poses. Otherwise, use the next 3 parameters when the data is available is separate bag files. 22 | * **bag_filename_left**: Path to ROSbag file containing left camera events 23 | * **bag_filename_right**: Path to ROSbag file containing right camera events 24 | * **bag_filename_pose**: Path to ROSbag file containing pose of stereo rig 25 | * **out_path**: Path to output results 26 | * **calib_type**: Refers to how calibration data has been provided (YAML, JSON, hard-coded etc.), depending on the source of the dataset. Refer to the `.conf` files to find the correct value of this parameter for each dataset. 27 | * **calib_path**: Path to intrinsic and extrinsic camera calibration file, needed for certain datasets specified by `calib_type`. 28 | * **mocap_calib_path**: Path to hand-eye calibration file, needed for certain datasets specified by `calib_type`. 29 | * **event_topic0**: Left event camera topic 30 | * **event_topic1**: Right event camera topic 31 | * **event_topic2** : Third event camera topic for trinocular fusion 32 | * **pose_topic**: Topic containing the pose of the stereo rig 33 | * **min_depth**: Minimum estimable depth of the scene 34 | * **max_depth**: Maximum estimable depth of the scene 35 | * **start_time_s**: Time (in seconds) from which we start processing events 36 | * **stop_time_s**: Time (in seconds) till which events are processed 37 | * **dimZ**: Number of depth planes to place within the assumed scene depth range. By default, the planes are uniformly distributed in _inverse depth_ space. To place depth planes uniformly along _depth_ space, turn the setting `DEFINE_USE_INVERSE_DEPTH` in `mapper_emvs_stereo/CMakeLists.txt` to `OFF`. 38 | * **process_method**: 1, 2 or 5. 39 | - 1: Algorithm 1 (fusion only across cameras); 40 | - 2: Algorithm 2 (fusion across cameras and time); 41 | - 5: Algorithm 2 + shuffling (shuffle sub-intervals before fusion) 42 | * **stereo_fusion**: A number between 1-6. It decides the method used to fuse DSIs across cameras. 43 | - 1: min; 44 | - 2: Harmonic mean; 45 | - 3: Geometric mean; 46 | - 4: Arithmetic mean; 47 | - 5: RMS mean; 48 | - 6: max 49 | * **temporal_fusion**: This parameter is used only when `process_method=2`. It describes the method used to fuse DSIs in time. Currently, only 2 (Harmonic mean) and 4 (Arithmetic mean) are supported. 50 | * **num_intervals**: This parameter is used only when `process_method=2`. Algorithm 2 divides the entire data duration into this many sub-intervals of equal duration. 51 | * **ts**: Timestamp (in seconds) of the left camera trajectory at which the reference view of the DSI is set. By default, it is set at mid-point of start and stop times. 52 | * **adaptive_threshold_c**: Parameter for Gaussian adaptive thresholding filter applied on the DSI before extracting depth map 53 | * **median_filter_size**: Kernel size of median filter applied on DSI for removing noise 54 | * **full_seq**: Divide a data sequence into smaller sections in time and process each chunk independently. A sliding window of a certain duration and stride is used to process the whole input sequence. This is used for generating sequences of depth maps over long sequences of data. 55 | * **duration**: Used only when `full_seq=true`. It specifies the duration (in seconds) each chunk of data that is processed independently. 56 | * **out_skip**: Used only when `full_seq=true`. It desribes the stride (in seconds) of the sliding window used to process the entire input sequence. The output depth frame rate is modulated with this parameter. 57 | * **max_confidence**: Use this value to manually set the upper limit of the range of values in each DSI, before normalization to [0, 255]. If set to 0 (by default), the maximum value of each DSI is used as its upper limit. For long sequences, setting a constant upper limit for all DSIs regularizes depth map filtering, and prevents sections with low event count from generating noisy depth estimates. In conjunction with `adaptive_threshold_c`, this values regulates the trade-off between reconstructing more 3D points and filtering out noise. Icreasing this parameter makes noise filtering stricter. 58 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(mapper_emvs_stereo) 2 | set(CMAKE_CXX_STANDARD 14) 3 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 4 | cmake_minimum_required(VERSION 2.8.3) 5 | 6 | find_package(catkin_simple REQUIRED COMPONENTS 7 | tf 8 | tf_conversions 9 | eigen_conversions 10 | vicon) 11 | catkin_simple(ALL_DEPS_REQUIRED) 12 | 13 | set(CMAKE_BUILD_TYPE RelWithDebInfo) # Release, RelWithDebInfo 14 | set(CMAKE_CXX_FLAGS "-O3 -fopenmp ${CMAKE_CXX_FLAGS}") 15 | 16 | set(HEADERS 17 | include/mapper_emvs_stereo/mapper_emvs_stereo.hpp 18 | include/mapper_emvs_stereo/data_loading.hpp 19 | include/mapper_emvs_stereo/depth_vector.hpp 20 | include/mapper_emvs_stereo/trajectory.hpp 21 | include/mapper_emvs_stereo/geometry_utils.hpp 22 | include/mapper_emvs_stereo/median_filtering.hpp 23 | include/mapper_emvs_stereo/calib.hpp 24 | include/mapper_emvs_stereo/utils.hpp 25 | include/mapper_emvs_stereo/process1.hpp 26 | include/mapper_emvs_stereo/process2.hpp 27 | include/mapper_emvs_stereo/process5.hpp 28 | ) 29 | 30 | set(SOURCES 31 | src/mapper_emvs_stereo.cpp 32 | src/data_loading.cpp 33 | src/median_filtering.cpp 34 | src/calib.cpp 35 | src/utils.cpp 36 | src/process1.cpp 37 | src/process2.cpp 38 | src/process5.cpp 39 | ) 40 | 41 | option(DEFINE_USE_INVERSE_DEPTH "Use linear spacing in inverse depth (if OFF, will use linear spacing in depth)" OFF) 42 | if(DEFINE_USE_INVERSE_DEPTH) 43 | add_definitions(-DUSE_INVERSE_DEPTH) 44 | endif(DEFINE_USE_INVERSE_DEPTH) 45 | 46 | cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS}) 47 | 48 | # Executables 49 | ################################################################################ 50 | 51 | cs_add_executable(run_emvs src/main.cpp) 52 | target_link_libraries(run_emvs ${PROJECT_NAME} yaml-cpp) 53 | 54 | ################################################################################ 55 | cs_install() 56 | cs_export() 57 | 58 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/DSEC/interlaken_00_b_2/dsec.conf: -------------------------------------------------------------------------------- 1 | --bag_filename_left=/home/suman/data/rpg/DSEC/interlaken_00_b/interlaken_00_b_events_left_2.bag 2 | --bag_filename_right=/home/suman/data/rpg/DSEC/interlaken_00_b/interlaken_00_b_events_right_2.bag 3 | --bag_filename_pose=/home/suman/data/rpg/DSEC/interlaken_00-odometry/pose.bag 4 | --out_path= 5 | --calib_type=dsec_yaml 6 | --calib_path=/home/suman/data/rpg/DSEC/interlaken_00_b/interlaken_00_b_calibration/cam_to_cam.yaml 7 | --mocap_calib_path=/home/suman/data/rpg/DSEC/interlaken_00_b/interlaken_00_b_calibration/cam_to_lidar.yaml 8 | --event_topic0=/dvs/left/events 9 | --event_topic1=/dvs/right/events 10 | --pose_topic=/pose 11 | --min_depth=4 12 | --max_depth=200 13 | --start_time_s=10 14 | --stop_time_s=35 15 | --duration=0.2 16 | --out_skip=1 17 | --dimZ=100 18 | --process_method=1 19 | --stereo_fusion=2 20 | --adaptive_threshold_c=4 21 | --max_confidence=0 22 | --forward_looking=true 23 | --full_seq=true 24 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/DSEC/zurich_04_a_full/dsec.conf: -------------------------------------------------------------------------------- 1 | --bag_filename_left=/home/suman/data/rpg/DSEC/zurich_city_04_a/left_events.bag 2 | --bag_filename_right=/home/suman/data/rpg/DSEC/zurich_city_04_a/right_events.bag 3 | --bag_filename_pose=/home/suman/data/rpg/DSEC/zurich_city_04-odometry/pose.bag 4 | --out_path= 5 | --calib_type=dsec_yaml 6 | --calib_path=/home/suman/data/rpg/DSEC/zurich_city_04_a/zurich_city_04_a_calibration/cam_to_cam.yaml 7 | --mocap_calib_path=/home/suman/data/rpg/DSEC/zurich_city_04_a/zurich_city_04_a_calibration/cam_to_lidar.yaml 8 | --event_topic0=/dvs/left/events 9 | --event_topic1=/dvs/right/events 10 | --pose_topic=/pose 11 | --min_depth=4 12 | --max_depth=200 13 | --start_time_s=10 14 | --stop_time_s=15 15 | --duration=0.2 16 | --out_skip=0.1 17 | --dimZ=100 18 | --process_method=1 19 | --stereo_fusion=2 20 | --adaptive_threshold_c=4 21 | --max_confidence=468 22 | --forward_looking=true 23 | --full_seq=true 24 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/evimo2/evimo2.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/mnt/HD4TB/data/evimo2/raw_sfm/sfm/eval/scene_03_00/scene_03_00.bag.filtered 2 | --out_path= 3 | --calib_path= 4 | --calib_type=evimo2 5 | --event_topic0=/samsung/camera/events 6 | --offset0=0.001 7 | --event_topic1=/prophesee/left/cd_events_buffer 8 | --offset1=-0.042 9 | --event_topic2=/prophesee/right/cd_events_buffer 10 | --offset2=-0.027 11 | --pose_topic=/vicon/dvs_rig 12 | --min_depth=0.5 13 | --max_depth=4 14 | --start_time_s=20.5 15 | --stop_time_s=21.5 16 | --ts=21 17 | --dimZ=100 18 | --process_method=1 19 | --stereo_fusion=2 20 | --adaptive_threshold_c=10 21 | --median_filter_size=9 22 | --save_mono=true 23 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/hkust/hkust_lab.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/home/suman/data/hkust/hkust_combined_filtered_hot.bag 2 | --out_path= 3 | --calib_path= 4 | --calib_type=hkust 5 | --event_topic0=/davis/left/events 6 | --event_topic1=/davis/right/events 7 | --pose_topic=/esvo_tracking/pose_filtered 8 | --min_depth=0.5 9 | --max_depth=4 10 | --start_time_s=31.5 11 | --stop_time_s=32.5 12 | --ts=32 13 | --dimZ=100 14 | --process_method=1 15 | --stereo_fusion=2 16 | --adaptive_threshold_c=8 17 | --median_filter_size=7 18 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/rpg_eccv18/boxes_edited/AtHc/rpg_boxes_edited.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/home/user/data/rpg_edited/rpg_boxes_edited.bag 2 | --out_path= 3 | --calib_type=eccv18 4 | --event_topic0=/davis/left/events 5 | --event_topic1=/davis/right/events 6 | --pose_topic=/optitrack/davis_stereo 7 | --min_depth=0.55 8 | --max_depth=6.25 9 | --start_time_s=0 10 | --stop_time_s=0.5 11 | --ts=0.25 12 | --dimZ=100 13 | --adaptive_threshold_c=5 14 | --process_method=2 15 | --stereo_fusion=2 16 | --temporal_fusion=4 17 | --num_intervals=2 18 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/rpg_eccv18/boxes_edited/AtHc_shuffled/rpg_boxes_edited.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/home/suman/data/rpg_edited/rpg_boxes_edited.bag 2 | --out_path= 3 | --calib_type=eccv18 4 | --event_topic0=/davis/left/events 5 | --event_topic1=/davis/right/events 6 | --pose_topic=/optitrack/davis_stereo 7 | --min_depth=0.55 8 | --max_depth=6.25 9 | --start_time_s=0 10 | --stop_time_s=0.5 11 | --ts=0.25 12 | --dimZ=100 13 | --adaptive_threshold_c=5 14 | --process_method=5 15 | --stereo_fusion=2 16 | --temporal_fusion=4 17 | --num_intervals=2 18 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/rpg_eccv18/boxes_edited/alg1/rpg_boxes_edited.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/home/suman/data/rpg_edited/rpg_boxes_edited.bag 2 | --out_path= 3 | --calib_type=eccv18 4 | --event_topic0=/davis/left/events 5 | --event_topic1=/davis/right/events 6 | --pose_topic=/optitrack/davis_stereo 7 | --min_depth=0.55 8 | --max_depth=6.25 9 | --start_time_s=0 10 | --stop_time_s=0.5 11 | --ts=0.25 12 | --dimZ=100 13 | --adaptive_threshold_c=5 14 | --process_method=1 15 | --stereo_fusion=2 16 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/rpg_eccv18/boxes_edited/fusionfuncs/alg1_AM/rpg_boxes_edited.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/home/user/data/rpg_edited/rpg_boxes_edited.bag 2 | --out_path= 3 | --calib_type=eccv18 4 | --event_topic0=/davis/left/events 5 | --event_topic1=/davis/right/events 6 | --pose_topic=/optitrack/davis_stereo 7 | --min_depth=0.55 8 | --max_depth=6.25 9 | --start_time_s=0 10 | --stop_time_s=0.5 11 | --ts=0.25 12 | --dimZ=100 13 | --adaptive_threshold_c=5 14 | --process_method=1 15 | --stereo_fusion=4 16 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/rpg_eccv18/boxes_edited/fusionfuncs/alg1_GM/rpg_boxes_edited.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/home/user/data/rpg_edited/rpg_boxes_edited.bag 2 | --out_path= 3 | --calib_type=eccv18 4 | --event_topic0=/davis/left/events 5 | --event_topic1=/davis/right/events 6 | --pose_topic=/optitrack/davis_stereo 7 | --min_depth=0.55 8 | --max_depth=6.25 9 | --start_time_s=0 10 | --stop_time_s=0.5 11 | --ts=0.25 12 | --dimZ=100 13 | --adaptive_threshold_c=5 14 | --process_method=1 15 | --stereo_fusion=3 16 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/rpg_eccv18/boxes_edited/fusionfuncs/alg1_HM/rpg_boxes_edited.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/home/user/data/rpg_edited/rpg_boxes_edited.bag 2 | --out_path= 3 | --calib_type=eccv18 4 | --event_topic0=/davis/left/events 5 | --event_topic1=/davis/right/events 6 | --pose_topic=/optitrack/davis_stereo 7 | --min_depth=0.55 8 | --max_depth=6.25 9 | --start_time_s=0 10 | --stop_time_s=0.5 11 | --ts=0.25 12 | --dimZ=100 13 | --adaptive_threshold_c=5 14 | --process_method=1 15 | --stereo_fusion=2 16 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/rpg_eccv18/boxes_edited/fusionfuncs/alg1_RMS/rpg_boxes_edited.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/home/user/data/rpg_edited/rpg_boxes_edited.bag 2 | --out_path= 3 | --calib_type=eccv18 4 | --event_topic0=/davis/left/events 5 | --event_topic1=/davis/right/events 6 | --pose_topic=/optitrack/davis_stereo 7 | --min_depth=0.55 8 | --max_depth=6.25 9 | --start_time_s=0 10 | --stop_time_s=0.5 11 | --ts=0.25 12 | --dimZ=100 13 | --adaptive_threshold_c=5 14 | --process_method=1 15 | --stereo_fusion=5 16 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/rpg_eccv18/boxes_edited/fusionfuncs/alg1_max/rpg_boxes_edited.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/home/user/data/rpg_edited/rpg_boxes_edited.bag 2 | --out_path= 3 | --calib_type=eccv18 4 | --event_topic0=/davis/left/events 5 | --event_topic1=/davis/right/events 6 | --pose_topic=/optitrack/davis_stereo 7 | --min_depth=0.55 8 | --max_depth=6.25 9 | --start_time_s=0 10 | --stop_time_s=0.5 11 | --ts=0.25 12 | --dimZ=100 13 | --adaptive_threshold_c=5 14 | --process_method=1 15 | --stereo_fusion=6 16 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/rpg_eccv18/boxes_edited/fusionfuncs/alg1_min/rpg_boxes_edited.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/home/user/data/rpg_edited/rpg_boxes_edited.bag 2 | --out_path= 3 | --calib_type=eccv18 4 | --event_topic0=/davis/left/events 5 | --event_topic1=/davis/right/events 6 | --pose_topic=/optitrack/davis_stereo 7 | --min_depth=0.55 8 | --max_depth=6.25 9 | --start_time_s=0 10 | --stop_time_s=0.5 11 | --ts=0.25 12 | --dimZ=100 13 | --adaptive_threshold_c=5 14 | --process_method=1 15 | --stereo_fusion=1 16 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/rpg_eccv18/monitor_edited/AtHc/rpg_monitor_edited_fixedts.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/home/user/data/rpg_edited/rpg_monitor_edited.bag 2 | --out_path= 3 | --calib_type=eccv18 4 | --event_topic0=/davis/left/events 5 | --event_topic1=/davis/right/events 6 | --pose_topic=/optitrack/davis_stereo 7 | --min_depth=0.55 8 | --max_depth=6.25 9 | --start_time_s=13.5 10 | --stop_time_s=14.5 11 | --dimZ=100 12 | --ts=14 13 | --median_filter_size=5 14 | --process_method=2 15 | --stereo_fusion=2 16 | --temporal_fusion=1 17 | --num_intervals=2 18 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/rpg_eccv18/monitor_edited/AtHc_shuffled/rpg_monitor_edited_fixedts.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/home/suman/data/rpg_edited/rpg_monitor_edited.bag 2 | --out_path= 3 | --calib_type=eccv18 4 | --event_topic0=/davis/left/events 5 | --event_topic1=/davis/right/events 6 | --pose_topic=/optitrack/davis_stereo 7 | --min_depth=0.55 8 | --max_depth=6.25 9 | --start_time_s=13.5 10 | --stop_time_s=14.5 11 | --dimZ=100 12 | --ts=14 13 | --median_filter_size=5 14 | --process_method=5 15 | --stereo_fusion=2 16 | --temporal_fusion=4 17 | --num_intervals=2 18 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/rpg_eccv18/monitor_edited/alg1/rpg_monitor_edited_fixedts.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/mnt/HD4TB/data/rpg/rpg_monitor_edited.bag 2 | --out_path= 3 | --calib_type=eccv18 4 | --event_topic0=/davis/left/events 5 | --event_topic1=/davis/right/events 6 | --pose_topic=/optitrack/davis_stereo 7 | --min_depth=0.55 8 | --max_depth=6.25 9 | --start_time_s=13.5 10 | --stop_time_s=14.5 11 | --dimZ=100 12 | --ts=14 13 | --median_filter_size=5 14 | --process_method=1 15 | --stereo_fusion=2 16 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/rpg_eccv18/monitor_edited/fusionfuncs/alg1_AM/rpg_monitor_edited_fixedts.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/home/user/data/rpg_edited/rpg_monitor_edited.bag 2 | --out_path= 3 | --calib_type=eccv18 4 | --event_topic0=/davis/left/events 5 | --event_topic1=/davis/right/events 6 | --pose_topic=/optitrack/davis_stereo 7 | --min_depth=0.55 8 | --max_depth=6.25 9 | --start_time_s=13.5 10 | --stop_time_s=14.5 11 | --dimZ=100 12 | --ts=14 13 | --median_filter_size=5 14 | --process_method=1 15 | --stereo_fusion=4 16 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/rpg_eccv18/monitor_edited/fusionfuncs/alg1_GM/rpg_monitor_edited_fixedts.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/home/user/data/rpg_edited/rpg_monitor_edited.bag 2 | --out_path= 3 | --calib_type=eccv18 4 | --event_topic0=/davis/left/events 5 | --event_topic1=/davis/right/events 6 | --pose_topic=/optitrack/davis_stereo 7 | --min_depth=0.55 8 | --max_depth=6.25 9 | --start_time_s=13.5 10 | --stop_time_s=14.5 11 | --dimZ=100 12 | --ts=14 13 | --median_filter_size=5 14 | --process_method=1 15 | --stereo_fusion=3 16 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/rpg_eccv18/monitor_edited/fusionfuncs/alg1_HM/rpg_monitor_edited_fixedts.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/home/user/data/rpg_edited/rpg_monitor_edited.bag 2 | --out_path= 3 | --calib_type=eccv18 4 | --event_topic0=/davis/left/events 5 | --event_topic1=/davis/right/events 6 | --pose_topic=/optitrack/davis_stereo 7 | --min_depth=0.55 8 | --max_depth=6.25 9 | --start_time_s=13.5 10 | --stop_time_s=14.5 11 | --dimZ=100 12 | --ts=14 13 | --median_filter_size=5 14 | --process_method=1 15 | --stereo_fusion=2 16 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/rpg_eccv18/monitor_edited/fusionfuncs/alg1_RMS/rpg_monitor_edited_fixedts.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/home/user/data/rpg_edited/rpg_monitor_edited.bag 2 | --out_path= 3 | --calib_type=eccv18 4 | --event_topic0=/davis/left/events 5 | --event_topic1=/davis/right/events 6 | --pose_topic=/optitrack/davis_stereo 7 | --min_depth=0.55 8 | --max_depth=6.25 9 | --start_time_s=13.5 10 | --stop_time_s=14.5 11 | --dimZ=100 12 | --ts=14 13 | --median_filter_size=5 14 | --process_method=1 15 | --stereo_fusion=5 16 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/rpg_eccv18/monitor_edited/fusionfuncs/alg1_max/rpg_monitor_edited_fixedts.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/home/user/data/rpg_edited/rpg_monitor_edited.bag 2 | --out_path= 3 | --calib_type=eccv18 4 | --event_topic0=/davis/left/events 5 | --event_topic1=/davis/right/events 6 | --pose_topic=/optitrack/davis_stereo 7 | --min_depth=0.55 8 | --max_depth=6.25 9 | --start_time_s=13.5 10 | --stop_time_s=14.5 11 | --dimZ=100 12 | --ts=14 13 | --median_filter_size=5 14 | --process_method=1 15 | --stereo_fusion=6 16 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/rpg_eccv18/monitor_edited/fusionfuncs/alg1_min/rpg_monitor_edited_fixedts.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/home/user/data/rpg_edited/rpg_monitor_edited.bag 2 | --out_path= 3 | --calib_type=eccv18 4 | --event_topic0=/davis/left/events 5 | --event_topic1=/davis/right/events 6 | --pose_topic=/optitrack/davis_stereo 7 | --min_depth=0.55 8 | --max_depth=6.25 9 | --start_time_s=13.5 10 | --stop_time_s=14.5 11 | --dimZ=100 12 | --ts=14 13 | --median_filter_size=5 14 | --process_method=1 15 | --stereo_fusion=1 16 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/rpg_eccv18/reader/alg1/rpg_reader_fixedts.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/home/suman/data/rpg/reader.bag.filtered 2 | --out_path= 3 | --calib_type=eccv18 4 | --event_topic0=/davis_left/events 5 | --event_topic1=/davis_right/events 6 | --pose_topic=/optitrack/davis_stereo 7 | --min_depth=0.55 8 | --max_depth=6.25 9 | --start_time_s=4.8 10 | --stop_time_s=5.6 11 | --dimZ=100 12 | --ts=5.2 13 | --process_method=1 14 | --stereo_fusion=2 15 | --adaptive_threshold_c=10 16 | --median_filter_size=7 17 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/tumvie/bike-easy-3_full/tum-vie.conf: -------------------------------------------------------------------------------- 1 | --bag_filename_left=/mnt/HD4TB/data/tum-vie/bike-easy-events_left_3.bag.filtered 2 | --bag_filename_right=/mnt/HD4TB/data/tum-vie/bike-easy-events_right_3.bag.filtered 3 | --bag_filename_pose=/mnt/HD4TB/data/tum-vie/bike-easy-vi_gt_data/basalt_pose.bag 4 | --out_path= 5 | --calib_type=json 6 | --calib_path=/mnt/HD4TB/data/tum-vie/camera-calibrationB.json 7 | --mocap_calib_path= 8 | --event_topic0=/dvs/left/events 9 | --event_topic1=/dvs/right/events 10 | --pose_topic=/pose 11 | --min_depth=3 12 | --max_depth=100 13 | --start_time_s=1 14 | --stop_time_s=15 15 | --dimZ=100 16 | --process_method=1 17 | --out_skip=0.5 18 | --duration=0.5 19 | --adaptive_threshold_c=5 20 | --forward_looking=true 21 | --full_seq=true 22 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/tumvie/bike-easy-4_full/tum-vie.conf: -------------------------------------------------------------------------------- 1 | --bag_filename_left=/mnt/HD4TB/data/tum-vie/bike-easy-events_left_4.bag.filtered 2 | --bag_filename_right=/mnt/HD4TB/data/tum-vie/bike-easy-events_right_4.bag.filtered 3 | --bag_filename_pose=/mnt/HD4TB/data/tum-vie/bike-easy-vi_gt_data/basalt_pose.bag 4 | --out_path= 5 | --calib_type=json 6 | --calib_path=/mnt/HD4TB/data/tum-vie/camera-calibrationB.json 7 | --mocap_calib_path= 8 | --event_topic0=/dvs/left/events 9 | --event_topic1=/dvs/right/events 10 | --pose_topic=/pose 11 | --min_depth=3 12 | --max_depth=100 13 | --start_time_s=1 14 | --stop_time_s=20 15 | --dimZ=100 16 | --process_method=1 17 | --out_skip=0.5 18 | --duration=0.5 19 | --adaptive_threshold_c=5 20 | --forward_looking=true 21 | --full_seq=true 22 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/tumvie/bike-easy-5_full/tum-vie.conf: -------------------------------------------------------------------------------- 1 | --bag_filename_left=/mnt/HD4TB/data/tum-vie/bike-easy-events_left_5.bag.filtered 2 | --bag_filename_right=/mnt/HD4TB/data/tum-vie/bike-easy-events_right_5.bag.filtered 3 | --bag_filename_pose=/mnt/HD4TB/data/tum-vie/bike-easy-vi_gt_data/basalt_pose.bag 4 | --out_path= 5 | --calib_type=json 6 | --calib_path=/mnt/HD4TB/data/tum-vie/camera-calibrationB.json 7 | --mocap_calib_path= 8 | --event_topic0=/dvs/left/events 9 | --event_topic1=/dvs/right/events 10 | --pose_topic=/pose 11 | --min_depth=3 12 | --max_depth=100 13 | --start_time_s=1 14 | --stop_time_s=15 15 | --dimZ=100 16 | --process_method=1 17 | --out_skip=0.1 18 | --duration=0.5 19 | --adaptive_threshold_c=5 20 | --forward_looking=true 21 | --full_seq=true 22 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/tumvie/desk2_full/tum-vie.conf: -------------------------------------------------------------------------------- 1 | --bag_filename_left=/mnt/HD4TB/data/tum-vie/mocap-desk2-events_left.bag.filtered 2 | --bag_filename_right=/mnt/HD4TB/data/tum-vie/mocap-desk2-events_right.bag.filtered 3 | --bag_filename_pose=/mnt/HD4TB/data/tum-vie/mocap-desk2-vi_gt_data/pose.bag 4 | --out_path= 5 | --calib_type=json 6 | --calib_path=/mnt/HD4TB/data/tum-vie/camera-calibrationA.json 7 | --mocap_calib_path=/mnt/HD4TB/data/tum-vie/mocap-imu-calibrationA.json 8 | --event_topic0=/dvs/left/events 9 | --event_topic1=/dvs/right/events 10 | --pose_topic=/pose 11 | --min_depth=0.45 12 | --max_depth=4 13 | --start_time_s=10 14 | --stop_time_s=22 15 | --dimZ=100 16 | --process_method=1 17 | --adaptive_threshold_c=7 18 | --out_skip=0.5 19 | --duration=0.5 20 | --full_seq=true 21 | --max_confidence=0 22 | --save_conf_stats=true 23 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/tumvie/mocap-1d-trans_full/tum-vie.conf: -------------------------------------------------------------------------------- 1 | --bag_filename_left=/mnt/HD4TB/data/tum-vie/mocap-1d-trans-events_left.bag.filtered 2 | --bag_filename_right=/mnt/HD4TB/data/tum-vie/mocap-1d-trans-events_right.bag.filtered 3 | --bag_filename_pose=/mnt/HD4TB/data/tum-vie/mocap-1d-trans-vi_gt_data/pose.bag 4 | --out_path= 5 | --calib_type=json 6 | --calib_path=/mnt/HD4TB/data/tum-vie/camera-calibrationB.json 7 | --mocap_calib_path=/mnt/HD4TB/data/tum-vie/mocap-imu-calibrationB.json 8 | --event_topic0=/dvs/left/events 9 | --event_topic1=/dvs/right/events 10 | --pose_topic=/pose 11 | --min_depth=0.45 12 | --max_depth=4 13 | --start_time_s=5 14 | --stop_time_s=30 15 | --out_skip=1 16 | --duration=0.1 17 | --dimZ=100 18 | --process_method=1 19 | --adaptive_threshold_c=7 20 | --forward_looking=false 21 | --full_seq=true 22 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/tumvie/mocap-3d-trans_full/tum-vie.conf: -------------------------------------------------------------------------------- 1 | --bag_filename_left=/mnt/HD4TB/data/tum-vie/mocap-3d-trans-events_left.bag.filtered 2 | --bag_filename_right=/mnt/HD4TB/data/tum-vie/mocap-3d-trans-events_right.bag.filtered 3 | --bag_filename_pose=/mnt/HD4TB/data/tum-vie/mocap-3d-trans-vi_gt_data/pose.bag 4 | --out_path= 5 | --calib_type=json 6 | --calib_path=/mnt/HD4TB/data/tum-vie/camera-calibrationB.json 7 | --mocap_calib_path=/mnt/HD4TB/data/tum-vie/mocap-imu-calibrationB.json 8 | --event_topic0=/dvs/left/events 9 | --event_topic1=/dvs/right/events 10 | --pose_topic=/pose 11 | --min_depth=0.45 12 | --max_depth=4 13 | --start_time_s=5 14 | --stop_time_s=30 15 | --out_skip=1 16 | --duration=0.1 17 | --dimZ=100 18 | --process_method=1 19 | --adaptive_threshold_c=7 20 | --forward_looking=false 21 | --full_seq=true 22 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/tumvie/running-easy-0_full/tum-vie.conf: -------------------------------------------------------------------------------- 1 | --bag_filename_left=/mnt/HD4TB/data/tum-vie/running-easy-events_left_0.bag.filtered 2 | --bag_filename_right=/mnt/HD4TB/data/tum-vie/running-easy-events_right_0.bag.filtered 3 | --bag_filename_pose=/mnt/HD4TB/data/tum-vie/running-easy-vi_gt_data/pose.bag 4 | --out_path= 5 | --calib_type=json 6 | --calib_path=/mnt/HD4TB/data/tum-vie/camera-calibrationB.json 7 | --mocap_calib_path=/mnt/HD4TB/data/tum-vie/mocap-imu-calibrationB.json 8 | --event_topic0=/dvs/left/events 9 | --event_topic1=/dvs/right/events 10 | --pose_topic=/pose 11 | --min_depth=1 12 | --max_depth=20 13 | --start_time_s=9 14 | --stop_time_s=22 15 | --ts=6 16 | --dimZ=100 17 | --process_method=1 18 | --adaptive_threshold_c=7 19 | --duration=0.5 20 | --out_skip=0.5 21 | --forward_looking=false 22 | --full_seq=true 23 | --save_conf_stats=true 24 | --max_confidence=0 25 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/tumvie/running-easy-1_full/tum-vie.conf: -------------------------------------------------------------------------------- 1 | --bag_filename_left=/mnt/HD4TB/data/tum-vie/running-easy-events_left_1.bag.filtered 2 | --bag_filename_right=/mnt/HD4TB/data/tum-vie/running-easy-events_right_1.bag.filtered 3 | --bag_filename_pose=/mnt/HD4TB/data/tum-vie/running-easy-vi_gt_data/basalt_pose.bag 4 | --out_path= 5 | --calib_type=json 6 | --calib_path=/mnt/HD4TB/data/tum-vie/camera-calibrationB.json 7 | --mocap_calib_path= 8 | --event_topic0=/dvs/left/events 9 | --event_topic1=/dvs/right/events 10 | --pose_topic=/pose 11 | --min_depth=1 12 | --max_depth=20 13 | --start_time_s=1 14 | --stop_time_s=20 15 | --dimZ=100 16 | --process_method=1 17 | --out_skip=0.1 18 | --duration=0.5 19 | --adaptive_threshold_c=5 20 | --forward_looking=true 21 | --full_seq=true 22 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/tumvie/skate-easy-1_full/tum-vie.conf: -------------------------------------------------------------------------------- 1 | --bag_filename_left=/mnt/HD4TB/data/tum-vie/skate-easy-events_left_1.bag.filtered 2 | --bag_filename_right=/mnt/HD4TB/data/tum-vie/skate-easy-events_right_1.bag.filtered 3 | --bag_filename_pose=/mnt/HD4TB/data/tum-vie/skate-easy-vi_gt_data/odometry.bag 4 | --out_path= 5 | --calib_type=json 6 | --calib_path=/mnt/HD4TB/data/tum-vie/camera-calibrationA.json 7 | --mocap_calib_path= 8 | --event_topic0=/dvs/left/events 9 | --event_topic1=/dvs/right/events 10 | --pose_topic=/rovio/pose_with_covariance_stamped 11 | --min_depth=1 12 | --max_depth=20 13 | --start_time_s=1 14 | --stop_time_s=20 15 | --dimZ=100 16 | --process_method=1 17 | --out_skip=0.1 18 | --duration=0.5 19 | --adaptive_threshold_c=5 20 | --forward_looking=true 21 | --full_seq=true 22 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/upenn_mvsec/mvsec_flying1_full/AcAt/flying1.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/mnt/HD4TB/data/mvsec/indoor_flying1_combined.bag.filtered 2 | --out_path= 3 | --calib_type=yaml_mvsec 4 | --calib_path=/mnt/HD4TB/data/mvsec/calib_in_flying/camchain-imucam-indoor_flying.yaml 5 | --event_topic0=/davis/left/events 6 | --event_topic1=/davis/right/events 7 | --pose_topic=/davis/left/pose 8 | --min_depth=1 9 | --max_depth=6.5 10 | --start_time_s=10 11 | --stop_time_s=65 12 | --dimZ=100 13 | --process_method=2 14 | --stereo_fusion=4 15 | --temporal_fusion=4 16 | --num_intervals=2 17 | --out_skip=0.05 18 | --duration=1 19 | --adaptive_threshold_c=14 20 | --median_filter_size=9 21 | --full_seq=true 22 | --save_conf_stats=false 23 | --max_confidence=28.4 24 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/upenn_mvsec/mvsec_flying1_full/AcHt/flying1.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/mnt/HD4TB/data/mvsec/indoor_flying1_combined.bag.filtered 2 | --out_path= 3 | --calib_type=yaml_mvsec 4 | --calib_path=/mnt/HD4TB/data/mvsec/calib_in_flying/camchain-imucam-indoor_flying.yaml 5 | --event_topic0=/davis/left/events 6 | --event_topic1=/davis/right/events 7 | --pose_topic=/davis/left/pose 8 | --min_depth=1 9 | --max_depth=6.5 10 | --start_time_s=10 11 | --stop_time_s=65 12 | --dimZ=100 13 | --process_method=2 14 | --stereo_fusion=4 15 | --temporal_fusion=2 16 | --num_intervals=2 17 | --out_skip=0.05 18 | --duration=1 19 | --adaptive_threshold_c=14 20 | --median_filter_size=9 21 | --full_seq=true 22 | --save_conf_stats=false 23 | --max_confidence=28.1 24 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/upenn_mvsec/mvsec_flying1_full/AtHc/flying1.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/mnt/HD4TB/data/mvsec/indoor_flying1_combined.bag.filtered 2 | --out_path= 3 | --calib_type=yaml_mvsec 4 | --calib_path=/mnt/HD4TB/data/mvsec/calib_in_flying/camchain-imucam-indoor_flying.yaml 5 | --event_topic0=/davis/left/events 6 | --event_topic1=/davis/right/events 7 | --pose_topic=/davis/left/pose 8 | --min_depth=1 9 | --max_depth=6.5 10 | --start_time_s=10 11 | --stop_time_s=65 12 | --dimZ=100 13 | --process_method=2 14 | --stereo_fusion=2 15 | --temporal_fusion=4 16 | --num_intervals=2 17 | --out_skip=0.05 18 | --duration=1 19 | --adaptive_threshold_c=14 20 | --median_filter_size=9 21 | --full_seq=true 22 | --save_conf_stats=false 23 | --max_confidence=28.3 24 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/upenn_mvsec/mvsec_flying1_full/AtHc_shuffled/flying1.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/mnt/HD4TB/data/mvsec/indoor_flying1_combined.bag.filtered 2 | --out_path= 3 | --calib_type=yaml_mvsec 4 | --calib_path=/mnt/HD4TB/data/mvsec/calib_in_flying/camchain-imucam-indoor_flying.yaml 5 | --event_topic0=/davis/left/events 6 | --event_topic1=/davis/right/events 7 | --pose_topic=/davis/left/pose 8 | --min_depth=1 9 | --max_depth=6.5 10 | --start_time_s=10 11 | --stop_time_s=65 12 | --dimZ=100 13 | --process_method=5 14 | --stereo_fusion=2 15 | --temporal_fusion=4 16 | --num_intervals=2 17 | --out_skip=0.05 18 | --duration=1 19 | --adaptive_threshold_c=14 20 | --median_filter_size=9 21 | --full_seq=true 22 | --save_conf_stats=false 23 | --max_confidence=28.3 24 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/upenn_mvsec/mvsec_flying1_full/HcHt/flying1.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/mnt/HD4TB/data/mvsec/indoor_flying1_combined.bag.filtered 2 | --out_path= 3 | --calib_type=yaml_mvsec 4 | --calib_path=/mnt/HD4TB/data/mvsec/calib_in_flying/camchain-imucam-indoor_flying.yaml 5 | --event_topic0=/davis/left/events 6 | --event_topic1=/davis/right/events 7 | --pose_topic=/davis/left/pose 8 | --min_depth=1 9 | --max_depth=6.5 10 | --start_time_s=10 11 | --stop_time_s=65 12 | --dimZ=100 13 | --process_method=2 14 | --stereo_fusion=2 15 | --temporal_fusion=2 16 | --num_intervals=2 17 | --out_skip=0.05 18 | --duration=1 19 | --adaptive_threshold_c=14 20 | --median_filter_size=9 21 | --full_seq=true 22 | --save_conf_stats=false 23 | --max_confidence=28.1 24 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/upenn_mvsec/mvsec_flying1_full/alg1/flying1.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/mnt/HD4TB/data/mvsec/indoor_flying1_combined.bag.filtered 2 | --out_path= 3 | --calib_type=yaml_mvsec 4 | --calib_path=/mnt/HD4TB/data/mvsec/calib_in_flying/camchain-imucam-indoor_flying.yaml 5 | --event_topic0=/davis/left/events 6 | --event_topic1=/davis/right/events 7 | --pose_topic=/davis/left/pose 8 | --min_depth=1 9 | --max_depth=6.5 10 | --start_time_s=10 11 | --stop_time_s=65 12 | --dimZ=100 13 | --process_method=1 14 | --full_seq=true 15 | --out_skip=0.05 16 | --duration=1 17 | --adaptive_threshold_c=14 18 | --median_filter_size=9 19 | --max_confidence=57.7 20 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/upenn_mvsec/mvsec_flying2_full/AcAt/flying2.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/mnt/HD4TB/data/mvsec/indoor_flying2_combined.bag.filtered 2 | --out_path= 3 | --calib_type=yaml_mvsec 4 | --calib_path=/mnt/HD4TB/data/mvsec/calib_in_flying/camchain-imucam-indoor_flying.yaml 5 | --event_topic0=/davis/left/events 6 | --event_topic1=/davis/right/events 7 | --pose_topic=/davis/left/pose 8 | --min_depth=1 9 | --max_depth=6.5 10 | --start_time_s=10 11 | --stop_time_s=79 12 | --dimZ=100 13 | --process_method=2 14 | --stereo_fusion=4 15 | --temporal_fusion=4 16 | --num_intervals=2 17 | --out_skip=0.05 18 | --duration=1 19 | --adaptive_threshold_c=14 20 | --median_filter_size=9 21 | --full_seq=true 22 | --save_conf_stats=false 23 | --max_confidence=30.4 24 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/upenn_mvsec/mvsec_flying2_full/AcHt/flying2.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/mnt/HD4TB/data/mvsec/indoor_flying2_combined.bag.filtered 2 | --out_path= 3 | --calib_type=yaml_mvsec 4 | --calib_path=/mnt/HD4TB/data/mvsec/calib_in_flying/camchain-imucam-indoor_flying.yaml 5 | --event_topic0=/davis/left/events 6 | --event_topic1=/davis/right/events 7 | --pose_topic=/davis/left/pose 8 | --min_depth=1 9 | --max_depth=6.5 10 | --start_time_s=10 11 | --stop_time_s=79 12 | --dimZ=100 13 | --process_method=2 14 | --stereo_fusion=4 15 | --temporal_fusion=2 16 | --num_intervals=2 17 | --out_skip=0.05 18 | --duration=1 19 | --adaptive_threshold_c=14 20 | --median_filter_size=9 21 | --full_seq=true 22 | --save_conf_stats=false 23 | --max_confidence=29.4 24 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/upenn_mvsec/mvsec_flying2_full/AtHc/flying2.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/mnt/HD4TB/data/mvsec/indoor_flying2_combined.bag.filtered 2 | --out_path= 3 | --calib_type=yaml_mvsec 4 | --calib_path=/mnt/HD4TB/data/mvsec/calib_in_flying/camchain-imucam-indoor_flying.yaml 5 | --event_topic0=/davis/left/events 6 | --event_topic1=/davis/right/events 7 | --pose_topic=/davis/left/pose 8 | --min_depth=1 9 | --max_depth=6.5 10 | --start_time_s=10 11 | --stop_time_s=79 12 | --dimZ=100 13 | --process_method=2 14 | --stereo_fusion=2 15 | --temporal_fusion=4 16 | --num_intervals=2 17 | --out_skip=0.05 18 | --duration=1 19 | --adaptive_threshold_c=14 20 | --median_filter_size=9 21 | --full_seq=true 22 | --save_conf_stats=false 23 | --max_confidence=30.1 24 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/upenn_mvsec/mvsec_flying2_full/AtHc_shuffled/flying2.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/mnt/HD4TB/data/mvsec/indoor_flying2_combined.bag.filtered 2 | --out_path= 3 | --calib_type=yaml_mvsec 4 | --calib_path=/mnt/HD4TB/data/mvsec/calib_in_flying/camchain-imucam-indoor_flying.yaml 5 | --event_topic0=/davis/left/events 6 | --event_topic1=/davis/right/events 7 | --pose_topic=/davis/left/pose 8 | --min_depth=1 9 | --max_depth=6.5 10 | --start_time_s=10 11 | --stop_time_s=79 12 | --dimZ=100 13 | --process_method=5 14 | --stereo_fusion=2 15 | --temporal_fusion=4 16 | --num_intervals=2 17 | --out_skip=0.05 18 | --duration=1 19 | --adaptive_threshold_c=14 20 | --median_filter_size=9 21 | --full_seq=true 22 | --save_conf_stats=false 23 | --max_confidence=30.1 24 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/upenn_mvsec/mvsec_flying2_full/HcHt/flying2.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/mnt/HD4TB/data/mvsec/indoor_flying2_combined.bag.filtered 2 | --out_path= 3 | --calib_type=yaml_mvsec 4 | --calib_path=/mnt/HD4TB/data/mvsec/calib_in_flying/camchain-imucam-indoor_flying.yaml 5 | --event_topic0=/davis/left/events 6 | --event_topic1=/davis/right/events 7 | --pose_topic=/davis/left/pose 8 | --min_depth=1 9 | --max_depth=6.5 10 | --start_time_s=10 11 | --stop_time_s=79 12 | --dimZ=100 13 | --process_method=2 14 | --stereo_fusion=2 15 | --temporal_fusion=2 16 | --num_intervals=2 17 | --out_skip=0.05 18 | --duration=1 19 | --adaptive_threshold_c=14 20 | --median_filter_size=9 21 | --full_seq=true 22 | --save_conf_stats=false 23 | --max_confidence=29.3 24 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/upenn_mvsec/mvsec_flying2_full/norm_max_c14/flying2.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/mnt/HD4TB/data/mvsec/indoor_flying2_combined.bag.filtered 2 | --out_path= 3 | --calib_type=yaml_mvsec 4 | --calib_path=/mnt/HD4TB/data/mvsec/calib_in_flying/camchain-imucam-indoor_flying.yaml 5 | --event_topic0=/davis/left/events 6 | --event_topic1=/davis/right/events 7 | --pose_topic=/davis/left/pose 8 | --min_depth=1 9 | --max_depth=6.5 10 | --start_time_s=10 11 | --stop_time_s=79 12 | --dimZ=100 13 | --process_method=1 14 | --full_seq=true 15 | --out_skip=0.05 16 | --duration=1 17 | --adaptive_threshold_c=14 18 | --median_filter_size=9 19 | --max_confidence=78 20 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/upenn_mvsec/mvsec_flying3_full/AcAt/flying3.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/mnt/HD4TB/data/mvsec/indoor_flying3_combined.bag.filtered 2 | --out_path= 3 | --calib_type=yaml_mvsec 4 | --calib_path=/mnt/HD4TB/data/mvsec/calib_in_flying/camchain-imucam-indoor_flying.yaml 5 | --event_topic0=/davis/left/events 6 | --event_topic1=/davis/right/events 7 | --pose_topic=/davis/left/pose 8 | --min_depth=1 9 | --max_depth=6.5 10 | --start_time_s=10 11 | --stop_time_s=89 12 | --dimZ=100 13 | --process_method=2 14 | --stereo_fusion=4 15 | --temporal_fusion=4 16 | --num_intervals=2 17 | --out_skip=0.05 18 | --duration=1 19 | --adaptive_threshold_c=14 20 | --median_filter_size=9 21 | --full_seq=true 22 | --save_conf_stats=false 23 | --max_confidence=33.6 24 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/upenn_mvsec/mvsec_flying3_full/AcHt/flying3.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/mnt/HD4TB/data/mvsec/indoor_flying3_combined.bag.filtered 2 | --out_path= 3 | --calib_type=yaml_mvsec 4 | --calib_path=/mnt/HD4TB/data/mvsec/calib_in_flying/camchain-imucam-indoor_flying.yaml 5 | --event_topic0=/davis/left/events 6 | --event_topic1=/davis/right/events 7 | --pose_topic=/davis/left/pose 8 | --min_depth=1 9 | --max_depth=6.5 10 | --start_time_s=10 11 | --stop_time_s=89 12 | --dimZ=100 13 | --process_method=2 14 | --stereo_fusion=4 15 | --temporal_fusion=2 16 | --num_intervals=2 17 | --out_skip=0.05 18 | --duration=1 19 | --adaptive_threshold_c=14 20 | --median_filter_size=9 21 | --full_seq=true 22 | --save_conf_stats=false 23 | --max_confidence=30.6 24 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/upenn_mvsec/mvsec_flying3_full/AtHc/flying3.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/mnt/HD4TB/data/mvsec/indoor_flying3_combined.bag.filtered 2 | --out_path= 3 | --calib_type=yaml_mvsec 4 | --calib_path=/mnt/HD4TB/data/mvsec/calib_in_flying/camchain-imucam-indoor_flying.yaml 5 | --event_topic0=/davis/left/events 6 | --event_topic1=/davis/right/events 7 | --pose_topic=/davis/left/pose 8 | --min_depth=1 9 | --max_depth=6.5 10 | --start_time_s=10 11 | --stop_time_s=89 12 | --dimZ=100 13 | --process_method=2 14 | --stereo_fusion=2 15 | --temporal_fusion=4 16 | --num_intervals=2 17 | --out_skip=0.05 18 | --duration=1 19 | --adaptive_threshold_c=14 20 | --median_filter_size=9 21 | --full_seq=true 22 | --save_conf_stats=false 23 | --max_confidence=33.4 24 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/upenn_mvsec/mvsec_flying3_full/AtHc_shuffled/flying3.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/mnt/HD4TB/data/mvsec/indoor_flying3_combined.bag.filtered 2 | --out_path= 3 | --calib_type=yaml_mvsec 4 | --calib_path=/mnt/HD4TB/data/mvsec/calib_in_flying/camchain-imucam-indoor_flying.yaml 5 | --event_topic0=/davis/left/events 6 | --event_topic1=/davis/right/events 7 | --pose_topic=/davis/left/pose 8 | --min_depth=1 9 | --max_depth=6.5 10 | --start_time_s=10 11 | --stop_time_s=89 12 | --dimZ=100 13 | --process_method=5 14 | --stereo_fusion=2 15 | --temporal_fusion=4 16 | --num_intervals=2 17 | --out_skip=0.05 18 | --duration=1 19 | --adaptive_threshold_c=14 20 | --median_filter_size=9 21 | --full_seq=true 22 | --save_conf_stats=false 23 | --max_confidence=33.4 24 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/upenn_mvsec/mvsec_flying3_full/HcHt/flying3.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/mnt/HD4TB/data/mvsec/indoor_flying3_combined.bag.filtered 2 | --out_path= 3 | --calib_type=yaml_mvsec 4 | --calib_path=/mnt/HD4TB/data/mvsec/calib_in_flying/camchain-imucam-indoor_flying.yaml 5 | --event_topic0=/davis/left/events 6 | --event_topic1=/davis/right/events 7 | --pose_topic=/davis/left/pose 8 | --min_depth=1 9 | --max_depth=6.5 10 | --start_time_s=10 11 | --stop_time_s=89 12 | --dimZ=100 13 | --process_method=2 14 | --stereo_fusion=2 15 | --temporal_fusion=2 16 | --num_intervals=2 17 | --out_skip=0.05 18 | --duration=1 19 | --adaptive_threshold_c=14 20 | --median_filter_size=9 21 | --full_seq=true 22 | --save_conf_stats=false 23 | --max_confidence=30.3 24 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/cfg/upenn_mvsec/mvsec_flying3_full/norm_max_c14/flying3.conf: -------------------------------------------------------------------------------- 1 | --bag_filename=/mnt/HD4TB/data/mvsec/indoor_flying3_combined.bag.filtered 2 | --out_path= 3 | --calib_type=yaml_mvsec 4 | --calib_path=/mnt/HD4TB/data/mvsec/calib_in_flying/camchain-imucam-indoor_flying.yaml 5 | --event_topic0=/davis/left/events 6 | --event_topic1=/davis/right/events 7 | --pose_topic=/davis/left/pose 8 | --min_depth=1 9 | --max_depth=6.5 10 | --start_time_s=10 11 | --stop_time_s=89 12 | --dimZ=100 13 | --process_method=1 14 | --full_seq=true 15 | --out_skip=0.05 16 | --duration=1 17 | --adaptive_threshold_c=14 18 | --median_filter_size=9 19 | --max_confidence=78.8 20 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/include/mapper_emvs_stereo/calib.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * \file calib.hpp 3 | * \brief header for calibration functions 4 | * \author (1) Suman Ghosh 5 | * \date 2022-09-01 6 | * \author (2) Guillermo Gallego 7 | * \date 2022-09-01 8 | * Copyright/Rights of Use: 9 | * 2022, Technische Universität Berlin 10 | * Prof. Guillermo Gallego 11 | * Robotic Interactive Perception 12 | * Marchstrasse 23, Sekr. MAR 5-5 13 | * 10587 Berlin, Germany 14 | */ 15 | 16 | #pragma once 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | // Calibration functions TEMPORARY. They should be read from input files 24 | 25 | void loadCalibInfo(const std::string &cameraSystemDir, bool bPrintCalibInfo); 26 | 27 | 28 | void get_camera_calib_yaml_m3ed(image_geometry::PinholeCameraModel& cam0, 29 | image_geometry::PinholeCameraModel& cam1, 30 | Eigen::Matrix4d& mat4_1_0, 31 | Eigen::Matrix4d& mat4_hand_eye, 32 | std::string calib_path); 33 | 34 | void get_camera_calib_dsec_yaml(image_geometry::PinholeCameraModel& cam0, 35 | image_geometry::PinholeCameraModel& cam1, 36 | Eigen::Matrix4d& mat4_1_0, 37 | Eigen::Matrix4d& mat4_hand_eye, 38 | std::string calib_path, 39 | std::string mocap_calib_path); 40 | 41 | void get_camera_calib_dsec_zurich04a(image_geometry::PinholeCameraModel& cam0, 42 | image_geometry::PinholeCameraModel& cam1, 43 | Eigen::Matrix4d& matR_L, 44 | Eigen::Matrix4d& mat4_hand_eye, 45 | std::string calib_path); 46 | 47 | void get_camera_calib_dsec_interlaken00b(image_geometry::PinholeCameraModel& cam0, 48 | image_geometry::PinholeCameraModel& cam1, 49 | Eigen::Matrix4d& matR_L, 50 | Eigen::Matrix4d& mat4_hand_eye, 51 | std::string calib_path); 52 | void get_camera_calib_yaml(image_geometry::PinholeCameraModel& cam0, 53 | image_geometry::PinholeCameraModel& cam1, 54 | Eigen::Matrix4d& mat4_1_0, 55 | Eigen::Matrix4d& mat4_hand_eye, 56 | std::string calib_path); 57 | 58 | void get_camera_calib_json(image_geometry::PinholeCameraModel& cam0, 59 | image_geometry::PinholeCameraModel& cam1, 60 | Eigen::Matrix4d& mat4_1_0, 61 | Eigen::Matrix4d& mat4_hand_eye, 62 | std::string calib_pathA, 63 | std::string calib_pathB); 64 | 65 | void get_camera_calib_slider(image_geometry::PinholeCameraModel& cam0, 66 | image_geometry::PinholeCameraModel& cam1, 67 | Eigen::Matrix4d& mat4_1_0, 68 | Eigen::Matrix4d& mat4_hand_eye, 69 | std::string calib_path); 70 | 71 | void get_camera_calib_hkust(image_geometry::PinholeCameraModel& cam0, 72 | image_geometry::PinholeCameraModel& cam1, 73 | Eigen::Matrix4d& mat4_1_0, 74 | Eigen::Matrix4d& mat4_hand_eye, 75 | std::string calib_path); 76 | 77 | void get_camera_calib_evimo2(image_geometry::PinholeCameraModel& cam0, 78 | image_geometry::PinholeCameraModel& cam1, 79 | image_geometry::PinholeCameraModel& cam2, 80 | Eigen::Matrix4d& mat4_1_0, 81 | Eigen::Matrix4d& mat4_2_0, 82 | Eigen::Matrix4d& mat4_hand_eye, 83 | std::string calib_path); 84 | 85 | void get_camera_calib_yaml_mvsec(image_geometry::PinholeCameraModel& cam0, 86 | image_geometry::PinholeCameraModel& cam1, 87 | Eigen::Matrix4d& mat4_1_0, 88 | Eigen::Matrix4d& mat4_hand_eye, 89 | std::string calib_path); 90 | 91 | void get_camera_calib_ESIM(image_geometry::PinholeCameraModel& cam0, 92 | image_geometry::PinholeCameraModel& cam1, 93 | Eigen::Matrix4d& mat4_1_0, 94 | Eigen::Matrix4d& mat4_hand_eye); 95 | 96 | // rpg_DAVIS_stereo dataset (Joey ECCV'18) 97 | void get_camera_calib_ECCV18(image_geometry::PinholeCameraModel& cam0, 98 | image_geometry::PinholeCameraModel& cam1, 99 | Eigen::Matrix4d& mat4_1_0, 100 | Eigen::Matrix4d& mat4_hand_eye); 101 | 102 | // Samsung DVS Gen3 stereo 103 | void get_camera_calib_DVS_Gen3(image_geometry::PinholeCameraModel& cam0, 104 | image_geometry::PinholeCameraModel& cam1, 105 | Eigen::Matrix4d& mat4_1_0, 106 | Eigen::Matrix4d& mat4_hand_eye); 107 | 108 | void get_camera_calib_sony(image_geometry::PinholeCameraModel& cam0, 109 | image_geometry::PinholeCameraModel& cam1, 110 | Eigen::Matrix4d& mat4_1_0, 111 | Eigen::Matrix4d& mat4_hand_eye, 112 | std::string calib_path, 113 | std::string mocap_calib_path); 114 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/include/mapper_emvs_stereo/data_loading.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * \file data_loading.hpp 3 | * \brief header for functions to load events and poses from ROSBags 4 | * \author (1) Suman Ghosh 5 | * \date 2022-09-01 6 | * \author (2) Guillermo Gallego 7 | * \date 2022-09-01 8 | * Copyright/Rights of Use: 9 | * 2022, Technische Universität Berlin 10 | * Prof. Guillermo Gallego 11 | * Robotic Interactive Perception 12 | * Marchstrasse 23, Sekr. MAR 5-5 13 | * 10587 Berlin, Germany 14 | */ 15 | 16 | 17 | #pragma once 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | namespace data_loading { 28 | 29 | void parse_rosbag(const std::string &rosbag, 30 | std::vector& events_, 31 | std::map& poses_, 32 | sensor_msgs::CameraInfo& camera_info_msg, 33 | const std::string& event_topic, 34 | const std::string& camera_info_topic, 35 | const std::string& pose_topic, 36 | const double tmin, 37 | const double tmax, 38 | const double events_offset); 39 | 40 | // MVSEC has separated files for data and ground truth 41 | void parse_rosbag(const std::string &rosbag, 42 | std::vector& events_, 43 | sensor_msgs::CameraInfo& camera_info_msg, 44 | const std::string& event_topic, 45 | const std::string& camera_info_topic, 46 | const double tmin, 47 | const double tmax, 48 | const double events_offset); 49 | 50 | void parse_rosbag_gt(const std::string &rosbag, 51 | std::map& poses_, 52 | const std::string& pose_topic, 53 | const double tmin, 54 | const double tmax); 55 | 56 | } // namespace data_loading 57 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/include/mapper_emvs_stereo/depth_vector.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace EMVS 7 | { 8 | 9 | // Stores a set of data associations index <-> depth 10 | // Used to abstract how the depth is sampled 11 | 12 | // Uses the "Curiously Recursive Template Pattern" to allow static polymorphism 13 | // for performance (since these functions will be called very often) 14 | 15 | template 16 | class DepthVector { 17 | public: 18 | DerivedDepthVector& derived() 19 | { return static_cast(*this); } 20 | 21 | DepthVector() {} 22 | 23 | DepthVector(float min_depth, 24 | float max_depth, 25 | size_t num_depth_cells) 26 | { 27 | CHECK_GT(min_depth, 0.0); 28 | CHECK_GT(max_depth, 0.0); 29 | CHECK_GE(num_depth_cells, 1u); 30 | 31 | num_depth_cells_ = num_depth_cells; 32 | min_depth_ = min_depth; 33 | max_depth_ = max_depth; 34 | if(min_depth_ > max_depth_) 35 | std::swap(min_depth_, max_depth_); 36 | 37 | derived().init(); 38 | } 39 | 40 | size_t size() const { return vec_.size(); } 41 | 42 | float cellIndexToDepth(size_t i) 43 | { 44 | return derived().cellIndexToDepth(i); 45 | } 46 | 47 | size_t depthToCellIndex(float depth) 48 | { 49 | return derived().depthToCellIndex(depth); 50 | } 51 | 52 | float depthToCell(float depth) 53 | { 54 | return derived().depthToCell(depth); 55 | } 56 | 57 | std::vector getDepthVector() 58 | { 59 | std::vector out; 60 | for(size_t i=0; i vec_; 69 | size_t num_depth_cells_; 70 | float min_depth_; 71 | float max_depth_; 72 | float depth_to_cell_idx_multiplier_; 73 | }; 74 | 75 | 76 | class LinearDepthVector : public DepthVector 77 | { 78 | public: 79 | 80 | LinearDepthVector() : DepthVector() {} 81 | 82 | LinearDepthVector(float min_depth, 83 | float max_depth, 84 | size_t num_depth_cells) : DepthVector(min_depth, 85 | max_depth, 86 | num_depth_cells) {} 87 | 88 | void init() 89 | { 90 | LOG(INFO) << "Using linear spacing in depth"; 91 | depth_to_cell_idx_multiplier_ = (float)((num_depth_cells_) / (max_depth_-min_depth_)); 92 | 93 | vec_.resize(num_depth_cells_); 94 | for(size_t i=0; i 120 | { 121 | public: 122 | 123 | InverseDepthVector() : DepthVector() {} 124 | 125 | InverseDepthVector(float min_depth, 126 | float max_depth, 127 | size_t num_depth_cells) : DepthVector(min_depth, 128 | max_depth, 129 | num_depth_cells) {} 130 | 131 | void init() 132 | { 133 | LOG(INFO) << "Using linear spacing in inverse depth"; 134 | inv_min_depth_ = 1.f / min_depth_; 135 | inv_max_depth_ = 1.f / max_depth_; 136 | depth_to_cell_idx_multiplier_ = (float)((num_depth_cells_) / (inv_min_depth_ - inv_max_depth_)); 137 | 138 | vec_.resize(num_depth_cells_); 139 | for(size_t i=0; i 4 | #include 5 | #include 6 | 7 | namespace geometry_utils { 8 | 9 | using Transformation = kindr::minimal::QuatTransformation; 10 | using Quaternion = kindr::minimal::RotationQuaternion; 11 | 12 | typedef float Depth; 13 | typedef float InverseDepth; 14 | typedef Eigen::Vector3d BearingVector; 15 | typedef Eigen::Vector2d Keypoint; // subpixel pixel coordinate 16 | typedef Eigen::Vector3d Point; 17 | typedef Eigen::Vector2d MotionVector; 18 | typedef Eigen::Vector2d GradientVector; 19 | typedef Eigen::Vector3d LinearVelocity; 20 | typedef Eigen::Vector3d AngularVelocity; 21 | 22 | 23 | class PinholeCamera 24 | { 25 | 26 | public: 27 | PinholeCamera() {} 28 | PinholeCamera(int width, int height, float fx, float fy, float cx, float cy) 29 | : width_(width), 30 | height_(height), 31 | fx_(fx), 32 | fy_(fy), 33 | cx_(cx), 34 | cy_(cy) 35 | { 36 | CHECK_GT(width_, 0); 37 | CHECK_GT(height_, 0); 38 | CHECK_GT(fx_, 0.0); 39 | CHECK_GT(fy_, 0.0); 40 | CHECK_GT(cx_, 0.0); 41 | CHECK_GT(cy_, 0.0); 42 | 43 | K_ << (float) fx_, 0.f , (float) cx_, 44 | 0.f , (float) fy_ , (float) cy_, 45 | 0.f , 0.f , 1.f; 46 | 47 | Kinv_ = K_.inverse(); 48 | } 49 | 50 | inline Keypoint project3dToPixel(const Point& P) 51 | { 52 | CHECK_GE(std::fabs(P[2]), 1e-6); 53 | return Keypoint(fx_*P[0]/P[2] + cx_, fy_*P[1]/P[2] + cy_); 54 | } 55 | 56 | inline BearingVector projectPixelTo3dRay(const Keypoint& u) 57 | { 58 | return BearingVector((u[0]-cx_)/fx_, (u[1]-cy_)/fy_, 1.0); 59 | } 60 | 61 | Eigen::Matrix3f K() const 62 | { 63 | return K_; 64 | } 65 | 66 | int width_; 67 | int height_; 68 | float fx_; 69 | float fy_; 70 | float cx_; 71 | float cy_; 72 | Eigen::Matrix3f K_; 73 | Eigen::Matrix3f Kinv_; 74 | }; 75 | 76 | } 77 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/include/mapper_emvs_stereo/mapper_emvs_stereo.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * \file mapper_emvs_stereo.hpp 3 | * \brief header file for functions to create and process DSIs 4 | * \author (1) Suman Ghosh 5 | * \date 2022-09-01 6 | * \author (2) Guillermo Gallego 7 | * \date 2022-09-01 8 | * Copyright/Rights of Use: 9 | * 2022, Technische Universität Berlin 10 | * Prof. Guillermo Gallego 11 | * Robotic Interactive Perception 12 | * Marchstrasse 23, Sekr. MAR 5-5 13 | * 10587 Berlin, Germany 14 | */ 15 | 16 | #pragma once 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | namespace EMVS { 30 | 31 | typedef pcl::PointXYZI PointType; 32 | typedef pcl::PointCloud PointCloud; 33 | 34 | #ifdef USE_INVERSE_DEPTH 35 | using TypeDepthVector = InverseDepthVector; 36 | #else 37 | using TypeDepthVector = LinearDepthVector; 38 | #endif 39 | 40 | struct ShapeDSI { 41 | 42 | public: 43 | 44 | ShapeDSI(){} 45 | 46 | ShapeDSI(size_t dimX, size_t dimY, size_t dimZ, 47 | float min_depth, float max_depth, 48 | float fov) 49 | : dimX_(dimX) 50 | , dimY_(dimY) 51 | , dimZ_(dimZ) 52 | , min_depth_(min_depth) 53 | , max_depth_(max_depth) 54 | , fov_(fov) {} 55 | 56 | size_t dimX_; 57 | size_t dimY_; 58 | size_t dimZ_; 59 | 60 | float min_depth_; 61 | float max_depth_; 62 | 63 | // Field of View 64 | float fov_; 65 | }; 66 | 67 | 68 | struct OptionsDepthMap 69 | { 70 | // Adaptive Gaussian Thresholding parameters 71 | int adaptive_threshold_kernel_size_; 72 | double adaptive_threshold_c_; 73 | double max_confidence; 74 | bool full_sequence; 75 | bool save_conf_stats; 76 | bool save_mono; 77 | double rv_pos; 78 | // Kernel size of median filter 79 | int median_filter_size_; 80 | }; 81 | 82 | 83 | struct OptionsPointCloud 84 | { 85 | // Outlier removal parameters 86 | float radius_search_; 87 | int min_num_neighbors_; 88 | }; 89 | 90 | 91 | typedef LinearTrajectory TrajectoryType; 92 | 93 | class MapperEMVS 94 | { 95 | public: 96 | 97 | MapperEMVS(){} 98 | MapperEMVS& operator=(const MapperEMVS& m){return *this;} 99 | 100 | MapperEMVS(const image_geometry::PinholeCameraModel& cam, 101 | const ShapeDSI &dsi_shape); 102 | 103 | bool evaluateDSI(const std::vector& events, 104 | const TrajectoryType& trajectory, 105 | const geometry_utils::Transformation& T_rv_w); 106 | 107 | void getDepthMapFromDSI(cv::Mat& depth_map, cv::Mat &confidence_map, cv::Mat &mask, const OptionsDepthMap &options_depth_map, int method=-1); 108 | void getDepthMapFromDSI(cv::Mat& depth_map, cv::Mat &confidence_map, cv::Mat &mask, const OptionsDepthMap &options_depth_map, cv::Mat& depth_map_dense, int method=-1); 109 | 110 | void getPointcloud(const cv::Mat& depth_map, 111 | const cv::Mat& mask, 112 | const OptionsPointCloud &options_pc, 113 | PointCloud::Ptr &pc_); 114 | 115 | Grid3D dsi_; 116 | std::string name; 117 | 118 | 119 | private: 120 | 121 | void setupDSI(); 122 | 123 | void precomputeRectifiedPoints(); 124 | 125 | void fillVoxelGrid(const std::vector &event_locations_z0, 126 | const std::vector &camera_centers); 127 | 128 | void convertDepthIndicesToValues(const cv::Mat &depth_cell_indices, cv::Mat &depth_map); 129 | 130 | void removeMaskBoundary(cv::Mat& mask, int border_size); 131 | 132 | 133 | // Intrinsics of the camera 134 | image_geometry::PinholeCameraModel dvs_cam_; 135 | Eigen::Matrix3f K_; 136 | int width_; 137 | int height_; 138 | std::stringstream distortion_model_; 139 | 140 | // (Constant) parameters that define the DSI (size and intrinsics) 141 | ShapeDSI dsi_shape_; 142 | geometry_utils::PinholeCamera virtual_cam_; 143 | 144 | // Precomputed vector of num_depth_cells_ inverse depths, 145 | // uniformly sampled in inverse depth space 146 | TypeDepthVector depths_vec_; 147 | std::vector raw_depths_vec_; 148 | 149 | // Precomputed (normalized) bearing vectors for each pixel of the reference image 150 | Eigen::Matrix2Xf precomputed_rectified_points_; 151 | 152 | const size_t packet_size_ = 1024; 153 | 154 | }; 155 | 156 | } 157 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/include/mapper_emvs_stereo/median_filtering.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | 6 | void huangMedianFilter(const cv::Mat& img, cv::Mat& out_img, const cv::Mat& mask, int patch_size); 7 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/include/mapper_emvs_stereo/process1.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * \file process1.hpp 3 | * \brief header file for Alg 1 4 | * \author (1) Suman Ghosh 5 | * \date 2022-09-01 6 | * \author (2) Guillermo Gallego 7 | * \date 2022-09-01 8 | * Copyright/Rights of Use: 9 | * 2022, Technische Universität Berlin 10 | * Prof. Guillermo Gallego 11 | * Robotic Interactive Perception 12 | * Marchstrasse 23, Sekr. MAR 5-5 13 | * 10587 Berlin, Germany 14 | */ 15 | 16 | #pragma once 17 | 18 | #include 19 | #include 20 | 21 | // Compute both DSIs, fuse by harmonic mean, then extract semi-dense depth map 22 | void process_1( 23 | const image_geometry::PinholeCameraModel& cam0, 24 | const image_geometry::PinholeCameraModel& cam1, 25 | const image_geometry::PinholeCameraModel& cam2, 26 | const LinearTrajectory& trajectory0, 27 | const LinearTrajectory& trajectory1, 28 | const LinearTrajectory& trajectory2, 29 | const std::vector& events0, 30 | const std::vector& events1, 31 | const std::vector& events2, 32 | const EMVS::OptionsDepthMap& opts_depth_map, 33 | const EMVS::ShapeDSI& dsi_shape, 34 | EMVS::MapperEMVS& mapper_fused, 35 | EMVS::MapperEMVS& mapper0, 36 | EMVS::MapperEMVS& mapper1, 37 | EMVS::MapperEMVS& mapper2, 38 | const std::string& out_path, 39 | double ts, 40 | int fusion_method 41 | ); 42 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/include/mapper_emvs_stereo/process2.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * \file process2.hpp 3 | * \brief header for Alg 2 4 | * \author (1) Suman Ghosh 5 | * \date 2022-09-01 6 | * \author (2) Guillermo Gallego 7 | * \date 2022-09-01 8 | * Copyright/Rights of Use: 9 | * 2022, Technische Universität Berlin 10 | * Prof. Guillermo Gallego 11 | * Robotic Interactive Perception 12 | * Marchstrasse 23, Sekr. MAR 5-5 13 | * 10587 Berlin, Germany 14 | */ 15 | 16 | #pragma once 17 | 18 | #include 19 | #include 20 | 21 | // Compute both DSIs, fuse by harmonic mean, then extract semi-dense depth map 22 | void process_2(const image_geometry::PinholeCameraModel& cam0, 23 | const image_geometry::PinholeCameraModel& cam1, 24 | const LinearTrajectory& trajectory0, 25 | const LinearTrajectory& trajectory1, 26 | const std::vector& events0, 27 | const std::vector& events1, 28 | const EMVS::OptionsDepthMap& opts_depth_map, 29 | const EMVS::ShapeDSI& dsi_shape, 30 | const int num_subintervals, 31 | EMVS::MapperEMVS& mapper_fused, 32 | EMVS::MapperEMVS& mapper_fused_camera_time, 33 | const std::string& out_path, 34 | double ts, 35 | int stereo_fusion, 36 | int temporal_fusion 37 | ); 38 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/include/mapper_emvs_stereo/process5.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * \file process5.hpp 3 | * \brief header file for Alg 2 + shuffling 4 | * \author (1) Suman Ghosh 5 | * \date 2022-09-01 6 | * \author (2) Guillermo Gallego 7 | * \date 2022-09-01 8 | * Copyright/Rights of Use: 9 | * 2022, Technische Universität Berlin 10 | * Prof. Guillermo Gallego 11 | * Robotic Interactive Perception 12 | * Marchstrasse 23, Sekr. MAR 5-5 13 | * 10587 Berlin, Germany 14 | */ 15 | 16 | #pragma once 17 | 18 | #include 19 | #include 20 | 21 | // Compute both DSIs, fuse by harmonic mean, then extract semi-dense depth map 22 | void process_5(const image_geometry::PinholeCameraModel& cam0, 23 | const image_geometry::PinholeCameraModel& cam1, 24 | const LinearTrajectory& trajectory0, 25 | const LinearTrajectory& trajectory1, 26 | const std::vector& events0, 27 | const std::vector& events1, 28 | const EMVS::OptionsDepthMap& opts_depth_map, 29 | const EMVS::ShapeDSI& dsi_shape, 30 | const int num_subintervals, 31 | EMVS::MapperEMVS& mapper_fused, 32 | const std::string& out_path, 33 | double ts, 34 | int stereo_fusion, 35 | int temporal_fusion 36 | ); 37 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/include/mapper_emvs_stereo/trajectory.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | template 8 | class Trajectory 9 | { 10 | public: 11 | typedef std::map PoseMap; 12 | 13 | DerivedTrajectory& derived() 14 | { return static_cast(*this); } 15 | 16 | Trajectory() {} 17 | 18 | Trajectory(const PoseMap& poses) 19 | : poses_(poses) 20 | {} 21 | 22 | // Returns T_W_C (mapping points from C to the world frame W) 23 | bool getPoseAt(const ros::Time& t, geometry_utils::Transformation& T) const 24 | { 25 | return derived().getPoseAt(t, T); 26 | } 27 | 28 | void getFirstControlPose(geometry_utils::Transformation* T, ros::Time* t) const 29 | { 30 | *t = poses_.begin()->first; 31 | *T = poses_.begin()->second; 32 | } 33 | 34 | void getLastControlPose(geometry_utils::Transformation* T, ros::Time* t) const 35 | { 36 | *t = poses_.rbegin()->first; 37 | *T = poses_.rbegin()->second; 38 | } 39 | 40 | size_t getNumControlPoses() const 41 | { 42 | return poses_.size(); 43 | } 44 | 45 | bool print() const 46 | { 47 | size_t control_pose_idx = 0u; 48 | for(auto it : poses_) 49 | { 50 | LOG(INFO) << "--Control pose #" << control_pose_idx++ << ". time = " << it.first; 51 | LOG(INFO) << "--T = "; 52 | LOG(INFO) << it.second; 53 | } 54 | return true; 55 | } 56 | 57 | void applyTransformationRight(const geometry_utils::Transformation& T) 58 | { 59 | for(auto it : poses_) 60 | { 61 | poses_[it.first] = it.second * T; 62 | } 63 | } 64 | 65 | void applyTransformationLeft(const geometry_utils::Transformation& T) 66 | { 67 | for(auto it : poses_) 68 | { 69 | poses_[it.first] = T * it.second; 70 | } 71 | } 72 | 73 | protected: 74 | PoseMap poses_; 75 | 76 | }; 77 | 78 | 79 | 80 | class LinearTrajectory : public Trajectory 81 | { 82 | public: 83 | 84 | LinearTrajectory() : Trajectory() {} 85 | 86 | LinearTrajectory(const PoseMap& poses) 87 | : Trajectory(poses) 88 | { 89 | CHECK_GE(poses_.size(), 2u) << "At least two poses need to be provided"; 90 | } 91 | 92 | bool getPoseAt(const ros::Time& t, geometry_utils::Transformation& T) const 93 | { 94 | ros::Time t0_, t1_; 95 | geometry_utils::Transformation T0_, T1_; 96 | 97 | // Check if it is between two known poses 98 | auto it1 = poses_.upper_bound(t); 99 | if(it1 == poses_.begin()) 100 | { 101 | LOG_FIRST_N(WARNING, 5) << "Cannot extrapolate in the past. Requested pose: " 102 | << t << " but the earliest pose available is at time: " 103 | << poses_.begin()->first; 104 | return false; 105 | } 106 | else if(it1 == poses_.end()) 107 | { 108 | LOG_FIRST_N(WARNING, 5) << "Cannot extrapolate in the future. Requested pose: " 109 | << t << " but the latest pose available is at time: " 110 | << (poses_.rbegin())->first; 111 | return false; 112 | } 113 | else 114 | { 115 | auto it0 = std::prev(it1); 116 | t0_ = (it0)->first; 117 | T0_ = (it0)->second; 118 | t1_ = (it1)->first; 119 | T1_ = (it1)->second; 120 | } 121 | 122 | // Linear interpolation in SE(3) 123 | auto T_relative = T0_.inverse() * T1_; 124 | auto delta_t = (t - t0_).toSec() / (t1_ - t0_).toSec(); 125 | T = T0_ * geometry_utils::Transformation::exp(delta_t * T_relative.log()); 126 | return true; 127 | } 128 | 129 | }; 130 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/include/mapper_emvs_stereo/utils.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * \file utils.hpp 3 | * \brief header file for utility functions like saving depth and confidence maps 4 | * \author (1) Suman Ghosh 5 | * \date 2022-09-01 6 | * \author (2) Guillermo Gallego 7 | * \date 2022-09-01 8 | * Copyright/Rights of Use: 9 | * 2022, Technische Universität Berlin 10 | * Prof. Guillermo Gallego 11 | * Robotic Interactive Perception 12 | * Marchstrasse 23, Sekr. MAR 5-5 13 | * 10587 Berlin, Germany 14 | */ 15 | 16 | #pragma once 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | void saveDepthMaps(const cv::Mat& depth_map, 23 | const cv::Mat& confidence_map, 24 | const cv::Mat& semidense_mask, 25 | const cv::Mat& depth_map_dense, 26 | const float min_depth, 27 | const float max_depth, 28 | const std::string& suffix, 29 | const std::string& out_path); 30 | 31 | void saveDepthMaps(const cv::Mat& depth_map, 32 | const cv::Mat& confidence_map, 33 | const cv::Mat& semidense_mask, 34 | const float min_depth, 35 | const float max_depth, 36 | const std::string& suffix, 37 | const std::string& out_path); 38 | 39 | // Harmonic mean of two DSIs in mapper objects 40 | void fuseDSIs_HarmonicMean(const EMVS::MapperEMVS& mapper0, 41 | const EMVS::MapperEMVS& mapper1, 42 | EMVS::MapperEMVS& mapper_fused); 43 | 44 | // Sum (e.g., arithmetic mean) of two DSIs in mapper objects 45 | void fuseDSIs_Sum(const EMVS::MapperEMVS& mapper0, 46 | const EMVS::MapperEMVS& mapper1, 47 | EMVS::MapperEMVS& mapper_fused); 48 | 49 | 50 | double computeFocus_MeanSquare(const cv::Mat& img); 51 | double computeFocus_Variance(const cv::Mat& img); 52 | 53 | 54 | void fuseDSIs_HarmonicMeanOfLocalFocus(const EMVS::MapperEMVS& mapper0, 55 | const EMVS::MapperEMVS& mapper1, 56 | const image_geometry::PinholeCameraModel& cam0, 57 | const image_geometry::PinholeCameraModel& cam1, 58 | const EMVS::ShapeDSI& dsi_shape, 59 | const int focus_method, // 0 for variance, 1 for MS 60 | EMVS::MapperEMVS& mapper_focus_fused); 61 | 62 | void accumulateEvents(const std::vector& events, 63 | const bool use_polarity, 64 | cv::Mat& img); 65 | 66 | void evaluateDepthMap(cv::Mat depth, cv::Mat gt); 67 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/launch/rpg_calib_info.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 13 | 14 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mapper_emvs_stereo 4 | 0.0.0 5 | MCEMVS (Multi-Camera Event-based Multi-View Stereo) 6 | Guillermo Gallego 7 | Guillermo Gallego 8 | 9 | see LICENSE 10 | 11 | catkin 12 | catkin_simple 13 | 14 | roscpp 15 | sensor_msgs 16 | dvs_msgs 17 | camera_info_manager 18 | image_geometry 19 | minkindr 20 | cartesian3dgrid 21 | rosbag 22 | glog_catkin 23 | gflags_catkin 24 | pcl_ros 25 | eigen_conversions 26 | tf 27 | tf_conversions 28 | vicon 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/scripts/depth_metrics.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def error_metrics(data, gt, name, b, f): 5 | data = data.flatten() 6 | gt = gt.flatten() 7 | 8 | # inlier ratios 9 | delta = np.maximum(data / gt, gt / data) 10 | delta1 = np.sum(delta < 1.25) / np.ma.count(delta) 11 | delta2 = np.sum(delta < 1.25 ** 2) / np.ma.count(delta) 12 | delta3 = np.sum(delta < 1.25 ** 3) / np.ma.count(delta) 13 | 14 | # SILog 15 | di = np.ma.log(gt) - np.ma.log(data) 16 | n = np.ma.count(di) 17 | SILog = 1 / n * np.sum(di ** 2) - 1 / (n * n) * np.sum(di) ** 2 18 | 19 | # abs rel diff error 20 | ARE = 1 / n * np.sum(np.abs(data - gt) / data) 21 | 22 | # log RMSE 23 | lRMSE = (1 / n * np.sum((np.ma.log(gt) - np.ma.log(data)) ** 2)) ** 0.5 24 | 25 | # bad-p 26 | err = np.abs(1 / data - 1 / gt) * b * f 27 | rel_err = err * gt / b / f 28 | badp = np.sum((err > 5) & (rel_err > 0.05)) / n 29 | 30 | print(name + "--------------------------------") 31 | print("delta1: ", delta1) 32 | print("delta2: ", delta2) 33 | print("delta3: ", delta3) 34 | print("SILog: ", SILog) 35 | print("Abs. Rel Error: ", ARE) 36 | print("log RMSE: ", lRMSE) 37 | print("bad-p: ", badp) 38 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/scripts/evaluate_mcemvs_dsec.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import os, sys 4 | import cv2 5 | from tqdm import tqdm 6 | from precision_completeness import precision_completeness 7 | from depth_metrics import error_metrics 8 | import yaml 9 | 10 | 11 | def load_intrinsics_yaml(prefix): 12 | with open(os.path.join(prefix, 'cam_to_cam.yaml'), "r") as stream: 13 | try: 14 | cam_cam = yaml.safe_load(stream) 15 | except yaml.YAMLError as exc: 16 | print(exc) 17 | 18 | with open(os.path.join(prefix, 'cam_to_lidar.yaml'), "r") as stream: 19 | try: 20 | cam_lidar = yaml.safe_load(stream) 21 | except yaml.YAMLError as exc: 22 | print(exc) 23 | 24 | Q = np.array(cam_cam['disparity_to_depth']['cams_03']) 25 | R_rect0 = np.array(cam_cam['extrinsics']['R_rect0']) 26 | T_rect0_0 = np.eye(4) 27 | T_rect0_0[:3, :3] = R_rect0 28 | int_cam_0 = np.array(cam_cam['intrinsics']['cam0']['camera_matrix']) 29 | K_cam_0 = np.array([int_cam_0[0], 0, int_cam_0[2], 30 | 0, int_cam_0[1], int_cam_0[3], 31 | 0, 0, 1]).reshape((3, 3)) 32 | dist_cam_0 = np.array(cam_cam['intrinsics']['cam0']['distortion_coeffs']) 33 | int_cam_3 = np.array(cam_cam['intrinsics']['cam3']['camera_matrix']) 34 | K_cam_3 = np.array([int_cam_3[0], 0, int_cam_3[2], 35 | 0, int_cam_3[1], int_cam_3[3], 36 | 0, 0, 1]).reshape((3, 3)) 37 | dist_cam_3 = np.array(cam_cam['intrinsics']['cam3']['distortion_coeffs']) 38 | return Q, T_rect0_0, K_cam_0, dist_cam_0, K_cam_3, dist_cam_3 39 | 40 | 41 | h = 480 42 | w = 640 43 | path = '/home/suman/data/rpg/DSEC/zurich_city_04_a/zurich_city_04_a_disparity_event/' 44 | flist = os.listdir(path) 45 | Q, T_rect0_0, K_cam_0, dist_cam_0, K_cam_3, dist_cam_3 = load_intrinsics_yaml(path + '../zurich_city_04_a_calibration/') 46 | K_0 = np.zeros((3, 4)) 47 | K_0[:, :3], _ = cv2.getOptimalNewCameraMatrix(K_cam_0, dist_cam_0, (640, 480), alpha=0) 48 | b = 0.6 49 | f = K_0[0, 0] 50 | dpoints_name = 'depth_points_fused_2.txt' 51 | event_start_time = 36470.599680000 52 | min_depth = 4 53 | max_depth = 50 54 | start = 0 55 | stop = 35 56 | d_path = '/home/suman/data/semvs_results/DSEC/zurich_04_a_full/test2/' 57 | d_path_mono = d_path 58 | f_ts = np.loadtxt('/home/suman/data/rpg/DSEC/zurich_city_04_a/zurich_city_04_a_disparity_timestamps.txt') 59 | f_path = '/home/suman/data/rpg/DSEC/zurich_city_04_a/zurich_city_04_a_disparity_event/' 60 | 61 | gt_depth_consolidated = [] 62 | est_depth_consolidated = [] 63 | est_depth_consolidated_mono = [] 64 | 65 | thicken_edges = False 66 | # thicken_edges = True 67 | 68 | 69 | def get_mcemvs_depth(path, time_prefix, suffix): 70 | fname = path + time_prefix + suffix 71 | depthmap = np.zeros((h, w)) + 255 72 | depth_points = np.loadtxt(fname).reshape(-1, 3) 73 | if depth_points.size != 0: 74 | depthmap[depth_points[:, 1].astype(int), depth_points[:, 0].astype(int)] = depth_points[:, 2] 75 | if thicken_edges: 76 | kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3)) 77 | depthmap = cv2.morphologyEx(depthmap, cv2.MORPH_ERODE, kernel) 78 | depthmap = np.ma.array(depthmap, mask=(depthmap == 255)) 79 | return depthmap 80 | 81 | 82 | # times = [8, 11.4, 12.5, 14.2, 31.2] 83 | invdname = 'inv_depth_colored_dilated_fused_2.png' 84 | dname = 'depth_map_fused_2.png' 85 | files = os.listdir(d_path) 86 | times = [] 87 | 88 | for file in tqdm(sorted(files)): 89 | if file.endswith(invdname): 90 | prefix = file[:file.find(invdname)] 91 | t = float(prefix) 92 | times.append(t) 93 | if tstop: 94 | continue 95 | 96 | depth_meter = get_mcemvs_depth(d_path, prefix, dpoints_name) 97 | 98 | frame_id = (np.abs(f_ts.astype(np.float64) - (t + event_start_time) * 1e6)).argmin() 99 | disp = plt.imread(f_path + str(frame_id * 2).zfill(6) + '.png') 100 | time_gt = f_ts[frame_id] 101 | dt = abs(float(time_gt) * 1e-6 - event_start_time - t) 102 | # print('Time difference between mcemvs and gt: ' + str(dt)) 103 | if dt >= 0.1: 104 | print('GT depth is too far away in time.... Skipping ...........') 105 | continue 106 | 107 | # convert disparity to depth and project to undistorted left event camera frame 108 | d = disp.astype(np.float32) * 256 109 | _3dImage = cv2.reprojectImageTo3D(d, Q) 110 | points = (_3dImage[np.where(_3dImage[:, :, 2] < np.inf)]).T 111 | P_homo = np.r_[points, np.ones((1, points.shape[1]))] 112 | P_new = np.linalg.inv(T_rect0_0) @ P_homo 113 | px = K_0 @ P_new 114 | px[0, :] /= px[2, :] 115 | px[1, :] /= px[2, :] 116 | out_d = np.zeros_like(d) 117 | try: 118 | out_d[px[1, :].astype(int), px[0, :].astype(int)] = P_new[2, :] 119 | except: 120 | print('Depth out of bounds') 121 | pass 122 | masked_gt_depth = np.ma.array(out_d, mask=(out_d < 0.05)) 123 | 124 | gt_depth_consolidated.append(masked_gt_depth) 125 | est_depth_consolidated.append(depth_meter) 126 | 127 | gt_depth_consolidated = np.ma.array(gt_depth_consolidated) 128 | est_depth_consolidated = np.ma.array(est_depth_consolidated) 129 | est_depth_consolidated_mono = np.ma.array(est_depth_consolidated_mono) 130 | 131 | scene_depth = np.max(gt_depth_consolidated[~ np.isnan(gt_depth_consolidated)]) 132 | 133 | print('----------------Stereo EMVS---------------') 134 | error = np.absolute(gt_depth_consolidated - est_depth_consolidated) 135 | # error = np.ma.array(error, mask=(error >= 10)) 136 | print("Mean " + str(np.ma.mean(error))) 137 | print("Median " + str(np.ma.median(error))) 138 | print('Max depth: ' + str(scene_depth)) 139 | print('Number of error points: ' + str(np.ma.count(error))) 140 | error_metrics(est_depth_consolidated, gt_depth_consolidated, name='Alg 1', b=b, f=f) 141 | ax1, ax2, ax3 = precision_completeness(est_depth_consolidated, gt_depth_consolidated, 'Alg 1') 142 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/scripts/mocap_txt2bag.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | import matplotlib.pyplot as plt 4 | from scipy.spatial.transform import Rotation as Rot 5 | import rospy 6 | import rosbag 7 | from geometry_msgs.msg import PoseStamped 8 | from tqdm import tqdm 9 | import argparse 10 | 11 | parser = argparse.ArgumentParser() 12 | parser.add_argument("--path_prefix", help="Folder that contains input pose") 13 | args = parser.parse_args() 14 | path_prefix = args.path_prefix 15 | # path_prefix = '/mnt/HD4TB/data/tum-vie/slide-vi_gt_data/' 16 | 17 | pose_fname = os.path.join(path_prefix, 'mocap_data.txt') 18 | pos = np.loadtxt(pose_fname, delimiter=' ', skiprows=1) 19 | 20 | bag_file_name = path_prefix + 'pose' 21 | topic = '/pose' 22 | 23 | with rosbag.Bag('{}.bag'.format(bag_file_name), 'w') as bag: 24 | for i in tqdm(range(pos.shape[0])): 25 | msg = PoseStamped() 26 | msg.pose.position.x = pos[i, 1] 27 | msg.pose.position.y = pos[i, 2] 28 | msg.pose.position.z = pos[i, 3] 29 | msg.pose.orientation.x = pos[i, 4] 30 | msg.pose.orientation.y = pos[i, 5] 31 | msg.pose.orientation.z = pos[i, 6] 32 | msg.pose.orientation.w = pos[i, 7] 33 | msg.header.stamp = rospy.Time.from_sec(pos[i, 0] / 1e6) 34 | msg.header.frame_id = 'map' 35 | bag.write(topic, msg, msg.header.stamp) 36 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/scripts/precision_completeness.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import pickle 4 | 5 | plt.rcParams.update({'font.size': 14}) 6 | 7 | 8 | def precision_completeness(data, gt, name): 9 | print('--------- Alg: ' + name + '--------------') 10 | err = np.absolute(data - gt) 11 | print('Number of gt points: ', np.ma.count(gt)) 12 | print('Number of estimated points: ', np.ma.count(data)) 13 | dashed_solid = '-' 14 | if name == '1': 15 | # name = '$H_c\circ A_t$ (Alg. 1)' 16 | name = '$H$ (Alg. 1)' 17 | elif name == 'min': 18 | name = '${min}$' 19 | elif name == 'Gc': 20 | name = '${G}$' 21 | elif name == 'Ac': 22 | # name = '${A}_c\circ A_t$' 23 | name = '${A}$' 24 | # color = 'r' 25 | # dashed_solid = 'C3' 26 | elif name == 'rms': 27 | name = '${RMS}$' 28 | elif name == 'max': 29 | name = '${max}$' 30 | elif name == 'HcHt': 31 | name = '${H}_c\circ H_t$' 32 | elif name == '2': 33 | name = '$A_t\circ H_c$' 34 | # dashed_solid = '--' 35 | elif name == 'SGM': 36 | dashed_solid = 'C4' 37 | elif name == 'GTS': 38 | dashed_solid = 'C5' 39 | 40 | # precision 41 | ax1 = plt.figure('p') 42 | binwidth = 0.01 43 | values, base = np.histogram(err.compressed(), bins=int(np.amax(err) / binwidth)) 44 | cumulative = np.cumsum(values) / np.ma.count(data) * 100 45 | plt.plot(base[:-1], cumulative, dashed_solid, label=name) 46 | plt.legend(prop={'size': 18}, frameon=False, loc=0) 47 | plt.title('Precision (%)') 48 | plt.xlabel('Error (m)') 49 | plt.xlim([0, .15]) 50 | plt.ylim([0, None]) 51 | # plt.ylim([0, 85]) 52 | # plt.gca().set_aspect(0.00583) 53 | plt.show() 54 | precision = cumulative 55 | 56 | # completeness 57 | ax2 = plt.figure('c') 58 | binwidth = 0.01 59 | values, base = np.histogram(err.compressed(), bins=int(np.amax(err) / binwidth)) 60 | cumulative = np.cumsum(values) / np.ma.count(gt) * 100 61 | plt.plot(base[:-1], cumulative, dashed_solid, label=name) 62 | plt.legend(prop={'size': 18}, frameon=False, loc=0) 63 | plt.title('Recall (%)') 64 | plt.xlabel('Error (m)') 65 | plt.xlim([0, .15]) 66 | plt.ylim([0, None]) 67 | # plt.ylim([0, 1.5]) 68 | plt.show() 69 | # plt.gca().set_aspect(0.1) 70 | recall = cumulative 71 | 72 | # F1 73 | ax3 = plt.figure('f') 74 | binwidth = 0.01 75 | values, base = np.histogram(err.compressed(), bins=int(np.amax(err) / binwidth)) 76 | f1 = 2 * precision * recall / (precision + recall) 77 | plt.plot(base[:-1], f1, dashed_solid, label=name) 78 | plt.legend(prop={'size': 18}, frameon=False, loc=0) 79 | plt.title('F1-score (%)') 80 | plt.xlabel('Error (m)') 81 | plt.xlim([0, .15]) 82 | plt.ylim([0, None]) 83 | # plt.ylim([0, 1.5]) 84 | # plt.rcParams.update({'font.size': 14}) 85 | plt.show() 86 | # plt.gca().set_aspect(0.1) 87 | 88 | # outliers 89 | ax1 = plt.figure('o') 90 | binwidth = 0.01 91 | values, base = np.histogram(err.compressed(), bins=int(np.amax(err) / binwidth)) 92 | cumulative = (np.ma.count(err.compressed()) - np.cumsum(values)) / np.ma.count(err.compressed()) * 100 93 | plt.plot(base[:-1], cumulative, dashed_solid, label=name) 94 | plt.legend(prop={'size': 18}, frameon=False, loc=0) 95 | plt.title('% of depth points beyond error threshold') 96 | plt.xlabel('Error (m)') 97 | plt.xlim([0, None]) 98 | plt.ylim([0, None]) 99 | # plt.ylim([0, 85]) 100 | # plt.gca().set_aspect(0.00583) 101 | plt.show() 102 | 103 | return ax1, ax2, ax3 104 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/scripts/visualize_dsi_slices.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import argparse 3 | import visvis as vv 4 | app = vv.use() 5 | 6 | if __name__ == "__main__": 7 | 8 | parser = argparse.ArgumentParser(description='Plot the disparity space image (DSI) using 3D slices') 9 | parser.add_argument('-i', '--input', default='dsi.npy', type=str, 10 | help='path to the NPY file containing the DSI (default: dsi.npy)') 11 | args = parser.parse_args() 12 | 13 | a = vv.gca() 14 | a.daspect = 1, -1, 1 15 | a.daspectAuto = False 16 | vol = np.load(args.input) 17 | 18 | # Reorder axis so that the Z axis points forward instead of up 19 | vol = np.swapaxes(vol, 0, 1) 20 | vol = np.flip(vol, axis=0) 21 | 22 | t = vv.volshow2(vol) 23 | t.colormap = vv.CM_HOT 24 | 25 | app.Run() 26 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/scripts/visualize_dsi_volume.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import argparse 3 | import visvis as vv 4 | app = vv.use() 5 | 6 | if __name__ == "__main__": 7 | 8 | parser = argparse.ArgumentParser(description='Plot the disparity space image (DSI) using 3D slices') 9 | parser.add_argument('-i', '--input', default='dsi.npy', type=str, 10 | help='path to the NPY file containing the DSI (default: dsi.npy)') 11 | args = parser.parse_args() 12 | 13 | a = vv.gca() 14 | a.daspect = 1, -1, 1 15 | a.daspectAuto = True 16 | vol = np.load(args.input) 17 | 18 | # Reorder axis so that the Z axis points forward instead of up 19 | vol = np.swapaxes(vol, 0, 1) 20 | vol = np.flip(vol, axis=0) 21 | 22 | t = vv.volshow(vol, renderStyle = 'mip') 23 | t.colormap = vv.CM_HOT 24 | 25 | app.Run() 26 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/scripts/visualize_pointcloud.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Load a PCD point cloud and visualize as a 3D scatter plot. 4 | 5 | To run, install [pypcd](https://github.com/dimatura/pypcd) as follows: 6 | 7 | pip install pypcd 8 | 9 | """ 10 | from mpl_toolkits.mplot3d import Axes3D # noqa: F401 unused import 11 | import matplotlib.pyplot as plt 12 | import numpy as np 13 | import argparse 14 | import pypcd 15 | 16 | 17 | def set_aspect_ratio_equal(Xr, Yr, Zr, ax): 18 | """ 19 | Set the apect ratio to 'equal' 20 | Code borrowed from: https://stackoverflow.com/a/21765085 21 | """ 22 | max_range = np.array([Xr.max() - Xr.min(), 23 | Yr.max() - Yr.min(), 24 | Zr.max() - Zr.min()]).max() / 2.0 25 | mid_x = (Xr.max() + Xr.min()) * 0.5 26 | mid_y = (Yr.max() + Yr.min()) * 0.5 27 | mid_z = (Zr.max() + Zr.min()) * 0.5 28 | ax.set_xlim(mid_x - max_range, mid_x + max_range) 29 | ax.set_ylim(mid_y - max_range, mid_y + max_range) 30 | ax.set_zlim(mid_z - max_range, mid_z + max_range) 31 | 32 | 33 | if __name__ == "__main__": 34 | 35 | parser = argparse.ArgumentParser(description='Plot a reconstructed point cloud in 3D') 36 | parser.add_argument('-i', '--input', default='pointcloud.pcd', type=str, 37 | help='path to the PCD file (default: pointcloud.pcd)') 38 | args = parser.parse_args() 39 | 40 | 41 | pc = pypcd.PointCloud.from_path(args.input) 42 | X, Y, Z, I = pc.pc_data['x'], pc.pc_data['y'], pc.pc_data['z'], pc.pc_data['intensity'] 43 | 44 | fig = plt.figure() 45 | ax = fig.add_subplot(111, projection='3d') 46 | ax.set_aspect('equal') 47 | 48 | # Rename the axis so that the Zr axis coincides with -X 49 | Xr, Yr, Zr = Z, -X, -Y 50 | 51 | # Plot point cloud 52 | ax.scatter(Xr, Yr, Zr, c=I, linewidths=0) 53 | ax.view_init(elev=30, azim=-150) 54 | ax.set_xlabel('Z') 55 | ax.set_ylabel('-X') 56 | ax.set_zlabel('-Y') 57 | set_aspect_ratio_equal(Xr, Yr, Zr, ax) 58 | 59 | plt.show() -------------------------------------------------------------------------------- /mapper_emvs_stereo/src/median_filtering.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #define FORWARD 1 5 | #define BACKWARD (-1) 6 | 7 | int compute_median_histogram(const int h[], int num_elements) 8 | { 9 | int middle = (num_elements+1)/2; 10 | int m, n; 11 | for(m=0,n=0; n < 256; n++) 12 | { 13 | m += h[n]; 14 | if(m >= middle) 15 | break; 16 | } 17 | return n; 18 | } 19 | 20 | 21 | inline int get_value(const cv::Mat& img, const cv::Mat& mask, int row, int col) 22 | { 23 | if(row >= 0 && col >= 0 && row < img.rows && col < img.cols && mask.at(row,col) > 0) 24 | return img.at(row,col); 25 | else 26 | return -1; 27 | } 28 | 29 | 30 | // 2D median filter. Complexity: O(p) where p is the patch size 31 | // "T. Huang, G. Yang, and G. Tang, "A Fast Two-Dimensional Median Filtering Algorithm" 32 | // Code adapted from: http://www.sergejusz.com/engineering_tips/median_filter.htm 33 | void huangMedianFilter(const cv::Mat& img, cv::Mat& out_img, const cv::Mat& mask, int patch_size) 34 | { 35 | CHECK(img.data); 36 | CHECK(mask.data); 37 | CHECK_GT(img.rows, 0); 38 | CHECK_GT(img.cols, 0); 39 | CHECK_EQ(img.rows, mask.rows); 40 | CHECK_EQ(img.cols, mask.cols); 41 | CHECK_EQ(img.type(), CV_8U); 42 | CHECK_EQ(patch_size % 2, 1); 43 | 44 | out_img = img.clone(); 45 | 46 | int p = patch_size / 2; 47 | 48 | int med; 49 | int prev, next; 50 | int h[256]; 51 | int direction = FORWARD; 52 | int row1, row2, col1, col2; 53 | int row, col, r, c; 54 | 55 | memset(h, 0, sizeof(h)); 56 | 57 | int num_elements = 0; 58 | 59 | // Histogram For (0,0)-element 60 | for(row = -p; row <= p; row++) 61 | { 62 | for(col = -p; col <= p; col++) 63 | { 64 | int val = get_value(img, mask, row, col); 65 | if(val >= 0) 66 | { 67 | h[val]++; 68 | num_elements++; 69 | } 70 | } 71 | } 72 | 73 | // Median 74 | med = compute_median_histogram(h, num_elements); 75 | 76 | // Now, Median Is Defined For (0,0)-element 77 | // Begin Scanning: direction - FORWARD 78 | out_img.at(0,0)=med; 79 | 80 | // main loop 81 | for(col=1, row=0; row=0 && col= 0 && value_out == value_in) 103 | continue; 104 | if(value_out >= 0) 105 | { 106 | h[value_out]--; 107 | num_elements--; 108 | } 109 | if(value_in >= 0) 110 | { 111 | h[value_in]++; 112 | num_elements++; 113 | } 114 | } 115 | 116 | // Update new median 117 | med = compute_median_histogram(h, num_elements); 118 | out_img.at(row,col) = med; 119 | } // end of column loop 120 | 121 | if(row == img.rows-1) 122 | return; 123 | 124 | // go back to the last/first pixel of the line 125 | col -= direction; 126 | // change direction to the opposite 127 | direction *= -1; 128 | 129 | // Shift Down One Line 130 | prev = row1; 131 | next = row2+1; 132 | 133 | col1 = col - p; 134 | col2 = col + p; 135 | 136 | for(c=col1; c<=col2; c++) 137 | { 138 | int value_out = get_value(img, mask, prev, c); 139 | int value_in = get_value(img, mask, next, c); 140 | if(value_in >= 0 && value_out == value_in) 141 | continue; 142 | if(value_out >= 0) 143 | { 144 | h[value_out]--; 145 | num_elements--; 146 | } 147 | if(value_in >= 0) 148 | { 149 | h[value_in]++; 150 | num_elements++; 151 | } 152 | } 153 | 154 | med = compute_median_histogram(h, num_elements); 155 | out_img.at(row+1,col) = med; 156 | col += direction; 157 | } 158 | } 159 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/src/process1.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * \file process1.cpp 3 | * \brief process events using Alg 1 4 | * \author (1) Suman Ghosh 5 | * \date 2022-09-01 6 | * \author (2) Guillermo Gallego 7 | * \date 2022-09-01 8 | * Copyright/Rights of Use: 9 | * 2022, Technische Universität Berlin 10 | * Prof. Guillermo Gallego 11 | * Robotic Interactive Perception 12 | * Marchstrasse 23, Sekr. MAR 5-5 13 | * 10587 Berlin, Germany 14 | */ 15 | 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | //#define EVIMO2 24 | //#define TIMING_LOOP 25 | 26 | 27 | // Alg 1: fuse DSI across cameras 28 | void process_1( 29 | const image_geometry::PinholeCameraModel& cam0, 30 | const image_geometry::PinholeCameraModel& cam1, 31 | const image_geometry::PinholeCameraModel& cam2, 32 | const LinearTrajectory& trajectory0, 33 | const LinearTrajectory& trajectory1, 34 | const LinearTrajectory& trajectory2, 35 | const std::vector& events0, 36 | const std::vector& events1, 37 | const std::vector& events2, 38 | const EMVS::OptionsDepthMap& opts_depth_map, 39 | const EMVS::ShapeDSI& dsi_shape, 40 | EMVS::MapperEMVS& mapper_fused, 41 | EMVS::MapperEMVS& mapper0, 42 | EMVS::MapperEMVS& mapper1, 43 | EMVS::MapperEMVS& mapper2, 44 | const std::string& out_path, 45 | double ts, 46 | int fusion_method 47 | ) 48 | { 49 | #ifdef TIMING_LOOP 50 | int nloops = 100; 51 | #endif 52 | double t_mid; 53 | 54 | // 1. Back-project events into the DSI 55 | { 56 | geometry_utils::Transformation T_rv_w; 57 | t_mid = ts; 58 | LOG(INFO) << "Setting DSI reference at timestamp: " << t_mid; 59 | 60 | geometry_utils::Transformation T_w_rv, T_w_l, T_w_r; 61 | trajectory0.getPoseAt(ros::Time(t_mid), T_w_l); 62 | trajectory1.getPoseAt(ros::Time(t_mid), T_w_r); 63 | //Put camera somewhere along the baseline between 2 cameras 64 | Eigen::Matrix4d baseline = Eigen::Matrix4d::Identity(4,4); 65 | baseline(0, 3) = opts_depth_map.rv_pos; 66 | geometry_utils::Transformation baselineTransform(baseline); 67 | T_w_rv = T_w_l * baselineTransform; 68 | T_rv_w = T_w_rv.inverse(); 69 | 70 | // Left camera: back-project events into the DSI 71 | LOG(INFO) << "Computing DSI for first camera"; 72 | std::chrono::high_resolution_clock::time_point t_start_dsi = std::chrono::high_resolution_clock::now(); 73 | #ifdef TIMING_LOOP 74 | for(int i=1; i<=nloops; i++){ 75 | #endif 76 | mapper0.evaluateDSI(events0, trajectory0, T_rv_w); 77 | #ifdef TIMING_LOOP 78 | } 79 | #endif 80 | std::chrono::high_resolution_clock::time_point t_end_dsi = std::chrono::high_resolution_clock::now(); 81 | auto duration_dsi = std::chrono::duration_cast(t_end_dsi - t_start_dsi ).count(); 82 | 83 | LOG(INFO) << "Time to evaluate DSI: " << duration_dsi << " milliseconds"; 84 | LOG(INFO) << "Number of events processed: " << events0.size() << " events"; 85 | LOG(INFO) << "Number of events processed per second: " << static_cast(events0.size()) / (1000.f * static_cast(duration_dsi)) << " Mev/s"; 86 | LOG(INFO) << "Mean square = " << mapper0.dsi_.computeMeanSquare(); 87 | 88 | // Right camera: back-project events into the DSI 89 | LOG(INFO) << "Computing DSI for second camera"; 90 | t_start_dsi = std::chrono::high_resolution_clock::now(); 91 | #ifdef TIMING_LOOP 92 | for (int i=1; i<=nloops; i++){ 93 | #endif 94 | mapper1.evaluateDSI(events1, trajectory1, T_rv_w); 95 | #ifdef TIMING_LOOP 96 | } 97 | #endif 98 | t_end_dsi = std::chrono::high_resolution_clock::now(); 99 | duration_dsi = std::chrono::duration_cast(t_end_dsi - t_start_dsi ).count(); 100 | LOG(INFO) << "Time to evaluate DSI: " << duration_dsi << " milliseconds"; 101 | LOG(INFO) << "Number of events processed: " << events1.size() << " events"; 102 | LOG(INFO) << "Number of events processed per second: " << static_cast(events1.size()) / (1000.f * static_cast(duration_dsi)) << " Mev/s"; 103 | LOG(INFO) << "Mean square = " << mapper1.dsi_.computeMeanSquare(); 104 | 105 | if (events2.size()>0){ 106 | 107 | // 3rd camera: back-project events into the DSI 108 | LOG(INFO) << "Computing DSI for third camera"; 109 | t_start_dsi = std::chrono::high_resolution_clock::now(); 110 | mapper2.evaluateDSI(events2, trajectory2, T_rv_w); 111 | t_end_dsi = std::chrono::high_resolution_clock::now(); 112 | duration_dsi = std::chrono::duration_cast(t_end_dsi - t_start_dsi ).count(); 113 | LOG(INFO) << "Time to evaluate DSI: " << duration_dsi << " milliseconds"; 114 | LOG(INFO) << "Number of events processed: " << events2.size() << " events"; 115 | LOG(INFO) << "Number of events processed per second: " << static_cast(events2.size()) / (1000.f * static_cast(duration_dsi)) << " Mev/s"; 116 | LOG(INFO) << "Mean square = " << mapper2.dsi_.computeMeanSquare(); 117 | } 118 | } 119 | 120 | // set up prefix including output path 121 | std::stringstream ss; 122 | ss << out_path << std::fixed << std::setprecision(9) << std::setfill('0') << std::setw(13) << t_mid; 123 | 124 | // 2. Fuse the DSIs 125 | { 126 | mapper_fused.dsi_.resetGrid(); 127 | mapper_fused.dsi_.addTwoGrids(mapper0.dsi_); // Initialize DSI 128 | 129 | // Sum of the two DSIs 130 | //mapper_fused.dsi_.addTwoGrids(mapper1.dsi_); 131 | 132 | std::chrono::high_resolution_clock::time_point t_start_fusion = std::chrono::high_resolution_clock::now(); 133 | #ifdef TIMING_LOOP 134 | for (int i=1; i<=nloops; i++){ 135 | #endif 136 | switch(fusion_method){ 137 | case 1: 138 | mapper_fused.dsi_.minTwoGrids(mapper1.dsi_); 139 | break; 140 | case 2: 141 | mapper_fused.dsi_.harmonicMeanTwoGrids(mapper1.dsi_); 142 | break; 143 | case 3: 144 | mapper_fused.dsi_.geometricMeanTwoGrids(mapper1.dsi_); 145 | break; 146 | case 4: 147 | mapper_fused.dsi_.arithmeticMeanTwoGrids(mapper1.dsi_); 148 | break; 149 | case 5: 150 | mapper_fused.dsi_.rmsTwoGrids(mapper1.dsi_); 151 | break; 152 | case 6: 153 | mapper_fused.dsi_.maxTwoGrids(mapper1.dsi_); 154 | break; 155 | default: 156 | LOG(INFO) << "Improper fusion method selected"; 157 | return; 158 | } 159 | #ifdef TIMING_LOOP 160 | } 161 | #endif 162 | 163 | 164 | std::chrono::high_resolution_clock::time_point t_end_fusion = std::chrono::high_resolution_clock::now(); 165 | auto t_fusion = std::chrono::duration_cast(t_end_fusion - t_start_fusion ).count(); 166 | LOG(INFO) << "Time to fuse DSIs: "< 0){ 170 | LOG(INFO) << "Fusing 3rd DSI"; 171 | switch(fusion_method){ 172 | case 1: 173 | mapper_fused.dsi_.minTwoGrids(mapper2.dsi_); 174 | break; 175 | case 2: 176 | mapper_fused.dsi_.harmonicMeanTwoGrids(mapper2.dsi_, 3); 177 | break; 178 | case 3: 179 | break; 180 | case 4: 181 | break; 182 | case 5: 183 | break; 184 | case 6: 185 | mapper_fused.dsi_.maxTwoGrids(mapper2.dsi_); 186 | break; 187 | default: 188 | LOG(INFO) << "Improper fusion method selected"; 189 | return; 190 | } 191 | } 192 | 193 | // Write the DSI (3D voxel grid) to disk 194 | // mapper0.dsi_.writeGridNpy((out_path+std::to_string(t_mid)+"dsi_0.npy").c_str()); 195 | // mapper1.dsi_.writeGridNpy((out_path+std::to_string(t_mid)+"dsi_1.npy").c_str()); 196 | // if (events2.size()>0) 197 | // mapper2.dsi_.writeGridNpy((out_path+std::to_string(t_mid)+"dsi_2.npy").c_str()); 198 | // mapper_fused.dsi_.writeGridNpy((out_path+std::to_string(t_mid)+"dsi_fused.npy").c_str()); 199 | } 200 | 201 | // 3. Extract semi-dense depth map from DSI 202 | cv::Mat depth_map, confidence_map, semidense_mask; 203 | 204 | if (opts_depth_map.save_mono){ 205 | // One DSI (voted by left-camera events) 206 | mapper0.getDepthMapFromDSI(depth_map, confidence_map, semidense_mask, opts_depth_map); 207 | saveDepthMaps(depth_map, confidence_map, semidense_mask, dsi_shape.min_depth_, dsi_shape.max_depth_, std::string("0"), ss.str()); 208 | // Another DSI (voted by right-camera events) 209 | mapper1.getDepthMapFromDSI(depth_map, confidence_map, semidense_mask, opts_depth_map); 210 | saveDepthMaps(depth_map, confidence_map, semidense_mask, dsi_shape.min_depth_, dsi_shape.max_depth_, std::string("1"), ss.str()); 211 | 212 | if(events2.size()>0){ 213 | // Another DSI (voted by 3rd camera events) 214 | mapper2.getDepthMapFromDSI(depth_map, confidence_map, semidense_mask, opts_depth_map); 215 | saveDepthMaps(depth_map, confidence_map, semidense_mask, dsi_shape.min_depth_, dsi_shape.max_depth_, std::string("2"), ss.str()); 216 | } 217 | } 218 | 219 | // Fused DSIs 220 | mapper_fused.getDepthMapFromDSI(depth_map, confidence_map, semidense_mask, opts_depth_map); 221 | saveDepthMaps(depth_map, confidence_map, semidense_mask, dsi_shape.min_depth_, dsi_shape.max_depth_, std::string("fused_" + std::to_string(fusion_method)), ss.str()); 222 | } 223 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/src/process2.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * \file process2.cpp 3 | * \brief process events using Alg 2 4 | * \author (1) Suman Ghosh 5 | * \date 2022-09-01 6 | * \author (2) Guillermo Gallego 7 | * \date 2022-09-01 8 | * Copyright/Rights of Use: 9 | * 2022, Technische Universität Berlin 10 | * Prof. Guillermo Gallego 11 | * Robotic Interactive Perception 12 | * Marchstrasse 23, Sekr. MAR 5-5 13 | * 10587 Berlin, Germany 14 | */ 15 | 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | //#define TIMING_LOOP 24 | 25 | // Alg 2: Fusion across cameras and time 26 | // Divide event stream into sub-intervals and generate DSIs. Fuse across cameras, then across time. 27 | // Finally, also do the converse: first fuse across time, then across cameras. 28 | void process_2(const image_geometry::PinholeCameraModel& cam0, 29 | const image_geometry::PinholeCameraModel& cam1, 30 | const LinearTrajectory& trajectory0, 31 | const LinearTrajectory& trajectory1, 32 | const std::vector& events0, 33 | const std::vector& events1, 34 | const EMVS::OptionsDepthMap& opts_depth_map, 35 | const EMVS::ShapeDSI& dsi_shape, 36 | const int num_subintervals, 37 | EMVS::MapperEMVS& mapper_fused, 38 | EMVS::MapperEMVS& mapper_fused_camera_time, 39 | const std::string& out_path, 40 | double ts, 41 | int stereo_fusion, 42 | int temporal_fusion 43 | ) 44 | { 45 | // Initialize mappers 46 | const unsigned int num_events_per_subinterval[2] = {static_cast(events0.size()) / num_subintervals, 47 | static_cast(events1.size()) / num_subintervals}; 48 | 49 | // Initialize mappers 50 | EMVS::MapperEMVS mapper0(cam0, dsi_shape); 51 | EMVS::MapperEMVS mapper1(cam1, dsi_shape); 52 | EMVS::MapperEMVS mapper_fused_subinterval(cam0, dsi_shape); 53 | EMVS::MapperEMVS mapper_fused_left(cam0, dsi_shape); 54 | EMVS::MapperEMVS mapper_fused_right(cam0, dsi_shape); 55 | 56 | geometry_utils::Transformation T_rv_w; 57 | double t_mid; 58 | 59 | // Set the location of the DSI (reference view) in the middle of the trajectory of the left camera 60 | // if(ts <0) 61 | // { 62 | // geometry_utils::Transformation T0, T1; 63 | // ros::Time t0, t1; 64 | // trajectory0.getFirstControlPose(&T0, &t0); 65 | // trajectory0.getLastControlPose(&T1, &t1); 66 | // t_mid = 0.5 * (t0.toSec() + t1.toSec()); 67 | // LOG(INFO) << "Setting DSI reference at middle of left camera trajectory: " << t_mid; 68 | // } // Set DSI reference at provided timestamp ts 69 | // else 70 | // { 71 | t_mid = ts; 72 | LOG(INFO) << "Setting DSI reference at specific timestamp: " << t_mid; 73 | // } 74 | 75 | // set up prefix including output path 76 | std::stringstream ss; 77 | ss << out_path << std::fixed << std::setprecision(9) << std::setfill('0') << std::setw(13) << t_mid; 78 | 79 | geometry_utils::Transformation T_w_rv; 80 | trajectory0.getPoseAt(ros::Time(t_mid), T_w_rv); 81 | T_rv_w = T_w_rv.inverse(); 82 | 83 | cv::Mat depth_map, confidence_map, semidense_mask; 84 | 85 | LOG(INFO) << "Computing DSI for the cameras"; 86 | LOG(INFO) << "Left : Number of events per sub-interval = " << num_events_per_subinterval[0] << " events"; 87 | LOG(INFO) << "Right: Number of events per sub-interval = " << num_events_per_subinterval[1] << " events"; 88 | 89 | // Fuse the DSIs 90 | mapper_fused.dsi_.resetGrid(); 91 | unsigned int idx_first_ev[2] = {0, 0}; 92 | #ifdef TIMING_LOOP 93 | int nloops = 100; 94 | #endif 95 | std::chrono::high_resolution_clock::time_point t_start, t_end = std::chrono::high_resolution_clock::now(); 96 | auto fusion_duration = std::chrono::duration_cast(t_end - t_end).count(); 97 | 98 | for (unsigned int k=0; k < num_subintervals; k++) 99 | { 100 | mapper0.dsi_.resetGrid(); 101 | mapper1.dsi_.resetGrid(); 102 | mapper_fused_subinterval.dsi_.resetGrid(); 103 | {// Left camera: 104 | // Approximation: this selection does not guarantee that events belong to the same time interval on both cameras 105 | std::vector events_subset = std::vector(events0.begin() + idx_first_ev[0], 106 | events0.begin() + idx_first_ev[0] + num_events_per_subinterval[0]); 107 | idx_first_ev[0] += num_events_per_subinterval[0]; 108 | 109 | {// Display accumulted events to visualize the amount of intra-camera shift/baseline 110 | cv::Size full_resolution = cam0.fullResolution(); 111 | cv::Mat event_image = cv::Mat(full_resolution,CV_8UC1); 112 | accumulateEvents(events_subset,true,event_image); 113 | std::stringstream filename; 114 | filename << "events_0_" << std::setfill('0') << std::setw(3) << k << ".png"; 115 | cv::imwrite(out_path+filename.str(),event_image); 116 | } 117 | 118 | // Back-project events into the DSI 119 | mapper0.evaluateDSI(events_subset, trajectory0, T_rv_w); 120 | LOG(INFO) << "Left : Sub-interval " << k << " Mean square = " << mapper0.dsi_.computeMeanSquare(); 121 | 122 | if (!opts_depth_map.full_sequence) { 123 | mapper0.getDepthMapFromDSI(depth_map, confidence_map, semidense_mask, opts_depth_map); 124 | std::stringstream ss_suffix; 125 | ss_suffix << "0_" << std::setfill('0') << std::setw(3) << k; 126 | saveDepthMaps(depth_map, confidence_map, semidense_mask, dsi_shape.min_depth_, dsi_shape.max_depth_, ss_suffix.str(), ss.str()); 127 | } 128 | } 129 | 130 | {// Right camera: 131 | // Approximation: this selection does not guarantee that events belong to the same time interval on both cameras 132 | std::vector events_subset = std::vector(events1.begin() + idx_first_ev[1], 133 | events1.begin() + idx_first_ev[1] + num_events_per_subinterval[1]); 134 | idx_first_ev[1] += num_events_per_subinterval[1]; 135 | 136 | {// Display accumulted events to visualize the amount of intra-camera shift/baseline 137 | cv::Size full_resolution = cam1.fullResolution(); 138 | cv::Mat event_image = cv::Mat(full_resolution,CV_8UC1); 139 | accumulateEvents(events_subset,true,event_image); 140 | std::stringstream filename; 141 | filename << "events_1_" << std::setfill('0') << std::setw(3) << k << ".png"; 142 | cv::imwrite(out_path+filename.str(),event_image); 143 | } 144 | 145 | // Back-project events into the DSI 146 | mapper1.evaluateDSI(events_subset, trajectory1, T_rv_w); 147 | LOG(INFO) << "Right: Sub-interval " << k << " Mean square = " << mapper1.dsi_.computeMeanSquare(); 148 | 149 | if (!opts_depth_map.full_sequence) { 150 | mapper1.getDepthMapFromDSI(depth_map, confidence_map, semidense_mask, opts_depth_map); 151 | std::stringstream ss_suffix; 152 | ss_suffix << "1_" << std::setfill('0') << std::setw(3) << k; 153 | saveDepthMaps(depth_map, confidence_map, semidense_mask, dsi_shape.min_depth_, dsi_shape.max_depth_, ss_suffix.str(), ss.str()); 154 | } 155 | 156 | } 157 | 158 | // Initialize stereo mapper for a single subinterval 159 | mapper_fused_subinterval.dsi_.resetGrid(); 160 | mapper_fused_subinterval.dsi_.addTwoGrids(mapper0.dsi_); 161 | 162 | // Fused DSI across cameras for a single sub-interval 163 | t_start = std::chrono::high_resolution_clock::now(); 164 | #ifdef TIMING_LOOP 165 | for (int i=1; i<=nloops; i++){ 166 | #endif 167 | switch(stereo_fusion){ 168 | case 1: 169 | mapper_fused_subinterval.dsi_.minTwoGrids(mapper1.dsi_); 170 | break; 171 | case 2: 172 | mapper_fused_subinterval.dsi_.harmonicMeanTwoGrids(mapper1.dsi_); 173 | break; 174 | case 3: 175 | mapper_fused_subinterval.dsi_.geometricMeanTwoGrids(mapper1.dsi_); 176 | break; 177 | case 4: 178 | mapper_fused_subinterval.dsi_.arithmeticMeanTwoGrids(mapper1.dsi_); 179 | break; 180 | case 5: 181 | mapper_fused_subinterval.dsi_.rmsTwoGrids(mapper1.dsi_); 182 | break; 183 | case 6: 184 | mapper_fused_subinterval.dsi_.maxTwoGrids(mapper1.dsi_); 185 | break; 186 | default: 187 | LOG(INFO) << "Improper fusion method selected"; 188 | return; 189 | } 190 | #ifdef TIMING_LOOP 191 | } 192 | #endif 193 | t_end = std::chrono::high_resolution_clock::now(); 194 | fusion_duration+=std::chrono::duration_cast(t_end-t_start).count(); 195 | 196 | if (!opts_depth_map.full_sequence) { 197 | // Save depth map from stereo DSI for a single interval 198 | mapper_fused_subinterval.getDepthMapFromDSI(depth_map, confidence_map, semidense_mask, opts_depth_map); 199 | { 200 | std::stringstream ss_suffix; 201 | ss_suffix << "fused_" << std::setfill('0') << std::setw(3) << k; 202 | saveDepthMaps(depth_map, confidence_map, semidense_mask, dsi_shape.min_depth_, dsi_shape.max_depth_, ss_suffix.str(), ss.str()); 203 | } 204 | } 205 | 206 | // Fuse DSIs across time 207 | t_start = std::chrono::high_resolution_clock::now(); 208 | #ifdef TIMING_LOOP 209 | for (int i=1; i<=nloops; i++){ 210 | #endif 211 | switch (temporal_fusion){ 212 | case 1: 213 | break; 214 | case 2: 215 | // HM 216 | mapper_fused_left.dsi_.addInverseOfTwoGrids(mapper0.dsi_); 217 | mapper_fused_right.dsi_.addInverseOfTwoGrids(mapper1.dsi_); 218 | mapper_fused.dsi_.addInverseOfTwoGrids(mapper_fused_subinterval.dsi_); 219 | if (k==num_subintervals-1){ 220 | mapper_fused_left.dsi_.computeHMfromSumOfInv(num_subintervals); 221 | mapper_fused_right.dsi_.computeHMfromSumOfInv(num_subintervals); 222 | mapper_fused.dsi_.computeHMfromSumOfInv(num_subintervals); 223 | } 224 | break; 225 | case 3: 226 | break; 227 | case 4: 228 | // AM 229 | mapper_fused_left.dsi_.addTwoGrids(mapper0.dsi_); 230 | mapper_fused_right.dsi_.addTwoGrids(mapper1.dsi_); 231 | mapper_fused.dsi_.addTwoGrids(mapper_fused_subinterval.dsi_); 232 | if (k==num_subintervals-1){ 233 | mapper_fused_left.dsi_.computeAMfromSum(num_subintervals); 234 | mapper_fused_right.dsi_.computeAMfromSum(num_subintervals); 235 | mapper_fused.dsi_.computeAMfromSum(num_subintervals); 236 | } 237 | break; 238 | case 5: 239 | break; 240 | case 6: 241 | break; 242 | } 243 | #ifdef TIMING_LOOP 244 | } 245 | #endif 246 | t_end = std::chrono::high_resolution_clock::now(); 247 | fusion_duration+= (std::chrono::duration_cast(t_end - t_start).count())/3; 248 | 249 | } 250 | 251 | LOG(INFO) << "Time taken to fuse across space and time "<< fusion_duration <<" ms"; 252 | LOG(INFO) << "Mean square = " << mapper_fused.dsi_.computeMeanSquare(); 253 | 254 | if (!opts_depth_map.full_sequence) { 255 | // Write the DSI (3D voxel grid) to disk 256 | mapper_fused_left.dsi_.writeGridNpy(std::string(out_path + "dsi_fused_0_temporalfusion.npy").c_str()); 257 | mapper_fused_right.dsi_.writeGridNpy(std::string(out_path + "dsi_fused_1_temporalfusion.npy").c_str()); 258 | mapper_fused.dsi_.writeGridNpy(std::string(out_path + "dsi_stereo_temporalfusion.npy").c_str()); 259 | } 260 | 261 | // Fused DSIs (using harmonic mean). 262 | if (!opts_depth_map.full_sequence) { 263 | mapper_fused_left.getDepthMapFromDSI(depth_map, confidence_map, semidense_mask, opts_depth_map); 264 | saveDepthMaps(depth_map, confidence_map, semidense_mask, dsi_shape.min_depth_, dsi_shape.max_depth_, std::string("left_temporal_" + std::to_string(temporal_fusion)), ss.str()); 265 | mapper_fused_right.getDepthMapFromDSI(depth_map, confidence_map, semidense_mask, opts_depth_map); 266 | saveDepthMaps(depth_map, confidence_map, semidense_mask, dsi_shape.min_depth_, dsi_shape.max_depth_, std::string("right_temporal_" + std::to_string(temporal_fusion)), ss.str()); 267 | } 268 | mapper_fused.getDepthMapFromDSI(depth_map, confidence_map, semidense_mask, opts_depth_map); 269 | saveDepthMaps(depth_map, confidence_map, semidense_mask, dsi_shape.min_depth_, dsi_shape.max_depth_, std::string("stereo_temporal_" + std::to_string(temporal_fusion)), ss.str()); 270 | 271 | // Now fusing all sub-intervals along time first, then across cameras (converse of above process) 272 | mapper_fused_camera_time.dsi_.addTwoGrids(mapper_fused_left.dsi_); 273 | switch(stereo_fusion){ 274 | case 1: 275 | mapper_fused_camera_time.dsi_.minTwoGrids(mapper_fused_right.dsi_); 276 | break; 277 | case 2: 278 | mapper_fused_camera_time.dsi_.harmonicMeanTwoGrids(mapper_fused_right.dsi_); 279 | break; 280 | case 3: 281 | mapper_fused_camera_time.dsi_.arithmeticMeanTwoGrids(mapper_fused_right.dsi_); 282 | break; 283 | case 4: 284 | mapper_fused_camera_time.dsi_.geometricMeanTwoGrids(mapper_fused_right.dsi_); 285 | break; 286 | case 5: 287 | mapper_fused_camera_time.dsi_.rmsTwoGrids(mapper_fused_right.dsi_); 288 | break; 289 | case 6: 290 | mapper_fused_camera_time.dsi_.maxTwoGrids(mapper_fused_right.dsi_); 291 | break; 292 | default: 293 | LOG(INFO) << "Improper stereo fusion method selected"; 294 | return; 295 | } 296 | 297 | if (!opts_depth_map.full_sequence) { 298 | mapper_fused_camera_time.dsi_.writeGridNpy(std::string(out_path + "dsi_stereo_temporalfusion_camera_time.npy").c_str()); 299 | } 300 | mapper_fused_camera_time.getDepthMapFromDSI(depth_map, confidence_map, semidense_mask, opts_depth_map); 301 | saveDepthMaps(depth_map, confidence_map, semidense_mask, dsi_shape.min_depth_, dsi_shape.max_depth_, std::string("stereo_temporal_camera_time" + std::to_string(temporal_fusion)), ss.str()); 302 | 303 | } 304 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/src/process5.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * \file process5.cpp 3 | * \brief process events using Alg 2 + shuffling 4 | * \author (1) Suman Ghosh 5 | * \date 2022-09-01 6 | * \author (2) Guillermo Gallego 7 | * \date 2022-09-01 8 | * Copyright/Rights of Use: 9 | * 2022, Technische Universität Berlin 10 | * Prof. Guillermo Gallego 11 | * Robotic Interactive Perception 12 | * Marchstrasse 23, Sekr. MAR 5-5 13 | * 10587 Berlin, Germany 14 | */ 15 | 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | //#define TIMING_LOOP 24 | 25 | // Alg 2 + shuffling: Split events into sub-intervals, shuffle mini-DSIs across time and cameras, fuse them 26 | 27 | void process_5(const image_geometry::PinholeCameraModel& cam0, 28 | const image_geometry::PinholeCameraModel& cam1, 29 | const LinearTrajectory& trajectory0, 30 | const LinearTrajectory& trajectory1, 31 | const std::vector& events0, 32 | const std::vector& events1, 33 | const EMVS::OptionsDepthMap& opts_depth_map, 34 | const EMVS::ShapeDSI& dsi_shape, 35 | const int num_subintervals, 36 | EMVS::MapperEMVS& mapper_fused, 37 | const std::string& out_path, 38 | double ts, 39 | int stereo_fusion, 40 | int temporal_fusion 41 | ) 42 | { 43 | // Initialize mappers 44 | const unsigned int num_events_per_subinterval[2] = {static_cast(events0.size()) / num_subintervals, 45 | static_cast(events1.size()) / num_subintervals}; 46 | 47 | // Initialize mappers 48 | EMVS::MapperEMVS mapper0(cam0, dsi_shape); 49 | EMVS::MapperEMVS mapper1(cam1, dsi_shape); 50 | EMVS::MapperEMVS mapper_fused_subinterval(cam0, dsi_shape); 51 | EMVS::MapperEMVS mapper_fused_left(cam0, dsi_shape); 52 | EMVS::MapperEMVS mapper_fused_right(cam0, dsi_shape); 53 | 54 | geometry_utils::Transformation T_rv_w; 55 | double t_mid; 56 | 57 | // Set the location of the DSI (reference view) in the middle of the trajectory of the left camera 58 | // if(ts <0) 59 | // { 60 | // geometry_utils::Transformation T0, T1; 61 | // ros::Time t0, t1; 62 | // trajectory0.getFirstControlPose(&T0, &t0); 63 | // trajectory0.getLastControlPose(&T1, &t1); 64 | // t_mid = 0.5 * (t0.toSec() + t1.toSec()); 65 | // LOG(INFO) << "Setting DSI reference at middle of left camera trajectory: " << t_mid; 66 | // } // Set DSI reference at provided timestamp ts 67 | // else 68 | // { 69 | t_mid = ts; 70 | LOG(INFO) << "Setting DSI reference at specific timestamp: " << t_mid; 71 | // } 72 | 73 | // set up prefix including output path 74 | std::stringstream ss; 75 | ss << out_path << std::fixed << std::setprecision(9) << std::setfill('0') << std::setw(13) << t_mid; 76 | 77 | geometry_utils::Transformation T_w_rv; 78 | trajectory0.getPoseAt(ros::Time(t_mid), T_w_rv); 79 | T_rv_w = T_w_rv.inverse(); 80 | 81 | cv::Mat depth_map, confidence_map, semidense_mask; 82 | 83 | LOG(INFO) << "Computing DSI for the cameras"; 84 | LOG(INFO) << "Left : Number of events per sub-interval = " << num_events_per_subinterval[0] << " events"; 85 | LOG(INFO) << "Right: Number of events per sub-interval = " << num_events_per_subinterval[1] << " events"; 86 | 87 | 88 | // When shift=0, there is no shuffling 89 | int shift = num_subintervals/2; 90 | 91 | // Fuse the DSIs 92 | mapper_fused.dsi_.resetGrid(); 93 | unsigned int idx_first_ev[2] = {0, shift*num_events_per_subinterval[1]}; 94 | #ifdef TIMING_LOOP 95 | int nloops = 100; 96 | #endif 97 | std::chrono::high_resolution_clock::time_point t_start, t_end = std::chrono::high_resolution_clock::now(); 98 | auto fusion_duration = std::chrono::duration_cast(t_end - t_end).count(); 99 | 100 | for (unsigned int k=0; k < num_subintervals; k++) 101 | { 102 | mapper0.dsi_.resetGrid(); 103 | mapper1.dsi_.resetGrid(); 104 | mapper_fused_subinterval.dsi_.resetGrid(); 105 | {// Left camera: 106 | // Approximation: this selection does not guarantee that events belong to the same time interval on both cameras 107 | // LOG(INFO)<< idx_first_ev[0] << "-" << idx_first_ev[0] + num_events_per_subinterval[0]; 108 | std::vector events_subset = std::vector(events0.begin() + idx_first_ev[0], 109 | events0.begin() + idx_first_ev[0] + num_events_per_subinterval[0]); 110 | idx_first_ev[0] += num_events_per_subinterval[0]; 111 | 112 | {// Display accumulted events to visualize the amount of intra-camera shift/baseline 113 | cv::Size full_resolution = cam0.fullResolution(); 114 | cv::Mat event_image = cv::Mat(full_resolution,CV_8UC1); 115 | accumulateEvents(events_subset,true,event_image); 116 | std::stringstream filename; 117 | filename << "events_0_" << std::setfill('0') << std::setw(3) << k << ".png"; 118 | cv::imwrite(out_path+filename.str(),event_image); 119 | } 120 | 121 | // Back-project events into the DSI 122 | mapper0.evaluateDSI(events_subset, trajectory0, T_rv_w); 123 | LOG(INFO) << "Left : Sub-interval " << k << " Mean square = " << mapper0.dsi_.computeMeanSquare(); 124 | 125 | // mapper0.getDepthMapFromDSI(depth_map, confidence_map, semidense_mask, opts_depth_map); 126 | // { 127 | // std::stringstream ss_suffix; 128 | // ss_suffix << "0_" << std::setfill('0') << std::setw(3) << k; 129 | // saveDepthMaps(depth_map, confidence_map, semidense_mask, dsi_shape.min_depth_, dsi_shape.max_depth_, ss_suffix.str(), ss.str()); 130 | // } 131 | } 132 | 133 | {// Right camera: 134 | // Approximation: this selection does not guarantee that events belong to the same time interval on both cameras 135 | std::vector events_subset; 136 | if (idx_first_ev[1] + num_events_per_subinterval[1] >= events1.size()){ 137 | 138 | // LOG(INFO)<< idx_first_ev[1] << "-" << idx_first_ev[1] + num_events_per_subinterval[1] - events1.size(); 139 | events_subset = std::vector(events1.begin() + idx_first_ev[1], 140 | events1.end()); 141 | events_subset.insert(events_subset.end(), events1.begin(), events1.begin() + idx_first_ev[1] + num_events_per_subinterval[1] - events1.size()); 142 | idx_first_ev[1]=idx_first_ev[1] + num_events_per_subinterval[1] - events1.size(); 143 | } 144 | else{ 145 | 146 | // LOG(INFO)<< idx_first_ev[1] << "-" << idx_first_ev[1] + num_events_per_subinterval[1]; 147 | events_subset = std::vector(events1.begin() + idx_first_ev[1], 148 | events1.begin() + idx_first_ev[1] + num_events_per_subinterval[1]); 149 | idx_first_ev[1] += num_events_per_subinterval[1]; 150 | } 151 | 152 | {// Display accumulted events to visualize the amount of intra-camera shift/baseline 153 | cv::Size full_resolution = cam1.fullResolution(); 154 | cv::Mat event_image = cv::Mat(full_resolution,CV_8UC1); 155 | accumulateEvents(events_subset,true,event_image); 156 | std::stringstream filename; 157 | filename << "events_1_" << std::setfill('0') << std::setw(3) << (k+shift)%num_subintervals << ".png"; 158 | cv::imwrite(out_path+filename.str(),event_image); 159 | } 160 | 161 | // Back-project events into the DSI 162 | mapper1.evaluateDSI(events_subset, trajectory1, T_rv_w); 163 | LOG(INFO) << "Right: Sub-interval " << (k+shift)%num_subintervals << " Mean square = " << mapper1.dsi_.computeMeanSquare(); 164 | 165 | // mapper1.getDepthMapFromDSI(depth_map, confidence_map, semidense_mask, opts_depth_map); 166 | // { 167 | // std::stringstream ss_suffix; 168 | // ss_suffix << "1_" << std::setfill('0') << std::setw(3) << (k+shift)%num_subintervals; 169 | // saveDepthMaps(depth_map, confidence_map, semidense_mask, dsi_shape.min_depth_, dsi_shape.max_depth_, ss_suffix.str(), ss.str()); 170 | // } 171 | } 172 | 173 | // Initialize stereo mapper for a single subinterval 174 | mapper_fused_subinterval.dsi_.resetGrid(); 175 | mapper_fused_subinterval.dsi_.addTwoGrids(mapper0.dsi_); 176 | 177 | // Fused DSI across cameras for a single sub-interval 178 | t_start = std::chrono::high_resolution_clock::now(); 179 | #ifdef TIMING_LOOP 180 | for (int i=1; i<=nloops; i++){ 181 | #endif 182 | switch(stereo_fusion){ 183 | case 1: 184 | mapper_fused_subinterval.dsi_.minTwoGrids(mapper1.dsi_); 185 | break; 186 | case 2: 187 | mapper_fused_subinterval.dsi_.harmonicMeanTwoGrids(mapper1.dsi_); 188 | break; 189 | case 3: 190 | mapper_fused_subinterval.dsi_.geometricMeanTwoGrids(mapper1.dsi_); 191 | break; 192 | case 4: 193 | mapper_fused_subinterval.dsi_.arithmeticMeanTwoGrids(mapper1.dsi_); 194 | break; 195 | case 5: 196 | mapper_fused_subinterval.dsi_.rmsTwoGrids(mapper1.dsi_); 197 | break; 198 | case 6: 199 | mapper_fused_subinterval.dsi_.maxTwoGrids(mapper1.dsi_); 200 | break; 201 | default: 202 | LOG(INFO) << "Improper fusion method selected"; 203 | return; 204 | } 205 | #ifdef TIMING_LOOP 206 | } 207 | #endif 208 | t_end = std::chrono::high_resolution_clock::now(); 209 | fusion_duration+=std::chrono::duration_cast(t_end-t_start).count(); 210 | 211 | // Fuse DSIs across time 212 | t_start = std::chrono::high_resolution_clock::now(); 213 | #ifdef TIMING_LOOP 214 | for (int i=1; i<=nloops; i++){ 215 | #endif 216 | switch (temporal_fusion){ 217 | case 1: 218 | break; 219 | case 2: 220 | // HM 221 | mapper_fused_left.dsi_.addInverseOfTwoGrids(mapper0.dsi_); 222 | mapper_fused_right.dsi_.addInverseOfTwoGrids(mapper1.dsi_); 223 | mapper_fused.dsi_.addInverseOfTwoGrids(mapper_fused_subinterval.dsi_); 224 | if (k==num_subintervals-1){ 225 | mapper_fused_left.dsi_.computeHMfromSumOfInv(num_subintervals); 226 | mapper_fused_right.dsi_.computeHMfromSumOfInv(num_subintervals); 227 | mapper_fused.dsi_.computeHMfromSumOfInv(num_subintervals); 228 | } 229 | break; 230 | case 3: 231 | break; 232 | case 4: 233 | // AM 234 | mapper_fused_left.dsi_.addTwoGrids(mapper0.dsi_); 235 | mapper_fused_right.dsi_.addTwoGrids(mapper1.dsi_); 236 | mapper_fused.dsi_.addTwoGrids(mapper_fused_subinterval.dsi_); 237 | if (k==num_subintervals-1){ 238 | mapper_fused_left.dsi_.computeAMfromSum(num_subintervals); 239 | mapper_fused_right.dsi_.computeAMfromSum(num_subintervals); 240 | mapper_fused.dsi_.computeAMfromSum(num_subintervals); 241 | } 242 | break; 243 | case 5: 244 | break; 245 | case 6: 246 | break; 247 | } 248 | #ifdef TIMING_LOOP 249 | } 250 | #endif 251 | t_end = std::chrono::high_resolution_clock::now(); 252 | fusion_duration+= (std::chrono::duration_cast(t_end - t_start).count())/3; 253 | 254 | } 255 | 256 | LOG(INFO) << "Time taken to fuse across space and time "<< fusion_duration <<" ms"; 257 | LOG(INFO) << "Mean square = " << mapper_fused.dsi_.computeMeanSquare(); 258 | mapper_fused.getDepthMapFromDSI(depth_map, confidence_map, semidense_mask, opts_depth_map); 259 | saveDepthMaps(depth_map, confidence_map, semidense_mask, dsi_shape.min_depth_, dsi_shape.max_depth_, std::string("stereo_temporal_" + std::to_string(temporal_fusion)), ss.str()); 260 | } 261 | -------------------------------------------------------------------------------- /mapper_emvs_stereo/src/utils.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * \file utils.cpp 3 | * \brief utility functions like saving depth and confidence maps 4 | * \author (1) Suman Ghosh 5 | * \date 2022-09-01 6 | * \author (2) Guillermo Gallego 7 | * \date 2022-09-01 8 | * Copyright/Rights of Use: 9 | * 2022, Technische Universität Berlin 10 | * Prof. Guillermo Gallego 11 | * Robotic Interactive Perception 12 | * Marchstrasse 23, Sekr. MAR 5-5 13 | * 10587 Berlin, Germany 14 | */ 15 | 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | void saveDepthMaps(const cv::Mat& depth_map, 23 | const cv::Mat& confidence_map, 24 | const cv::Mat& semidense_mask, 25 | const cv::Mat& depth_map_dense, 26 | const float min_depth, 27 | const float max_depth, 28 | const std::string& suffix, 29 | const std::string& out_path) 30 | { 31 | // save masked depth points with double precision 32 | std::ofstream of; 33 | std::string savePath(out_path + "depth_points_" + suffix); 34 | savePath.append(".txt"); 35 | of.open(savePath, std::ofstream::out); 36 | if(of.is_open()) 37 | { 38 | for (int r=0; r(r, c)>0){ 41 | of << c << " " << r << " " << depth_map.at(r, c) << "\n"; 42 | } 43 | } 44 | } 45 | } 46 | of.close(); 47 | 48 | // Save depth map, confidence map and semi-dense mask 49 | std::stringstream ss_suffix; 50 | ss_suffix << suffix << ".png"; 51 | // Save semi-dense mask as an image 52 | //cv::imwrite(out_path + "semidense_mask_" + ss_suffix.str(), 255 * semidense_mask); 53 | 54 | // Save confidence map as an 8-bit image 55 | cv::Mat confidence_map_255; 56 | cv::normalize(confidence_map, confidence_map_255, 0, 255.0, cv::NORM_MINMAX, CV_32FC1); 57 | // cv::imwrite(out_path + "confidence_map_" + ss_suffix.str(), confidence_map_255); 58 | cv::imwrite(out_path + "confidence_map_negated_" + ss_suffix.str(), 255-confidence_map_255); 59 | 60 | // Normalize depth map using given min and max depth values 61 | cv::Mat depth_map_255 = (depth_map - min_depth) * (255.0 / (max_depth - min_depth)); 62 | // cv::imwrite(out_path + "depth_map_" + ss_suffix.str(), depth_map_255); 63 | 64 | // Save pseudo-colored depth map on white canvas 65 | // cv::Mat depthmap_8bit, depthmap_color; 66 | // depth_map_255.convertTo(depthmap_8bit, CV_8U); 67 | // cv::applyColorMap(depthmap_8bit, depthmap_color, cv::COLORMAP_RAINBOW); 68 | // cv::Mat depth_on_canvas = cv::Mat(depth_map.rows, depth_map.cols, CV_8UC3, cv::Scalar(1,1,1)*255); 69 | // depthmap_color.copyTo(depth_on_canvas, semidense_mask); 70 | // cv::imwrite(out_path + "depth_colored_" + ss_suffix.str(), depth_on_canvas); 71 | 72 | // Save dilated colored depth map on black canvas 73 | // cv::applyColorMap(depthmap_8bit, depthmap_color, cv::COLORMAP_JET); 74 | // depth_on_canvas = cv::Mat(depth_map.rows, depth_map.cols, CV_8UC3, cv::Scalar(1,1,1)*0); 75 | // depthmap_color.copyTo(depth_on_canvas, semidense_mask); 76 | cv::Mat element = cv::getStructuringElement( cv::MORPH_ELLIPSE, cv::Size(3,3)); 77 | // cv::dilate(depth_on_canvas, depth_on_canvas, element); 78 | // cv::imwrite(out_path + "depth_colored_dilated" + ss_suffix.str(), depth_on_canvas); 79 | 80 | 81 | // Jet colormap with black background for inverse depth maps (similar to ESVO) 82 | cv::Mat invdepthmap_8bit, invdepthmap_color; 83 | cv::Mat invmap = 1./depth_map; 84 | float mod_max_depth = 1 * max_depth; 85 | cv::Mat invdepth_map_255 = (invmap - 1./mod_max_depth) / (1./min_depth - 1./mod_max_depth) * 255.; 86 | invdepth_map_255.convertTo(invdepthmap_8bit, CV_8U); 87 | cv::applyColorMap(invdepthmap_8bit, invdepthmap_color, cv::COLORMAP_JET); 88 | cv::Mat invdepth_on_canvas = cv::Mat(depth_map.rows, depth_map.cols, CV_8UC3, cv::Scalar(1,1,1)*0); 89 | invdepthmap_color.copyTo(invdepth_on_canvas, semidense_mask); 90 | // cv::imwrite(out_path + "inv_depth_colored_" + ss_suffix.str(), invdepth_on_canvas); 91 | element = cv::getStructuringElement( cv::MORPH_ELLIPSE, cv::Size(3,3)); 92 | cv::dilate(invdepth_on_canvas, invdepth_on_canvas, element); 93 | cv::imwrite(out_path + "inv_depth_colored_dilated_" + ss_suffix.str(), invdepth_on_canvas); 94 | 95 | // if (depth_map_dense.rows > 0) 96 | // { 97 | // // Normalize depth map using given min and max depth values 98 | // depth_map_255 = (depth_map_dense - min_depth) * (255.0 / (max_depth - min_depth)); 99 | // // Save pseudo-colored depth map on white canvas 100 | // depth_map_255.convertTo(depthmap_8bit, CV_8U); 101 | // cv::applyColorMap(depthmap_8bit, depthmap_color, cv::COLORMAP_RAINBOW); 102 | // cv::imwrite(out_path + "depth_colored_dense_" + ss_suffix.str(), depthmap_color); 103 | // } 104 | } 105 | 106 | 107 | void saveDepthMaps(const cv::Mat& depth_map, 108 | const cv::Mat& confidence_map, 109 | const cv::Mat& semidense_mask, 110 | const float min_depth, 111 | const float max_depth, 112 | const std::string& suffix, 113 | const std::string& out_path) 114 | { 115 | cv::Mat depth_map_dense; 116 | saveDepthMaps(depth_map, confidence_map, semidense_mask, depth_map_dense, min_depth, max_depth, suffix, out_path); 117 | } 118 | 119 | 120 | // Harmonic mean of two DSIs in mapper objects 121 | void fuseDSIs_HarmonicMean(const EMVS::MapperEMVS& mapper0, 122 | const EMVS::MapperEMVS& mapper1, 123 | EMVS::MapperEMVS& mapper_fused) 124 | { 125 | mapper_fused.dsi_.resetGrid(); 126 | mapper_fused.dsi_.addTwoGrids(mapper0.dsi_); // Initialize DSI 127 | mapper_fused.dsi_.harmonicMeanTwoGrids(mapper1.dsi_); 128 | } 129 | 130 | // Sum (e.g., arithmetic mean) of two DSIs in mapper objects 131 | void fuseDSIs_Sum(const EMVS::MapperEMVS& mapper0, 132 | const EMVS::MapperEMVS& mapper1, 133 | EMVS::MapperEMVS& mapper_fused) 134 | { 135 | mapper_fused.dsi_.resetGrid(); 136 | mapper_fused.dsi_.addTwoGrids(mapper0.dsi_); // Initialize DSI 137 | mapper_fused.dsi_.addTwoGrids(mapper1.dsi_); 138 | } 139 | 140 | 141 | // Temporary 142 | double computeFocus_MeanSquare(const cv::Mat& img) 143 | { 144 | return cv::mean(img.mul(img))[0]; 145 | } 146 | 147 | double computeFocus_Variance(const cv::Mat& img) 148 | { 149 | cv::Scalar f_mean, f_stddev; 150 | cv::meanStdDev(img, f_mean, f_stddev); 151 | return f_stddev[0]*f_stddev[0]; // variance 152 | } 153 | 154 | 155 | // Fusion: Harmonic mean of the local focus scores of both DSIs 156 | void fuseDSIs_HarmonicMeanOfLocalFocus(const EMVS::MapperEMVS& mapper0, 157 | const EMVS::MapperEMVS& mapper1, 158 | const image_geometry::PinholeCameraModel& cam0, 159 | const image_geometry::PinholeCameraModel& cam1, 160 | const EMVS::ShapeDSI& dsi_shape, 161 | const int focus_method, // 0 for variance, 1 for MS 162 | EMVS::MapperEMVS& mapper_focus_fused) 163 | { 164 | // Local focus score of each DSI 165 | // Using Gaussian weights (by convolution) 166 | EMVS::MapperEMVS mapper_focus0(cam0, dsi_shape); 167 | EMVS::MapperEMVS mapper_focus1(cam1, dsi_shape); 168 | 169 | mapper_focus0.dsi_.resetGrid(); 170 | mapper_focus0.dsi_.addTwoGrids(mapper0.dsi_); // Initialize DSI 171 | mapper_focus0.dsi_.computeLocalFocusInPlace(focus_method); 172 | 173 | mapper_focus1.dsi_.resetGrid(); 174 | mapper_focus1.dsi_.addTwoGrids(mapper1.dsi_); // Initialize DSI 175 | mapper_focus1.dsi_.computeLocalFocusInPlace(focus_method); 176 | 177 | // Harmonic mean of the local focus scores of both DSIs 178 | mapper_focus_fused.dsi_.resetGrid(); 179 | mapper_focus_fused.dsi_.addTwoGrids(mapper_focus0.dsi_); // Initialize DSI 180 | mapper_focus_fused.dsi_.harmonicMeanTwoGrids(mapper_focus1.dsi_); 181 | } 182 | 183 | 184 | void accumulateEvents(const std::vector& events, 185 | const bool use_polarity, 186 | cv::Mat& img) 187 | { 188 | if (use_polarity) 189 | { 190 | cv::Mat imgf = cv::Mat::zeros(img.size(),CV_32FC1); 191 | for(auto e: events) 192 | { 193 | imgf.at(e.y,e.x) += (e.polarity ? 1 : -1); 194 | } 195 | // Normalize to [0,255], with value 128 corresponding to zero events 196 | double min, max; 197 | cv::minMaxLoc(imgf, &min, &max); 198 | const double half_range = std::max( fabs(min), fabs(max) ); 199 | if (half_range > 0) 200 | { 201 | imgf = (imgf * (128/half_range)) + 128; 202 | imgf.convertTo(img,CV_8UC1); 203 | } else 204 | { 205 | img.setTo(128); 206 | } 207 | } else 208 | { 209 | img.setTo(0); 210 | for(auto e: events) 211 | { 212 | img.at(e.y,e.x) += 1; 213 | } 214 | cv::normalize(img, img, 0, 255, cv::NORM_MINMAX, CV_8UC1); 215 | } 216 | } 217 | --------------------------------------------------------------------------------