├── .gitignore ├── .vscode ├── c_cpp_properties.json ├── launch.json ├── settings.json └── tasks.json ├── Doxyfile ├── README.md ├── cartesian3dgrid ├── CMakeLists.txt ├── include │ └── cartesian3dgrid │ │ └── cartesian3dgrid.h ├── package.xml └── src │ ├── cartesian3dgrid.cpp │ └── cartesian3dgrid_IO.cpp ├── dependencies.yaml ├── esvo_core ├── CMakeLists.txt ├── calib │ ├── hkust │ │ ├── left.yaml │ │ └── right.yaml │ ├── rpg_mono │ │ ├── left.yaml │ │ └── right.yaml │ ├── rpg_slider │ │ ├── left.yaml │ │ └── right.yaml │ ├── rpg_stereo │ │ ├── left.yaml │ │ └── right.yaml │ ├── simu │ │ ├── left.yaml │ │ └── right.yaml │ └── upenn │ │ ├── left.yaml │ │ └── right.yaml ├── cfg │ ├── DVS_MappingStereo.cfg │ ├── mapping │ │ ├── mapping_hkust.yaml │ │ ├── mapping_rpg_stereo.yaml │ │ ├── mapping_simu.yaml │ │ └── mapping_upenn.yaml │ ├── monomapping │ │ ├── mapping_hkust.yaml │ │ ├── mapping_rpg.yaml │ │ ├── mapping_rpg_mono.yaml │ │ ├── mapping_rpg_simu.yaml │ │ └── mapping_upenn.yaml │ ├── mvsmono │ │ ├── mvsmono_rpg_mono.yaml │ │ ├── mvsmono_rpg_slider.yaml │ │ ├── mvsmono_rpg_stereo.yaml │ │ ├── mvsmono_simu.yaml │ │ └── mvsmono_upenn.yaml │ ├── mvstereo │ │ ├── mvstereo_rpg_stereo.yaml │ │ └── mvstereo_upenn.yaml │ ├── time_surface │ │ └── ts_parameters.yaml │ └── tracking │ │ ├── tracking_hkust.yaml │ │ ├── tracking_rpg_mono.yaml │ │ ├── tracking_rpg_stereo.yaml │ │ ├── tracking_simu.yaml │ │ └── tracking_upenn.yaml ├── esvo_monosystem.perspective ├── esvo_monosystem.rviz ├── esvo_mvsmono.perspective ├── esvo_mvsmono.rviz ├── esvo_mvstereo.perspective ├── esvo_mvstereo.rviz ├── esvo_system.perspective ├── esvo_system.rviz ├── include │ ├── emvs_core │ │ ├── DepthVector.hpp │ │ ├── MapperEMVS.hpp │ │ ├── MedianFilter.hpp │ │ └── Trajectory.hpp │ ├── esvo_core │ │ ├── Tracking.h │ │ ├── container │ │ │ ├── CameraSystem.h │ │ │ ├── CameraSystemMono.h │ │ │ ├── DepthMap.h │ │ │ ├── DepthPoint.h │ │ │ ├── EventMatchPair.h │ │ │ ├── ResidualItem.h │ │ │ ├── SmartGrid.h │ │ │ └── TimeSurfaceObservation.h │ │ ├── core │ │ │ ├── DepthFusion.h │ │ │ ├── DepthProblem.h │ │ │ ├── DepthProblemConfig.h │ │ │ ├── DepthProblemSolver.h │ │ │ ├── DepthRegularization.h │ │ │ ├── EventBM.h │ │ │ ├── EventMatcher.h │ │ │ ├── RegProblemLM.h │ │ │ └── RegProblemSolverLM.h │ │ ├── esvo_MVSMono.h │ │ ├── esvo_MVStereo.h │ │ ├── esvo_Mapping.h │ │ ├── esvo_MonoMapping.h │ │ ├── esvo_Tracking.h │ │ ├── optimization │ │ │ └── OptimizationFunctor.h │ │ └── tools │ │ │ ├── TicToc.h │ │ │ ├── Visualization.h │ │ │ ├── cayley.h │ │ │ ├── params_helper.h │ │ │ ├── sobel.h │ │ │ └── utils.h │ └── initial │ │ ├── InitialMotionEstimator.h │ │ ├── ModelSelector.h │ │ ├── MonoInitializer.h │ │ ├── feature_manager.h │ │ ├── initial_sfm.h │ │ └── solve_5pts.h ├── launch │ ├── monosystem │ │ ├── monosystem_rpg_stereo.launch │ │ └── monosystem_simu.launch │ ├── mvsmono │ │ ├── mvsmono_rpg_mono.launch │ │ ├── mvsmono_rpg_slider.launch │ │ └── mvsmono_simu.launch │ ├── mvstereo │ │ ├── mvstereo_rpg_stereo.launch │ │ └── mvstereo_upenn.launch │ ├── system │ │ ├── system_hkust.launch │ │ ├── system_rpg_stereo.launch │ │ ├── system_simu.launch │ │ └── system_upenn.launch │ └── tracking │ │ ├── tracking_rpg_mono.launch │ │ ├── tracking_rpg_stereo.launch │ │ └── tracking_simu.launch ├── package.xml ├── script │ ├── fn_constants.py │ ├── fn_constants.pyc │ └── run_esvo.py ├── src │ ├── Tracking.cpp │ ├── TrackingNode.cpp │ ├── container │ │ ├── CameraSystem.cpp │ │ ├── CameraSystemMono.cpp │ │ ├── DepthPoint.cpp │ │ ├── EventPoint.cpp │ │ └── ResidualItem.cpp │ ├── core │ │ ├── DepthFusion.cpp │ │ ├── DepthProblem.cpp │ │ ├── DepthProblemSolver.cpp │ │ ├── DepthRegularization.cpp │ │ ├── EventBM.cpp │ │ ├── EventMatcher.cpp │ │ ├── RegProblemLM.cpp │ │ └── RegProblemSolverLM.cpp │ ├── emvs_core │ │ ├── MapperEMVS.cpp │ │ └── MedianFilter.cpp │ ├── esvo_MVSMono.cpp │ ├── esvo_MVSMonoNode.cpp │ ├── esvo_MVStereo.cpp │ ├── esvo_MVStereoNode.cpp │ ├── esvo_Mapping.cpp │ ├── esvo_MappingNode.cpp │ ├── esvo_MonoMapping.cpp │ ├── esvo_MonoMappingNode.cpp │ ├── esvo_Tracking.cpp │ ├── esvo_TrackingNode.cpp │ ├── initial │ │ ├── InitialMotionEstimator.cpp │ │ ├── ModelSelector.cpp │ │ ├── MonoInitializer.cpp │ │ ├── feature_manager.cpp │ │ ├── initial_sfm.cpp │ │ └── solve_5pts.cpp │ └── tools │ │ ├── Visualization.cpp │ │ ├── cayley.cpp │ │ ├── publishMap.cpp │ │ ├── saveMap.cpp │ │ └── sobel.cpp └── test │ ├── test_key_event_processing.cpp │ ├── test_key_mapping.cpp │ ├── test_key_tracking.cpp │ └── test_transform.cpp ├── esvo_time_surface ├── CMakeLists.txt ├── cfg │ └── parameters.yaml ├── esvo_time_surface.perspective ├── include │ └── esvo_time_surface │ │ ├── TicToc.h │ │ └── TimeSurface.h ├── launch │ ├── mono_time_surface.launch │ ├── rosbag_launcher │ │ ├── hkust │ │ │ ├── hkust_calib_info.launch │ │ │ └── hkust_lab.launch │ │ ├── rpg_mono │ │ │ ├── rpg_boxes_6dof.launch │ │ │ └── rpg_dynamic_6dof.launch │ │ ├── rpg_simu │ │ │ ├── rpg_checkerboard_6dof.launch │ │ │ ├── rpg_office_6dof.launch │ │ │ └── rpg_shapes_poster_6dof.launch │ │ ├── rpg_slider │ │ │ ├── rpg_calib_info.launch │ │ │ ├── rpg_slider_depth.launch │ │ │ └── rpg_slider_far.launch │ │ ├── rpg_stereo │ │ │ ├── rpg_calib_info.launch │ │ │ ├── rpg_stereo_bin.launch │ │ │ ├── rpg_stereo_boxes.launch │ │ │ ├── rpg_stereo_desk.launch │ │ │ └── rpg_stereo_monitor.launch │ │ └── upenn │ │ │ ├── upenn_indoor_flying1.launch │ │ │ └── upenn_indoor_flying3.launch │ └── stereo_time_surface.launch ├── package.xml └── src │ ├── TimeSurface.cpp │ ├── TimeSurface_global_timer.cpp │ └── TimeSurface_node.cpp ├── events_repacking_helper ├── CMakeLists.txt ├── README.md ├── launch │ └── repacking.launch ├── package.xml ├── script │ ├── extract_topics.py │ ├── ijrr_rpg_bag_edited.sh │ ├── merge.py │ └── simu_bag_edited.sh └── src │ ├── EventMessageEditor.cpp │ └── EventMessageEditor_Mono.cpp ├── pict ├── MVSMONO_simu_office_planar.gif ├── Tracking_upenn_flying3.gif └── tracker_comparison.png └── stereo_rig_design ├── Stereo_Rig_Base.pdf ├── Stereo_Rig_Base.step └── camMount.SLDPRT /.gitignore: -------------------------------------------------------------------------------- 1 | doxgen_generated 2 | 3 | esvo_core/test/test_angleaxis.cpp 4 | 5 | -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Linux", 5 | "includePath": [ 6 | "${workspaceFolder}/**", 7 | "/usr/local/", 8 | "/usr/include/pcl-1.8/", 9 | "/opt/ros/melodic/include/**", 10 | "~/catkin_ws/devel/include/**", 11 | "~/catkin_ws/src/localization/eth_asl/", 12 | "/usr/include/eigen3" 13 | ], 14 | "defines": [], 15 | "compilerPath": "/usr/bin/clang", 16 | "cStandard": "c11", 17 | "cppStandard": "c++17", 18 | "intelliSenseMode": "clang-x64" 19 | } 20 | ], 21 | "version": 4 22 | } 23 | -------------------------------------------------------------------------------- /.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | // Use IntelliSense to learn about possible attributes. 3 | // Hover to view descriptions of existing attributes. 4 | // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 5 | "version": "0.2.0", 6 | "configurations": [ 7 | { 8 | "name": "g++-7 - Build and debug active file", 9 | "type": "cppdbg", 10 | "request": "launch", 11 | "program": "${fileDirname}/${fileBasenameNoExtension}", 12 | "args": [], 13 | "stopAtEntry": false, 14 | "cwd": "${workspaceFolder}", 15 | "environment": [], 16 | "externalConsole": false, 17 | "MIMode": "gdb", 18 | "setupCommands": [ 19 | { 20 | "description": "Enable pretty-printing for gdb", 21 | "text": "-enable-pretty-printing", 22 | "ignoreFailures": true 23 | } 24 | ], 25 | "preLaunchTask": "g++-7 build active file", 26 | "miDebuggerPath": "/usr/bin/gdb" 27 | } 28 | ] 29 | } -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "*.ipp": "cpp", 4 | "array": "cpp", 5 | "bitset": "cpp", 6 | "string_view": "cpp", 7 | "hash_map": "cpp", 8 | "hash_set": "cpp", 9 | "slist": "cpp", 10 | "initializer_list": "cpp", 11 | "utility": "cpp", 12 | "valarray": "cpp", 13 | "cctype": "cpp", 14 | "clocale": "cpp", 15 | "cmath": "cpp", 16 | "csignal": "cpp", 17 | "cstdarg": "cpp", 18 | "cstddef": "cpp", 19 | "cstdio": "cpp", 20 | "cstdlib": "cpp", 21 | "cstring": "cpp", 22 | "ctime": "cpp", 23 | "cwchar": "cpp", 24 | "cwctype": "cpp", 25 | "atomic": "cpp", 26 | "strstream": "cpp", 27 | "*.tcc": "cpp", 28 | "cfenv": "cpp", 29 | "chrono": "cpp", 30 | "cinttypes": "cpp", 31 | "codecvt": "cpp", 32 | "complex": "cpp", 33 | "condition_variable": "cpp", 34 | "cstdint": "cpp", 35 | "deque": "cpp", 36 | "list": "cpp", 37 | "unordered_map": "cpp", 38 | "unordered_set": "cpp", 39 | "vector": "cpp", 40 | "exception": "cpp", 41 | "algorithm": "cpp", 42 | "functional": "cpp", 43 | "optional": "cpp", 44 | "ratio": "cpp", 45 | "system_error": "cpp", 46 | "tuple": "cpp", 47 | "type_traits": "cpp", 48 | "fstream": "cpp", 49 | "iomanip": "cpp", 50 | "iosfwd": "cpp", 51 | "iostream": "cpp", 52 | "istream": "cpp", 53 | "limits": "cpp", 54 | "memory": "cpp", 55 | "mutex": "cpp", 56 | "new": "cpp", 57 | "ostream": "cpp", 58 | "numeric": "cpp", 59 | "sstream": "cpp", 60 | "stdexcept": "cpp", 61 | "streambuf": "cpp", 62 | "thread": "cpp", 63 | "typeindex": "cpp", 64 | "typeinfo": "cpp", 65 | "iterator": "cpp", 66 | "map": "cpp", 67 | "memory_resource": "cpp", 68 | "random": "cpp", 69 | "set": "cpp", 70 | "string": "cpp", 71 | "forward_list": "cpp", 72 | "filesystem": "cpp", 73 | "matrixfunctions": "cpp", 74 | "future": "cpp", 75 | "regex": "cpp", 76 | "*.txx": "cpp", 77 | "*.cuh": "cpp", 78 | "__tree": "cpp", 79 | "eigen": "cpp", 80 | "__split_buffer": "cpp" 81 | }, 82 | "cmake.configureOnOpen": true, 83 | "C_Cpp.errorSquiggles": "Disabled" 84 | } -------------------------------------------------------------------------------- /.vscode/tasks.json: -------------------------------------------------------------------------------- 1 | { 2 | "tasks": [ 3 | { 4 | "type": "shell", 5 | "label": "g++-7 build active file", 6 | "command": "/usr/bin/g++-7", 7 | "args": [ 8 | "-g", 9 | "${file}", 10 | "-o", 11 | "${fileDirname}/${fileBasenameNoExtension}" 12 | ], 13 | "options": { 14 | "cwd": "/usr/bin" 15 | } 16 | } 17 | ], 18 | "version": "2.0.0" 19 | } -------------------------------------------------------------------------------- /cartesian3dgrid/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(esvo_cartesian3dgrid) 2 | cmake_minimum_required(VERSION 2.8.3) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | set(CMAKE_BUILD_TYPE RelWithDebInfo) # Release, RelWithDebInfo 8 | set(CMAKE_CXX_FLAGS "-O3 -std=c++11 ${CMAKE_CXX_FLAGS}") 9 | 10 | file(GLOB SOURCE_FILES src/*.cpp) 11 | 12 | cs_add_library(${PROJECT_NAME} ${SOURCE_FILES} ${HEADER_FILES}) 13 | target_link_libraries(${PROJECT_NAME}) 14 | 15 | #target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) 16 | 17 | cs_export() 18 | 19 | -------------------------------------------------------------------------------- /cartesian3dgrid/include/cartesian3dgrid/cartesian3dgrid.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | // class GridType 8 | // { 9 | // public: 10 | // GridType() 11 | // { 12 | // score_ = 0.0f; 13 | // num_ = 0.0f; 14 | // } 15 | // float score_; 16 | // float num_; 17 | // }; 18 | 19 | class Grid3D 20 | { 21 | public: 22 | Grid3D(); 23 | Grid3D(const unsigned int dimX, const unsigned int dimY, const unsigned int dimZ); 24 | ~Grid3D(); 25 | 26 | void allocate(const unsigned int dimX, const unsigned int dimY, const unsigned int dimZ); 27 | void deallocate(); 28 | 29 | void printInfo() const; 30 | 31 | // The 3D data is stored in a 1D array that is ordered such that 32 | // volume[x + dimX*(y + dimY*z)] = data value at (x,y,z). 33 | 34 | // Accessing elements of the grid: 35 | 36 | // At integer location 37 | inline float getGridValueAt(const unsigned int ix, const unsigned int iy, const unsigned int iz) const 38 | { 39 | return data_array_.at(ix + size_[0]*(iy + size_[1]*iz)); 40 | } 41 | 42 | // At floating point location within a Z slice. Bilinear interpolation or voting. 43 | inline void accumulateGridValueAt(const float x_f, const float y_f, float* grid); 44 | 45 | void collapseMaxZSlice(cv::Mat* max_val, cv::Mat* max_pos) const; 46 | 47 | // Statistics 48 | double computeMeanSquare() const; 49 | 50 | void resetGrid(); 51 | 52 | // Output 53 | int writeGridNpy(const char szFilename[]) const; 54 | 55 | void getDimensions(int* dimX, int* dimY, int* dimZ) const 56 | { 57 | *dimX = size_[0]; 58 | *dimY = size_[1]; 59 | *dimZ = size_[2]; 60 | } 61 | 62 | float* getPointerToSlice(int layer) 63 | { 64 | return &data_array_.data()[layer * size_[0] * size_[1]]; 65 | } 66 | 67 | inline bool isValidGrid(const float x_f, const float y_f) 68 | { 69 | if (x_f >= 0.f && y_f >= 0.f && x_f < size_[0] && y_f < size_[1]) 70 | return true; 71 | else 72 | return false; 73 | } 74 | 75 | private: 76 | std::vector data_array_; 77 | unsigned int numCells_; 78 | unsigned int size_[3]; 79 | 80 | }; 81 | 82 | // Function implementation 83 | // Bilinear voting within a Z-slice, if point (x,y) is given as float 84 | inline void Grid3D::accumulateGridValueAt(const float x_f, const float y_f, float* grid) 85 | { 86 | if (x_f >= 0.f && y_f >= 0.f) // check if stay in the DSI 87 | { 88 | const int x = x_f, y = y_f; 89 | if (x + 1 < size_[0] && 90 | y + 1 < size_[1]) 91 | { 92 | float* g = grid + x + y * size_[0]; 93 | const float fx = x_f - x, 94 | fy = y_f - y, 95 | fx1 = 1.f - fx, 96 | fy1 = 1.f - fy; 97 | 98 | g[0] += fx1 * fy1; 99 | g[1] += fx * fy1; 100 | g[size_[0]] += fx1 * fy; 101 | g[size_[0] + 1] += fx * fy; 102 | } 103 | } 104 | } 105 | -------------------------------------------------------------------------------- /cartesian3dgrid/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | esvo_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 | #include "../include/cartesian3dgrid/cartesian3dgrid.h" 2 | 3 | #include 4 | #include 5 | 6 | 7 | Grid3D::Grid3D() 8 | { 9 | deallocate(); 10 | } 11 | 12 | 13 | Grid3D::Grid3D(const unsigned int dimX, const unsigned int dimY, const unsigned int dimZ) 14 | { 15 | deallocate(); 16 | allocate(dimX,dimY,dimZ); 17 | } 18 | 19 | 20 | void Grid3D::allocate(const unsigned int dimX, const unsigned int dimY, const unsigned int dimZ) 21 | { 22 | size_[0] = dimX; 23 | size_[1] = dimY; 24 | size_[2] = dimZ; 25 | numCells_ = size_[0]*size_[1]*size_[2]; 26 | data_array_.resize(numCells_); 27 | resetGrid(); 28 | } 29 | 30 | 31 | Grid3D::~Grid3D() 32 | { 33 | deallocate(); 34 | } 35 | 36 | 37 | void Grid3D::deallocate() 38 | { 39 | size_[0] = size_[1] = size_[2] = 0; 40 | data_array_.clear(); 41 | } 42 | 43 | 44 | void Grid3D::printInfo() const 45 | { 46 | std::cout << "Grid3D Dimensions: " << "(" << size_[0] << "," << size_[1] << "," << size_[2] << ")" << std::endl; 47 | std::cout << "Grid3D Data_array size: " << data_array_.size() << std::endl; 48 | } 49 | 50 | void Grid3D::resetGrid() 51 | { 52 | std::fill(data_array_.begin(), data_array_.end(), 0.f); 53 | } 54 | 55 | /** 56 | * @brief: record the position of the local maximum of each slice 57 | */ 58 | void Grid3D::collapseMaxZSlice(cv::Mat *max_val, cv::Mat *max_pos_idx) const 59 | { 60 | // Z-slice 61 | unsigned int u_size = size_[0], 62 | v_size = size_[1], 63 | w_size = size_[2]; 64 | 65 | *max_val = cv::Mat(v_size, u_size, CV_32FC1); // (y,x) as in images 66 | *max_pos_idx = cv::Mat(v_size, u_size, CV_8U); // WARNING: Max 256 depth layers 67 | 68 | std::vector grid_vals_vec(w_size); 69 | for (unsigned int v = 0; v < v_size; v++) 70 | { 71 | for (unsigned int u = 0; u < u_size; u++) 72 | { 73 | // Build vector containing the grid values at a given (u,v), as a function of depth 74 | for (unsigned int k = 0; k < w_size; ++k) 75 | grid_vals_vec.at(k) = getGridValueAt(u, v, k); 76 | 77 | // Look for maximum of vector and its location 78 | auto max = std::max_element(std::begin(grid_vals_vec), std::end(grid_vals_vec)); 79 | (*max_val).at(v, u) = *max; 80 | (*max_pos_idx).at(v, u) = std::distance(std::begin(grid_vals_vec), max); 81 | } 82 | } 83 | } 84 | 85 | /** 86 | * @brief: This function computes the mean square score of each cell 87 | */ 88 | double Grid3D::computeMeanSquare() const 89 | { 90 | double result = 0.; 91 | for (int i = 0; i < numCells_; i++) 92 | { 93 | double tmp = (double)data_array_.at(i); 94 | result += tmp * tmp; 95 | } 96 | 97 | return result / numCells_; 98 | } 99 | -------------------------------------------------------------------------------- /cartesian3dgrid/src/cartesian3dgrid_IO.cpp: -------------------------------------------------------------------------------- 1 | #include "cartesian3dgrid/cartesian3dgrid.h" 2 | 3 | #include 4 | 5 | #include 6 | 7 | 8 | //--------------------- Output for Numpy ----------------------------------------- 9 | 10 | int Grid3D::writeGridNpy(const char filename[]) const 11 | { 12 | cnpy::npy_save(std::string(filename), 13 | &data_array_[0], 14 | {size_[2], size_[1], size_[0]}, "w"); 15 | } 16 | -------------------------------------------------------------------------------- /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 | minkindr_ros: 31 | type: git 32 | url: https://github.com/ethz-asl/minkindr_ros.git 33 | version: master 34 | 35 | -------------------------------------------------------------------------------- /esvo_core/calib/hkust/left.yaml: -------------------------------------------------------------------------------- 1 | image_width: 346 2 | image_height: 260 3 | camera_name: hkust_DAVIS346_left 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [263.796, 0, 176.994, 0, 263.738, 124.373, 0, 0, 1] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 4 12 | data: [-0.386589, 0.157241, 0.000322143, 6.13759e-06] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.999809, 0.0161928, 0.0109163, 17 | -0.0162088, 0.999868, 0.0013701, 18 | -0.0108927, -0.00154678, 0.999939] 19 | projection_matrix: 20 | rows: 3 21 | cols: 4 22 | data: [189.705, 0, 165.382, 0, 0, 189.705, 121.295, 0, 0, 0, 1, 0] 23 | T_right_left: 24 | rows: 3 25 | cols: 4 26 | data: [9.99990798e-01, -6.32492385e-04, -4.24307214e-03, -7.30597639e-02, 27 | 6.44736387e-04, 9.99995631e-01, 2.88489843e-03, -1.23275257e-03, 28 | 4.24122892e-03, -2.88760755e-03, 9.99986837e-01, -1.10420407e-03] -------------------------------------------------------------------------------- /esvo_core/calib/hkust/right.yaml: -------------------------------------------------------------------------------- 1 | image_width: 346 2 | image_height: 260 3 | camera_name: hkust_DAVIS346_right 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [263.485, 0, 162.942, 0, 263.276, 118.029, -0.0151344, 0.00133093, 0.999885] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 4 12 | data: [-0.383425, 0.152823, -0.000257745, 0.000268432] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.9993960957463914, 0.0034732142808621717, -0.03457427641222047, 17 | -0.0035085878889783376, 0.9999933816804096, -0.0009625000798637905, 18 | 0.03457070461958685, 0.0010832257094615543, 0.9994016675011942] 19 | projection_matrix: 20 | rows: 3 21 | cols: 4 22 | data: [189.705, 0, 165.382, -13.8634, 0, 189.705, 121.295, 0, 0, 0, 1, 0] 23 | T_right_left: 24 | rows: 3 25 | cols: 4 26 | data: [9.99990798e-01, -6.32492385e-04, -4.24307214e-03, -7.30597639e-02, 27 | 6.44736387e-04, 9.99995631e-01, 2.88489843e-03, -1.23275257e-03, 28 | 4.24122892e-03, -2.88760755e-03, 9.99986837e-01, -1.10420407e-03] -------------------------------------------------------------------------------- /esvo_core/calib/rpg_mono/left.yaml: -------------------------------------------------------------------------------- 1 | image_width: 240 2 | image_height: 180 3 | camera_name: rpg_mono 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [199.0923665423112, 0.0, 132.1920713777002, 8 | 0.0, 198.8288204700886, 110.7126600112956, 9 | 0.0, 0.0, 1.0] 10 | distortion_model: plumb_bob 11 | distortion_coefficients: 12 | rows: 1 13 | cols: 4 14 | data: [-0.3684363117977873, 0.1509472435566583, -0.0002961305343848646, -0.000759431726241032] 15 | rectification_matrix: 16 | rows: 3 17 | cols: 3 18 | data: [1, 0, 0, 19 | 0, 1, 0, 20 | 0, 0, 1] 21 | projection_matrix: 22 | rows: 3 23 | cols: 4 24 | data: [168.6294097900391, 0.0, 135.348079770296, 0.0, 25 | 0.0, 178.5641784667969, 113.6189973794753, 0.0, 26 | 0.0, 0.0, 1.0, 0.0] 27 | T_right_left: 28 | rows: 3 29 | cols: 4 30 | data: [1, 0, 0, 0, 31 | 0, 1, 0, 0, 32 | 0, 0, 1, 0] -------------------------------------------------------------------------------- /esvo_core/calib/rpg_mono/right.yaml: -------------------------------------------------------------------------------- 1 | image_width: 240 2 | image_height: 180 3 | camera_name: rpg_mono 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [199.0923665423112, 0.0, 132.1920713777002, 8 | 0.0, 198.8288204700886, 110.7126600112956, 9 | 0.0, 0.0, 1.0] 10 | distortion_model: plumb_bob 11 | distortion_coefficients: 12 | rows: 1 13 | cols: 4 14 | data: [-0.3684363117977873, 0.1509472435566583, -0.0002961305343848646, -0.000759431726241032] 15 | rectification_matrix: 16 | rows: 3 17 | cols: 3 18 | data: [1, 0, 0, 19 | 0, 1, 0, 20 | 0, 0, 1] 21 | projection_matrix: 22 | rows: 3 23 | cols: 4 24 | data: [168.6294097900391, 0.0, 135.348079770296, 0.0, 25 | 0.0, 178.5641784667969, 113.6189973794753, 0.0, 26 | 0.0, 0.0, 1.0, 0.0] 27 | T_right_left: 28 | rows: 3 29 | cols: 4 30 | data: [1, 0, 0, 0, 31 | 0, 1, 0, 0, 32 | 0, 0, 1, 0] -------------------------------------------------------------------------------- /esvo_core/calib/rpg_slider/left.yaml: -------------------------------------------------------------------------------- 1 | image_width: 240 2 | image_height: 180 3 | camera_name: rpg_slider 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [335.4194629584808, 0.0, 129.9246633794451, 8 | 0.0, 335.3529356120773, 99.18643034473205, 9 | 0.0, 0.0, 1.0] 10 | distortion_model: plumb_bob 11 | distortion_coefficients: 12 | rows: 1 13 | cols: 4 14 | data: [-0.1385927674081299, 0.09337366641919795, -0.0003355869875320301, 0.0001737201582276446] 15 | rectification_matrix: 16 | rows: 3 17 | cols: 3 18 | data: [1, 0, 0, 19 | 0, 1, 0, 20 | 0, 0, 1] 21 | projection_matrix: 22 | rows: 3 23 | cols: 4 24 | data: [335.4194629584808, 0.0, 129.9246633794451, 0.0, 25 | 0.0, 335.3529356120773, 99.18643034473205, 0.0, 26 | 0.0, 0.0, 1.0, 0.0] 27 | # data: [328.3079223632812, 0.0, 129.7166999252022, 0.0, 0.0, 330.1483459472656, 98.78069942616821, 0.0, 0.0, 0.0, 1.0, 0.0] 28 | T_right_left: 29 | rows: 3 30 | cols: 4 31 | data: [1, 0, 0, 0, 32 | 0, 1, 0, 0, 33 | 0, 0, 1, 0] -------------------------------------------------------------------------------- /esvo_core/calib/rpg_slider/right.yaml: -------------------------------------------------------------------------------- 1 | image_width: 240 2 | image_height: 180 3 | camera_name: rpg_slider 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [335.4194629584808, 0.0, 129.9246633794451, 8 | 0.0, 335.3529356120773, 99.18643034473205, 9 | 0.0, 0.0, 1.0] 10 | distortion_model: plumb_bob 11 | distortion_coefficients: 12 | rows: 1 13 | cols: 4 14 | data: [-0.1385927674081299, 0.09337366641919795, -0.0003355869875320301, 0.0001737201582276446] 15 | rectification_matrix: 16 | rows: 3 17 | cols: 3 18 | data: [1, 0, 0, 19 | 0, 1, 0, 20 | 0, 0, 1] 21 | projection_matrix: 22 | rows: 3 23 | cols: 4 24 | data: [335.4194629584808, 0.0, 129.9246633794451, 0.0, 25 | 0.0, 335.3529356120773, 99.18643034473205, 0.0, 26 | 0.0, 0.0, 1.0, 0.0] 27 | # data: [328.3079223632812, 0.0, 129.7166999252022, 0.0, 0.0, 330.1483459472656, 98.78069942616821, 0.0, 0.0, 0.0, 1.0, 0.0] 28 | T_right_left: 29 | rows: 3 30 | cols: 4 31 | data: [1, 0, 0, 0, 32 | 0, 1, 0, 0, 33 | 0, 0, 1, 0] -------------------------------------------------------------------------------- /esvo_core/calib/rpg_stereo/left.yaml: -------------------------------------------------------------------------------- 1 | image_width: 240 2 | image_height: 180 3 | camera_name: rpg_DAVIS240C_left 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [196.639, 0, 105.064, 0, 196.733, 72.4717, 0.0, 0.0, 1.0] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 4 12 | data: [-0.336733, 0.111789, -0.00140053, -0.000459594] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.999791, -0.018779, -0.00802416, 0.0187767, 0.999824, -0.000360707, 0.00802952, 0.000209964, 0.999968] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [156.925, 0, 108.167, 0, 0, 156.925, 78.4205, 0, 0, 0, 1, 0] 21 | T_right_left: 22 | rows: 3 23 | cols: 4 24 | data: [0.9991089760393723, -0.04098010198963204, 0.010093821797214667, -0.1479883582369969, 25 | 0.04098846609277917, 0.9991594254283246, -0.000623077121092687, -0.003289908601915284, 26 | -0.010059803423311134, 0.0010362522169301642, 0.9999488619606629, 0.0026798262366239016] -------------------------------------------------------------------------------- /esvo_core/calib/rpg_stereo/right.yaml: -------------------------------------------------------------------------------- 1 | image_width: 240 2 | image_height: 180 3 | camera_name: rpg_DAVIS240C_right 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [196.426, 0, 110.745, 0, 196.564, 88.1131, 0.0, 0.0, 1.0] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 4 12 | data: [-0.346294, 0.12772, -0.000272051, -0.000195801] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.999589, 0.0222217, -0.0181009, -0.0222166, 0.999753, 0.000486491, 0.0181073, -8.41512e-05, 0.999836] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [156.925, 0, 108.167, -23.2327, 21 | 0, 156.925, 78.4205, 0, 22 | 0, 0, 1, 0] 23 | T_right_left: 24 | rows: 3 25 | cols: 4 26 | data: [0.9991089760393723, -0.04098010198963204, 0.010093821797214667, -0.1479883582369969, 27 | 0.04098846609277917, 0.9991594254283246, -0.000623077121092687, -0.003289908601915284, 28 | -0.010059803423311134, 0.0010362522169301642, 0.9999488619606629, 0.0026798262366239016] -------------------------------------------------------------------------------- /esvo_core/calib/simu/left.yaml: -------------------------------------------------------------------------------- 1 | image_width: 346 2 | image_height: 260 3 | camera_name: simu_left 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [200.0, 0.0, 173.0, 8 | 0.0, 200.0, 130.0, 9 | 0.0, 0.0, 1.0] 10 | distortion_model: plumb_bob 11 | distortion_coefficients: 12 | rows: 1 13 | cols: 4 14 | data: [0.0, 0.0, 0.0, 0.0] 15 | rectification_matrix: 16 | rows: 3 17 | cols: 3 18 | data: [1, 0, 0, 19 | 0, 1, 0, 20 | 0, 0, 1] 21 | projection_matrix: 22 | rows: 3 23 | cols: 4 24 | data: [200.0, 0.0, 173.0, 0.0, 25 | 0.0, 200.0, 130.0, 0.0, 26 | 0.0, 0.0, 1.0, 0.0] 27 | T_right_left: 28 | rows: 3 29 | cols: 4 30 | data: [1, 0, 0, -0.15, 31 | 0, 1, 0, 0, 32 | 0, 0, 1, 0] 33 | 34 | -------------------------------------------------------------------------------- /esvo_core/calib/simu/right.yaml: -------------------------------------------------------------------------------- 1 | image_width: 346 2 | image_height: 260 3 | camera_name: simu_right 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [200.0, 0.0, 173.0, 8 | 0.0, 200.0, 130.0, 9 | 0.0, 0.0, 1.0] 10 | distortion_model: plumb_bob 11 | distortion_coefficients: 12 | rows: 1 13 | cols: 4 14 | data: [0.0, 0.0, 0.0, 0.0] 15 | rectification_matrix: 16 | rows: 3 17 | cols: 3 18 | data: [1, 0, 0, 19 | 0, 1, 0, 20 | 0, 0, 1] 21 | projection_matrix: 22 | rows: 3 23 | cols: 4 24 | data: [200.0, 0.0, 173.0, -30.0, 25 | 0.0, 200.0, 130.0, 0.0, 26 | 0.0, 0.0, 1.0, 0.0] 27 | T_right_left: 28 | rows: 3 29 | cols: 4 30 | data: [1, 0, 0, -0.15, 31 | 0, 1, 0, 0, 32 | 0, 0, 1, 0] -------------------------------------------------------------------------------- /esvo_core/calib/upenn/left.yaml: -------------------------------------------------------------------------------- 1 | image_width: 346 2 | image_height: 260 3 | camera_name: upenn_DAVIS346_left 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [226.38018519795807, 0.0, 173.6470807871759, 0.0, 226.15002947047415, 133.73271487507847, 0, 0, 1] 8 | distortion_model: equidistant 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 4 12 | data: [-0.048031442223833355, 0.011330957517194437, -0.055378166304281135, 0.021500973881459395] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.999877311526236, 0.015019439766575743, -0.004447282784398257, 17 | -0.014996983873604017, 0.9998748347535599, 0.005040367172759556, 18 | 0.004522429630305261, -0.004973052949604937, 0.9999774079320989] 19 | projection_matrix: 20 | rows: 3 21 | cols: 4 22 | data: [199.6530123165822, 0.0, 177.43276376280926, 0.0, 23 | 0.0, 199.6530123165822, 126.81215684365904, 0.0, 24 | 0.0, 0.0, 1.0, 0.0] 25 | T_right_left: 26 | rows: 3 27 | cols: 4 28 | data: [0.9999285439274112, 0.011088072985503046, -0.004467849222081981, -0.09988137641750752, 29 | -0.011042817783611191, 0.9998887260774646, 0.01002953830336461, -0.0003927067773089277, 30 | 0.004578560319692358, -0.009979483987103495, 0.9999397215256256, 1.8880107752680777e-06] -------------------------------------------------------------------------------- /esvo_core/calib/upenn/right.yaml: -------------------------------------------------------------------------------- 1 | image_width: 346 2 | image_height: 260 3 | camera_name: upenn_DAVIS346_right 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [226.0181418548734, 0, 174.5433576736815, 0, 225.7869434267677, 124.21627572590607, 0, 0, 1] 8 | distortion_model: equidistant 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 4 12 | data: [-0.04846669832871334, 0.010092844338123635, -0.04293073765014637, 0.005194706897326005] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.9999922706537476, 0.003931701344419404, -1.890238450965101e-05, 17 | -0.003931746704476347, 0.9999797362744968, -0.005006836150689904, 18 | -7.83382948021244e-07, 0.0050068717705076754, 0.9999874655386736] 19 | projection_matrix: 20 | rows: 3 21 | cols: 4 22 | data: [199.6530123165822, 0.0, 177.43276376280926, -19.941771812941038, 23 | 0.0, 199.6530123165822, 126.81215684365904, 0.0, 24 | 0.0, 0.0, 1.0, 0.0] 25 | T_right_left: 26 | rows: 3 27 | cols: 4 28 | data: [0.9999285439274112, 0.011088072985503046, -0.004467849222081981, -0.09988137641750752, 29 | -0.011042817783611191, 0.9998887260774646, 0.01002953830336461, -0.0003927067773089277, 30 | 0.004578560319692358, -0.009979483987103495, 0.9999397215256256, 1.8880107752680777e-06] -------------------------------------------------------------------------------- /esvo_core/cfg/DVS_MappingStereo.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "esvo_core" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | # for EventMatcher 9 | gen.add("EM_TIME_THRESHOLD", double_t, 0, "EM_TIME_THRESHOLD", 0.0001, 0.000001, 0.001) 10 | gen.add("EM_EPIPOLAR_THRESHOLD", double_t, 0, "EM_EPIPOLAR_THRESHOLD", 0.5, 0.1, 5) 11 | gen.add("EM_TS_NCC_THRESHOLD", double_t, 0, "EM_TS_NCC_THRESHOLD", 0.1, 0.0, 1.0) 12 | gen.add("EM_NUM_EVENT_MATCHING", int_t, 0, "EM_NUM_EVENT_MATCHING", 30000, 0, 100000) 13 | gen.add("EM_PATCH_INTENSITY_THRESHOLD", int_t, 0, "EM_PATCH_INTENSITY_THRESHOLD", 125, 0, 255) 14 | gen.add("EM_PATCH_VALID_RATIO", double_t, 0, "EM_PATCH_VALID_RATIO", 0.1, 0, 1.0) 15 | 16 | # for Block Matcher 17 | gen.add("BM_MAX_NUM_EVENTS_PER_MATCHING", int_t, 0, "BM_MAX_NUM_EVENTS_PER_MATCHING", 400, 1, 5000) 18 | gen.add("BM_min_disparity", int_t, 0, "BM_min_disparity", 0, 0, 5) 19 | gen.add("BM_max_disparity", int_t, 0, "BM_max_disparity", 40, 0, 50) 20 | gen.add("BM_step", int_t, 0, "BM_step", 2, 1, 5) 21 | gen.add("BM_ZNCC_Threshold", double_t, 0, "BM_ZNCC_Threshold", 0.1, 0, 1.0) 22 | 23 | # for esvo_Mapping 24 | gen.add("invDepth_min_range", double_t, 0, "InvDepth_MIN_RANGE", 0.16, 0.0, 10.0) 25 | gen.add("invDepth_max_range", double_t, 0, "InvDepth_MAX_RANGE", 2.0, 0.0, 10.0) 26 | 27 | gen.add("residual_vis_threshold", double_t, 0, "Residual_VIS_THRESHOLD", 12, 0.0, 50) 28 | gen.add("stdVar_vis_threshold", double_t, 0, "StdVariance_VIS_THRESHOLD", 0.12, 0.0, 3.0) 29 | 30 | gen.add("age_max_range", int_t, 0, "Age_MAX_RANGE", 5, 0, 10) 31 | gen.add("age_vis_threshold", int_t, 0, "Age_VIS_RANGE", 5, 0, 10) 32 | 33 | gen.add("fusion_radius", int_t, 0, "fusion radius", 0, 0, 5) 34 | gen.add("maxNumFusionFrames", int_t, 0, "Max Fusion Times", 0, 0, 100) 35 | gen.add("maxNumFusionPoints", int_t, 0, "Max Fusion Points",5000, 1000, 10000) 36 | gen.add("PROCESS_EVENT_NUM", int_t, 0, "#processed event", 100, 0, 10000) 37 | gen.add("TS_HISTORY_LENGTH", int_t, 0, "time_surface history length", 100, 0, 300) 38 | gen.add("mapping_rate_hz", int_t, 0, "mapping rate", 20, 1, 30) 39 | 40 | gen.add("Denoising", bool_t, 0, "denoising the events", False) 41 | gen.add("Regularization", bool_t, 0, "perform regularization", False) 42 | 43 | # for esvo_System 44 | gen.add("ResetButton", bool_t, 0, "reset system (used as a button...)", False) 45 | exit(gen.generate(PACKAGE, "esvo_core", "DVS_MappingStereo")) -------------------------------------------------------------------------------- /esvo_core/cfg/mapping/mapping_hkust.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.25 2 | invDepth_max_range: 2 3 | residual_vis_threshold: 20 4 | stdVar_vis_threshold: 0.15 5 | age_max_range: 10 6 | age_vis_threshold: 1 7 | fusion_radius: 0 8 | FUSION_STRATEGY: "CONST_FRAMES" #"CONST_POINTS"# #"CONST_FRAMES"# 9 | maxNumFusionFrames: 20 10 | maxNumFusionPoints: 4000 11 | Denoising: True 12 | SmoothTimeSurface: False 13 | Regularization: True 14 | bVisualizeGlobalPC: True 15 | visualizeGPC_interval: 1 # (unit: s) 16 | NumGPC_added_oper_refresh: 1500 17 | visualize_range: 2.5 18 | PROCESS_EVENT_NUM: 1000 19 | TS_HISTORY_LENGTH: 100 20 | INIT_SGM_DP_NUM_THRESHOLD: 500 21 | mapping_rate_hz: 20 22 | # DepthProblemConfig 23 | patch_size_X: 15 24 | patch_size_Y: 7 25 | LSnorm: Tdist #(Tdist or l2) 26 | Tdist_nu: 2.1897 27 | Tdist_scale: 16.6397 28 | Tdist_stdvar: 56.5347 29 | # EventBM parameters 30 | BM_half_slice_thickness: 0.001 31 | BM_min_disparity: 1 32 | BM_max_disparity: 40 33 | BM_step: 1 34 | BM_ZNCC_Threshold: 0.1 35 | BM_bUpDownConfiguration: False -------------------------------------------------------------------------------- /esvo_core/cfg/mapping/mapping_rpg_stereo.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.2 2 | invDepth_max_range: 2 3 | residual_vis_threshold: 20 4 | stdVar_vis_threshold: 0.015 5 | age_max_range: 10 6 | age_vis_threshold: 1 7 | fusion_radius: 0 8 | FUSION_STRATEGY: "CONST_POINTS" #"CONST_FRAMES" or "CONST_POINTS" 9 | maxNumFusionFrames: 40 10 | maxNumFusionPoints: 5000 11 | Denoising: True 12 | SmoothTimeSurface: False 13 | Regularization: True 14 | bVisualizeGlobalPC: True 15 | visualizeGPC_interval: 3 16 | NumGPC_added_oper_refresh: 1000 #1500 #3000 17 | visualize_range: 5.0 18 | PROCESS_EVENT_NUM: 1000 19 | TS_HISTORY_LENGTH: 100 20 | INIT_SGM_DP_NUM_THRESHOLD: 500 21 | mapping_rate_hz: 20 22 | # DepthProblemConfig 23 | patch_size_X: 15 24 | patch_size_Y: 7 25 | LSnorm: Tdist #(Tdist or l2) 26 | Tdist_nu: 2.1897 27 | Tdist_scale: 16.6397 28 | Tdist_stdvar: 56.5347 29 | # EventBM parameters 30 | BM_half_slice_thickness: 0.001 31 | BM_min_disparity: 1 32 | BM_max_disparity: 40 33 | BM_step: 1 34 | BM_ZNCC_Threshold: 0.1 35 | BM_bUpDownConfiguration: False 36 | 37 | -------------------------------------------------------------------------------- /esvo_core/cfg/mapping/mapping_simu.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.5 2 | invDepth_max_range: 2 3 | residual_vis_threshold: 20 4 | stdVar_vis_threshold: 0.015 5 | age_max_range: 10 6 | age_vis_threshold: 1 7 | fusion_radius: 0 8 | FUSION_STRATEGY: "CONST_POINTS" #"CONST_FRAMES" or "CONST_POINTS" 9 | maxNumFusionFrames: 40 10 | maxNumFusionPoints: 5000 11 | Denoising: False 12 | SmoothTimeSurface: False 13 | Regularization: False 14 | bVisualizeGlobalPC: True 15 | visualizeGPC_interval: 3 16 | NumGPC_added_oper_refresh: 1000 #1500 #3000 17 | visualize_range: 5.0 18 | PROCESS_EVENT_NUM: 1000 19 | TS_HISTORY_LENGTH: 100 20 | INIT_SGM_DP_NUM_THRESHOLD: 500 21 | mapping_rate_hz: 20 22 | # DepthProblemConfig 23 | patch_size_X: 15 24 | patch_size_Y: 7 25 | LSnorm: Tdist #(Tdist or l2) 26 | Tdist_nu: 2.1897 27 | Tdist_scale: 16.6397 28 | Tdist_stdvar: 56.5347 29 | # EventBM parameters 30 | BM_half_slice_thickness: 0.001 31 | BM_min_disparity: 1 32 | BM_max_disparity: 40 33 | BM_step: 1 34 | BM_ZNCC_Threshold: 0.1 35 | BM_bUpDownConfiguration: False 36 | 37 | -------------------------------------------------------------------------------- /esvo_core/cfg/mapping/mapping_upenn.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.16 2 | invDepth_max_range: 1.0 3 | residual_vis_threshold: 20 4 | stdVar_vis_threshold: 0.15 5 | age_max_range: 10 6 | age_vis_threshold: 1 7 | fusion_radius: 0 8 | FUSION_STRATEGY: "CONST_POINTS" # "CONST_FRAMES" 9 | maxNumFusionFrames: 40 10 | maxNumFusionPoints: 3000 11 | Denoising: False 12 | SmoothTimeSurface: False 13 | Regularization: False 14 | bVisualizeGlobalPC: True 15 | visualizeGPC_interval: 1 16 | NumGPC_added_oper_refresh: 3000 17 | visualize_range: 5.0 18 | PROCESS_EVENT_NUM: 1000 19 | TS_HISTORY_LENGTH: 100 20 | INIT_SGM_DP_NUM_THRESHOLD: 500 21 | mapping_rate_hz: 20 22 | # DepthProblemConfig 23 | patch_size_X: 15 24 | patch_size_Y: 7 25 | LSnorm: Tdist # l2 26 | Tdist_nu: 2.182 27 | Tdist_scale: 17.277 28 | Tdist_stdvar: 59.763 29 | # EventBM parameters 30 | BM_half_slice_thickness: 0.001 31 | BM_min_disparity: 1 32 | BM_max_disparity: 40 33 | BM_step: 1 34 | BM_ZNCC_Threshold: 0.1 35 | BM_bUpDownConfiguration: False 36 | -------------------------------------------------------------------------------- /esvo_core/cfg/monomapping/mapping_hkust.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.25 2 | invDepth_max_range: 2 3 | residual_vis_threshold: 20 4 | stdVar_vis_threshold: 0.15 5 | age_max_range: 10 6 | age_vis_threshold: 1 7 | fusion_radius: 0 8 | FUSION_STRATEGY: "CONST_FRAMES" #"CONST_POINTS"# #"CONST_FRAMES"# 9 | maxNumFusionFrames: 20 10 | maxNumFusionPoints: 4000 11 | Denoising: True 12 | SmoothTimeSurface: False 13 | Regularization: True 14 | bVisualizeGlobalPC: True 15 | visualizeGPC_interval: 1 # (unit: s) 16 | NumGPC_added_oper_refresh: 1500 17 | visualize_range: 2.5 18 | PROCESS_EVENT_NUM: 1000 19 | TS_HISTORY_LENGTH: 100 20 | INIT_SGM_DP_NUM_THRESHOLD: 500 21 | mapping_rate_hz: 20 22 | # DepthProblemConfig 23 | patch_size_X: 15 24 | patch_size_Y: 7 25 | LSnorm: Tdist #(Tdist or l2) 26 | Tdist_nu: 2.1897 27 | Tdist_scale: 16.6397 28 | Tdist_stdvar: 56.5347 29 | # EventBM parameters 30 | BM_half_slice_thickness: 0.001 31 | BM_min_disparity: 1 32 | BM_max_disparity: 40 33 | BM_step: 1 34 | BM_ZNCC_Threshold: 0.1 35 | BM_bUpDownConfiguration: False -------------------------------------------------------------------------------- /esvo_core/cfg/monomapping/mapping_rpg.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.2 2 | invDepth_max_range: 2 3 | residual_vis_threshold: 20 4 | stdVar_vis_threshold: 0.015 5 | age_max_range: 10 6 | age_vis_threshold: 1 7 | fusion_radius: 0 8 | FUSION_STRATEGY: "CONST_POINTS" #"CONST_FRAMES" or "CONST_POINTS" 9 | maxNumFusionFrames: 40 10 | maxNumFusionPoints: 5000 11 | Denoising: True 12 | SmoothTimeSurface: False 13 | Regularization: True 14 | bVisualizeGlobalPC: True 15 | visualizeGPC_interval: 3 16 | NumGPC_added_oper_refresh: 1000 #1500 #3000 17 | visualize_range: 5.0 18 | PROCESS_EVENT_NUM: 1000 19 | TS_HISTORY_LENGTH: 100 20 | INIT_SGM_DP_NUM_THRESHOLD: 500 21 | mapping_rate_hz: 100 22 | # DepthProblemConfig 23 | patch_size_X: 15 24 | patch_size_Y: 7 25 | LSnorm: Tdist #(Tdist or l2) 26 | Tdist_nu: 2.1897 27 | Tdist_scale: 16.6397 28 | Tdist_stdvar: 56.5347 29 | # EventBM parameters 30 | BM_half_slice_thickness: 0.001 31 | BM_min_disparity: 1 32 | BM_max_disparity: 40 33 | BM_step: 1 34 | BM_ZNCC_Threshold: 0.1 35 | BM_bUpDownConfiguration: False 36 | 37 | # Monocular Mapping 38 | opts_dim_x: 0 39 | opts_dim_y: 0 40 | opts_dim_z: 100 41 | opts_depth_map_kernal_size: 5 # Size of the Gaussian kernel used for adaptive thresholding" 42 | opts_depth_map_threshold_c: 7 # A value in [0, 255]. The smaller the noisier and more dense reconstruction" 43 | opts_depth_map_median_filter_size: 15 # Size of the median filter used to clean the depth map" 44 | opts_pc_radius_search: 0.03 45 | opts_pc_min_num_neighbors: 3 46 | CAM_CALIB_PATH: "/home/jjiao/catkin_ws/src/localization/ESVO/esvo_core/calib/rpg/left_pinhole.yaml" 47 | PATH_TO_SAVE_RESULT: "/home/jjiao/ESVO_result/rpg/" # CHANGE THIS PATH 48 | KEYFRAME_LINEAR_DIS: 0.15 # m 49 | KEYFRAME_ORIENTATION_DIS: 5 # degree -------------------------------------------------------------------------------- /esvo_core/cfg/monomapping/mapping_rpg_mono.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.2 2 | invDepth_max_range: 2 3 | residual_vis_threshold: 20 4 | stdVar_vis_threshold: 0.015 5 | age_max_range: 10 6 | age_vis_threshold: 1 7 | fusion_radius: 0 8 | FUSION_STRATEGY: "CONST_POINTS" #"CONST_FRAMES" or "CONST_POINTS" 9 | maxNumFusionFrames: 40 10 | maxNumFusionPoints: 5000 11 | Denoising: True 12 | SmoothTimeSurface: False 13 | Regularization: True 14 | bVisualizeGlobalPC: True 15 | visualizeGPC_interval: 3 16 | NumGPC_added_oper_refresh: 1000 #1500 #3000 17 | visualize_range: 5.0 18 | PROCESS_EVENT_NUM: 1000 19 | TS_HISTORY_LENGTH: 100 20 | INIT_SGM_DP_NUM_THRESHOLD: 500 21 | mapping_rate_hz: 100 22 | # DepthProblemConfig 23 | patch_size_X: 15 24 | patch_size_Y: 7 25 | LSnorm: Tdist #(Tdist or l2) 26 | Tdist_nu: 2.1897 27 | Tdist_scale: 16.6397 28 | Tdist_stdvar: 56.5347 29 | # EventBM parameters 30 | BM_half_slice_thickness: 0.001 31 | BM_min_disparity: 1 32 | BM_max_disparity: 40 33 | BM_step: 1 34 | BM_ZNCC_Threshold: 0.1 35 | BM_bUpDownConfiguration: False 36 | 37 | # Monocular Mapping 38 | opts_dim_x: 0 39 | opts_dim_y: 0 40 | opts_dim_z: 100 41 | opts_depth_map_kernal_size: 5 # Size of the Gaussian kernel used for adaptive thresholding" 42 | opts_depth_map_threshold_c: 7 # A value in [0, 255]. The smaller the noisier and more dense reconstruction" 43 | opts_depth_map_median_filter_size: 15 # Size of the median filter used to clean the depth map" 44 | opts_pc_radius_search: 0.03 45 | opts_pc_min_num_neighbors: 3 46 | CAM_CALIB_PATH: "/home/jjiao/catkin_ws/src/localization/ESVO/esvo_core/calib/rpg/left_pinhole.yaml" 47 | PATH_TO_SAVE_RESULT: "/home/jjiao/ESVO_result/rpg/" # CHANGE THIS PATH 48 | KEYFRAME_LINEAR_DIS: 0.15 # m 49 | KEYFRAME_ORIENTATION_DIS: 5 # degree -------------------------------------------------------------------------------- /esvo_core/cfg/monomapping/mapping_rpg_simu.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.66 2 | invDepth_max_range: 2.0 3 | residual_vis_threshold: 20 4 | stdVar_vis_threshold: 0.05 5 | age_max_range: 10 6 | age_vis_threshold: 1 7 | fusion_radius: 0 8 | FUSION_STRATEGY: "CONST_FRAMES" #"CONST_FRAMES" or "CONST_POINTS" 9 | maxNumFusionFrames: 5 10 | maxNumFusionPoints: 5000 11 | Denoising: False 12 | SmoothTimeSurface: False 13 | Regularization: True 14 | bVisualizeGlobalPC: True 15 | visualizeGPC_interval: 3 16 | NumGPC_added_oper_refresh: 1000 #1500 #3000 17 | visualize_range: 5.0 18 | PROCESS_EVENT_NUM: 20000 19 | TS_HISTORY_LENGTH: 100 20 | INIT_SGM_DP_NUM_THRESHOLD: 500 21 | mapping_rate_hz: 20 22 | # DepthProblemConfig 23 | patch_size_X: 15 24 | patch_size_Y: 7 25 | LSnorm: Tdist #(Tdist or l2) 26 | Tdist_nu: 2.1897 27 | Tdist_scale: 16.6397 28 | Tdist_stdvar: 56.5347 29 | # Publish result parameters 30 | bVisualizeGlobalPC: False 31 | visualizeGPC_interval: 3 32 | NumGPC_added_oper_refresh: 1000 33 | 34 | SAVE_RESULT: False 35 | PATH_TO_SAVE_RESULT: "/home/jjiao/ESVO_result/rpg_simu/" # CHANGE THIS PATH 36 | 37 | # EMVS Mapping 38 | opts_dim_x: 0 39 | opts_dim_y: 0 40 | opts_dim_z: 50 41 | opts_depth_map_kernal_size: 5 # Size of the Gaussian kernel used for adaptive thresholding" 42 | opts_depth_map_threshold_c: 7 # A value in [0, 255]. The smaller the noisier and more dense reconstruction" 43 | opts_depth_map_median_filter_size: 15 # Size of the median filter used to clean the depth map" 44 | opts_depth_map_contrast_threshold: 1 # Threshold of the contrast to remove outliers in the mask 45 | opts_pc_radius_search: 0.05 46 | opts_pc_min_num_neighbors: 3 47 | opts_mapper_parallex: 0.0 48 | opts_mapper_TS_score: 1.0 49 | KEYFRAME_MEANDEPTH_DIS: 0.5 # percentage 50 | EMVS_Init_event: 400000000 #200000 51 | EMVS_Keyframe_event: 2000000 #200000 52 | stdVar_init: 0.01 53 | invDepth_init: 1.0 -------------------------------------------------------------------------------- /esvo_core/cfg/monomapping/mapping_upenn.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.16 2 | invDepth_max_range: 1.0 3 | residual_vis_threshold: 20 4 | stdVar_vis_threshold: 0.15 5 | age_max_range: 10 6 | age_vis_threshold: 1 7 | fusion_radius: 0 8 | FUSION_STRATEGY: "CONST_POINTS" # "CONST_FRAMES" 9 | maxNumFusionFrames: 40 10 | maxNumFusionPoints: 3000 11 | Denoising: False 12 | SmoothTimeSurface: False 13 | Regularization: False 14 | bVisualizeGlobalPC: True 15 | visualizeGPC_interval: 1 16 | NumGPC_added_oper_refresh: 3000 17 | visualize_range: 5.0 18 | PROCESS_EVENT_NUM: 1000 19 | TS_HISTORY_LENGTH: 100 20 | INIT_SGM_DP_NUM_THRESHOLD: 500 21 | mapping_rate_hz: 20 22 | # DepthProblemConfig 23 | patch_size_X: 15 24 | patch_size_Y: 7 25 | LSnorm: Tdist # l2 26 | Tdist_nu: 2.182 27 | Tdist_scale: 17.277 28 | Tdist_stdvar: 59.763 29 | # EventBM parameters 30 | BM_half_slice_thickness: 0.001 31 | BM_min_disparity: 1 32 | BM_max_disparity: 40 33 | BM_step: 1 34 | BM_ZNCC_Threshold: 0.1 35 | BM_bUpDownConfiguration: False 36 | 37 | # Monocular Mapping 38 | opts_depth_map_kernal_size: 5 # Size of the Gaussian kernel used for adaptive thresholding" 39 | opts_depth_map_threshold_c: 7 # A value in [0, 255]. The smaller the noisier and more dense reconstruction" 40 | opts_depth_map_median_filter_size: 5 # Size of the median filter used to clean the depth map" 41 | opts_pc_radius_search: 0.03 42 | opts_pc_min_num_neighbors: 3 43 | CAM_CALIB_PATH: "/home/jjiao/catkin_ws/src/localization/ESVO/esvo_core/calib/upenn/left_pinhole.yaml" 44 | PATH_TO_SAVE_RESULT: "/home/jjiao/ESVO_result/upenn/" # CHANGE THIS PATH 45 | KEYFRAME_LINEAR_DIS: 0.15 # m 46 | KEYFRAME_ORIENTATION_DIS: 5 # degree -------------------------------------------------------------------------------- /esvo_core/cfg/mvsmono/mvsmono_rpg_mono.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.2 2 | invDepth_max_range: 2 3 | residual_vis_threshold: 20 4 | stdVar_vis_threshold: 0.05 5 | age_max_range: 10 6 | age_vis_threshold: 1 7 | fusion_radius: 0 8 | FUSION_STRATEGY: "CONST_FRAMES" #"CONST_FRAMES" or "CONST_POINTS" 9 | maxNumFusionFrames: 5 # the near merged keyframes 10 | maxNumFusionPoints: 5000 11 | Denoising: False 12 | SmoothTimeSurface: False 13 | Regularization: False 14 | PROCESS_EVENT_NUM: 10000 15 | TS_HISTORY_LENGTH: 100 16 | mapping_rate_hz: 20 17 | # DepthProblemConfig 18 | patch_size_X: 15 19 | patch_size_Y: 7 20 | LSnorm: Tdist #(Tdist or l2) 21 | Tdist_nu: 2.1897 22 | Tdist_scale: 16.6397 23 | Tdist_stdvar: 56.5347 24 | # Event Matcher (EM) parameters 25 | EM_Slice_Thickness: 0.001 26 | EM_Time_THRESHOLD: 0.0005 27 | EM_EPIPOLAR_THRESHOLD: 1.0 28 | EM_TS_NCC_THRESHOLD: 0.1 29 | EM_NUM_EVENT_MATCHING: 3000 30 | EM_PATCH_INTENSITY_THRESHOLD: 10 31 | EM_PATCH_VALID_RATIO: 0.75 32 | # EventBM parameters 33 | BM_half_slice_thickness: 0.001 34 | BM_min_disparity: 1 35 | BM_max_disparity: 40 36 | BM_step: 1 37 | BM_ZNCC_Threshold: 0.1 38 | BM_bUpDownConfiguration: False 39 | # Publish result parameters 40 | bVisualizeGlobalPC: True 41 | visualizeGPC_interval: 3 42 | NumGPC_added_oper_refresh: 1000 43 | 44 | # MVStereo Mode 45 | MVSMonoMode: 6 46 | # PURE_EVENT_MATCHING //0 EM [26] 47 | # PURE_BLOCK_MATCHING //1 BM 48 | # EM_PLUS_ESTIMATION //2 EM [26] + nonlinear opt. 49 | # BM_PLUS_ESTIMATION //3 ESVO's mapper: BM + nonlinear opt. 50 | # SEMI_GLOBAL_MATCHING //4 [45] 51 | # PURE_EMVS //5 [33] 52 | # PURE_EMVS_PLUS_ESTIMATION //6 [33] + nonlinear opt. 53 | 54 | SAVE_RESULT: False 55 | PATH_TO_SAVE_RESULT: "/home/jjiao/ESVO_result/" # CHANGE THIS PATH 56 | Dataset_Name: "rpg_mono" 57 | Sequence_Name: "dynamic_6dof" 58 | EVENTS_SAVE_MAP: 3000000 59 | 60 | # EMVS Mapping 61 | opts_dim_x: 0 62 | opts_dim_y: 0 63 | opts_dim_z: 100 64 | opts_depth_map_kernal_size: 5 # Size of the Gaussian kernel used for adaptive thresholding" 65 | opts_depth_map_threshold_c: 7 # A value in [0, 255]. The smaller the noisier and more dense reconstruction" 66 | opts_depth_map_median_filter_size: 15 # Size of the median filter used to clean the depth map" 67 | opts_depth_map_contrast_threshold: 1 # Threshold of the contrast to remove outliers in the mask 68 | opts_pc_radius_search: 0.05 69 | opts_pc_min_num_neighbors: 3 70 | opts_mapper_parallex: 5.0 71 | opts_mapper_PatchSize_X: 3 72 | opts_mapper_PatchSize_Y: 3 73 | opts_mapper_TS_score: 1.0 74 | KEYFRAME_LINEAR_DIS: 0.15 # m 75 | KEYFRAME_ORIENTATION_DIS: 5 # degree 76 | KEYFRAME_MEANDEPTH_DIS: 2.0 # percentage 77 | EMVS_Init_event: 100000 #200000 78 | EMVS_Keyframe_event: 2000000 #200000 79 | stdVar_init: 0.05 -------------------------------------------------------------------------------- /esvo_core/cfg/mvsmono/mvsmono_rpg_slider.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.333 2 | invDepth_max_range: 2.127 3 | residual_vis_threshold: 20 4 | stdVar_vis_threshold: 0.05 5 | age_max_range: 10 6 | age_vis_threshold: 1 7 | fusion_radius: 0 8 | FUSION_STRATEGY: "CONST_FRAMES" #"CONST_FRAMES" or "CONST_POINTS" 9 | maxNumFusionFrames: 5 # the near merged keyframes 10 | maxNumFusionPoints: 5000 11 | Denoising: False 12 | SmoothTimeSurface: False 13 | Regularization: False 14 | PROCESS_EVENT_NUM: 100000 15 | TS_HISTORY_LENGTH: 100 16 | mapping_rate_hz: 20 17 | # DepthProblemConfig 18 | patch_size_X: 15 19 | patch_size_Y: 7 20 | LSnorm: Tdist #(Tdist or l2) 21 | Tdist_nu: 2.1897 22 | Tdist_scale: 16.6397 23 | Tdist_stdvar: 56.5347 24 | # Event Matcher (EM) parameters 25 | EM_Slice_Thickness: 0.001 26 | EM_Time_THRESHOLD: 0.0005 27 | EM_EPIPOLAR_THRESHOLD: 1.0 28 | EM_TS_NCC_THRESHOLD: 0.1 29 | EM_NUM_EVENT_MATCHING: 3000 30 | EM_PATCH_INTENSITY_THRESHOLD: 10 31 | EM_PATCH_VALID_RATIO: 0.75 32 | # EventBM parameters 33 | BM_half_slice_thickness: 0.001 34 | BM_min_disparity: 1 35 | BM_max_disparity: 40 36 | BM_step: 1 37 | BM_ZNCC_Threshold: 0.1 38 | BM_bUpDownConfiguration: False 39 | # Publish result parameters 40 | bVisualizeGlobalPC: True 41 | visualizeGPC_interval: 3 42 | NumGPC_added_oper_refresh: 1000 43 | 44 | # MVStereo Mode 45 | MVSMonoMode: 6 46 | # PURE_EVENT_MATCHING //0 EM [26] 47 | # PURE_BLOCK_MATCHING //1 BM 48 | # EM_PLUS_ESTIMATION //2 EM [26] + nonlinear opt. 49 | # BM_PLUS_ESTIMATION //3 ESVO's mapper: BM + nonlinear opt. 50 | # SEMI_GLOBAL_MATCHING //4 [45] 51 | # PURE_EMVS //5 [33] 52 | # PURE_EMVS_PLUS_ESTIMATION //6 [33] + nonlinear opt. 53 | 54 | SAVE_RESULT: False 55 | PATH_TO_SAVE_RESULT: "/home/jjiao/ESVO_result/rpg_slider/" # CHANGE THIS PATH 56 | Dataset_Name: "rpg_slider" 57 | 58 | # EMVS Mapping 59 | opts_dim_x: 0 60 | opts_dim_y: 0 61 | opts_dim_z: 100 62 | opts_depth_map_kernal_size: 5 # Size of the Gaussian kernel used for adaptive thresholding" 63 | opts_depth_map_threshold_c: 7 # A value in [0, 255]. The smaller the noisier and more dense reconstruction" 64 | opts_depth_map_median_filter_size: 15 # Size of the median filter used to clean the depth map" 65 | opts_depth_map_contrast_threshold: 1 # Threshold of the contrast to remove outliers in the mask 66 | opts_pc_radius_search: 0.05 67 | opts_pc_min_num_neighbors: 3 68 | opts_mapper_parallex: 5.0 69 | opts_mapper_PatchSize_X: 3 70 | opts_mapper_PatchSize_Y: 3 71 | opts_mapper_TS_score: 1.0 72 | KEYFRAME_LINEAR_DIS: 0.15 # m 73 | KEYFRAME_ORIENTATION_DIS: 5 # degree 74 | KEYFRAME_MEANDEPTH_DIS: 0.3 # percentage 75 | EMVS_Init_event: 100000 #200000 76 | EMVS_Keyframe_event: 200000 #200000 77 | stdVar_init: 0.05 -------------------------------------------------------------------------------- /esvo_core/cfg/mvsmono/mvsmono_rpg_stereo.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.2 2 | invDepth_max_range: 2 3 | residual_vis_threshold: 20 4 | stdVar_vis_threshold: 0.05 5 | age_max_range: 10 6 | age_vis_threshold: 1 7 | fusion_radius: 0 8 | FUSION_STRATEGY: "CONST_FRAMES" #"CONST_FRAMES" or "CONST_POINTS" 9 | maxNumFusionFrames: 5 # the near merged keyframes 10 | maxNumFusionPoints: 5000 11 | Denoising: False 12 | SmoothTimeSurface: False 13 | Regularization: True 14 | PROCESS_EVENT_NUM: 10000 15 | TS_HISTORY_LENGTH: 100 16 | mapping_rate_hz: 20 17 | # DepthProblemConfig 18 | patch_size_X: 15 19 | patch_size_Y: 7 20 | LSnorm: Tdist #(Tdist or l2) 21 | Tdist_nu: 2.1897 22 | Tdist_scale: 16.6397 23 | Tdist_stdvar: 56.5347 24 | # Event Matcher (EM) parameters 25 | EM_Slice_Thickness: 0.001 26 | EM_Time_THRESHOLD: 0.0005 27 | EM_EPIPOLAR_THRESHOLD: 1.0 28 | EM_TS_NCC_THRESHOLD: 0.1 29 | EM_NUM_EVENT_MATCHING: 3000 30 | EM_PATCH_INTENSITY_THRESHOLD: 10 31 | EM_PATCH_VALID_RATIO: 0.75 32 | # EventBM parameters 33 | BM_half_slice_thickness: 0.001 34 | BM_min_disparity: 1 35 | BM_max_disparity: 40 36 | BM_step: 1 37 | BM_ZNCC_Threshold: 0.1 38 | BM_bUpDownConfiguration: False 39 | # Publish result parameters 40 | bVisualizeGlobalPC: False 41 | visualizeGPC_interval: 3.0 42 | NumGPC_added_oper_refresh: 1000 43 | 44 | # MVStereo Mode 45 | MVSMonoMode: 6 46 | # PURE_EVENT_MATCHING //0 EM [26] 47 | # PURE_BLOCK_MATCHING //1 BM 48 | # EM_PLUS_ESTIMATION //2 EM [26] + nonlinear opt. 49 | # BM_PLUS_ESTIMATION //3 ESVO's mapper: BM + nonlinear opt. 50 | # SEMI_GLOBAL_MATCHING //4 [45] 51 | # PURE_EMVS //5 [33] 52 | # PURE_EMVS_PLUS_ESTIMATION //6 [33] + nonlinear opt. 53 | 54 | SAVE_RESULT: false 55 | PATH_TO_SAVE_RESULT: "/home/jjiao/ESVO_result/rpg_stereo/" # CHANGE THIS PATH 56 | Dataset_Name: "rpg_stereo" 57 | 58 | # EMVS Mapping 59 | opts_dim_x: 0 60 | opts_dim_y: 0 61 | opts_dim_z: 100 62 | opts_depth_map_kernal_size: 5 # Size of the Gaussian kernel used for adaptive thresholding" 63 | opts_depth_map_threshold_c: 7 # A value in [0, 255]. The smaller the noisier and more dense reconstruction" 64 | opts_depth_map_median_filter_size: 15 # Size of the median filter used to clean the depth map" 65 | opts_depth_map_contrast_threshold: 1 # Threshold of the contrast to remove outliers in the mask 66 | opts_pc_radius_search: 0.03 67 | opts_pc_min_num_neighbors: 3 68 | opts_mapper_parallex: 5.0 69 | opts_mapper_PatchSize_X: 3 70 | opts_mapper_PatchSize_Y: 3 71 | opts_mapper_TS_score: 1.0 72 | KEYFRAME_LINEAR_DIS: 0.15 # m 73 | KEYFRAME_ORIENTATION_DIS: 5 # degree 74 | KEYFRAME_MEANDEPTH_DIS: 0.3 # percentage 75 | EMVS_Init_event: 50000 #200000 76 | EMVS_Keyframe_event: 200000 #200000 77 | stdVar_init: 0.05 -------------------------------------------------------------------------------- /esvo_core/cfg/mvsmono/mvsmono_simu.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.5 2 | invDepth_max_range: 5 3 | Denoising: False 4 | PROCESS_EVENT_NUM: 20000 5 | TS_HISTORY_LENGTH: 100 6 | mapping_rate_hz: 20 7 | # Publish result parameters 8 | bVisualizeGlobalPC: False 9 | visualizeGPC_interval: 3 10 | NumGPC_added_oper_refresh: 1000 11 | 12 | SAVE_RESULT: False 13 | PATH_TO_SAVE_RESULT: "/home/jjiao/ESVO_result/" # CHANGE THIS PATH 14 | Dataset_Name: "simu" 15 | Sequence_Name: "checkerboard_planar" 16 | EVENTS_SAVE_MAP: 5000000 17 | 18 | # EMVS Mapping 19 | opts_dim_x: 0 20 | opts_dim_y: 0 21 | opts_dim_z: 100 22 | opts_depth_map_kernal_size: 5 # Size of the Gaussian kernel used for adaptive thresholding" 23 | opts_depth_map_threshold_c: 7 # A value in [0, 255]. The smaller the noisier and more dense reconstruction" 24 | opts_depth_map_median_filter_size: 15 # Size of the median filter used to clean the depth map" 25 | opts_depth_map_contrast_threshold: 1 # Threshold of the contrast to remove outliers in the mask 26 | opts_pc_radius_search: 0.05 27 | opts_pc_min_num_neighbors: 3 28 | opts_mapper_parallex: 5.0 29 | opts_mapper_PatchSize_X: 3 30 | opts_mapper_PatchSize_Y: 3 31 | opts_mapper_TS_score: 1.0 32 | KEYFRAME_MEANDEPTH_DIS: 1.0 # percentage 33 | EMVS_Init_event: 100000 #200000 34 | EMVS_Keyframe_event: 3000000 #200000 35 | stdVar_init: 0.01 -------------------------------------------------------------------------------- /esvo_core/cfg/mvsmono/mvsmono_upenn.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.16 2 | invDepth_max_range: 1.0 3 | residual_vis_threshold: 20 4 | stdVar_vis_threshold: 0.15 5 | age_max_range: 10 6 | age_vis_threshold: 1 7 | fusion_radius: 0 8 | FUSION_STRATEGY: "CONST_FRAMES" #"CONST_FRAMES" or "CONST_POINTS" 9 | maxNumFusionFrames: 5 10 | maxNumFusionPoints: 3000 11 | Denoising: False 12 | SmoothTimeSurface: False 13 | Regularization: True 14 | PROCESS_EVENT_NUM: 1000 15 | TS_HISTORY_LENGTH: 100 16 | mapping_rate_hz: 20 17 | # DepthProblemConfig 18 | patch_size_X: 15 19 | patch_size_Y: 7 20 | LSnorm: Tdist #(Tdist or l2) 21 | Tdist_nu: 2.182 22 | Tdist_scale: 17.277 23 | Tdist_stdvar: 59.763 24 | # Event Matcher (EM) parameters 25 | EM_Slice_Thickness: 0.001 26 | EM_Time_THRESHOLD: 0.0005 27 | EM_EPIPOLAR_THRESHOLD: 1.0 28 | EM_TS_NCC_THRESHOLD: 0.1 29 | EM_NUM_EVENT_MATCHING: 3000 30 | EM_PATCH_INTENSITY_THRESHOLD: 10 31 | EM_PATCH_VALID_RATIO: 0.75 32 | # EventBM parameters 33 | BM_half_slice_thickness: 0.001 34 | BM_min_disparity: 1 35 | BM_max_disparity: 40 36 | BM_step: 1 37 | BM_ZNCC_Threshold: 0.1 38 | BM_bUpDownConfiguration: False 39 | # Publish result parameters 40 | bVisualizeGlobalPC: False 41 | visualizeGPC_interval: 3.0 42 | NumGPC_added_oper_refresh: 1000 43 | 44 | # MVSMonoMode Mode 45 | MVSMonoMode: 6 46 | # PURE_EVENT_MATCHING //0 EM [26] 47 | # PURE_BLOCK_MATCHING //1 BM 48 | # EM_PLUS_ESTIMATION //2 EM [26] + nonlinear opt. 49 | # BM_PLUS_ESTIMATION //3 ESVO's mapper: BM + nonlinear opt. 50 | # SEMI_GLOBAL_MATCHING //4 [45] 51 | # PURE_EMVS //5 [33] 52 | # PURE_EMVS_PLUS_ESTIMATION //6 [33] + nonlinear opt. 53 | 54 | SAVE_RESULT: false 55 | PATH_TO_SAVE_RESULT: "/home/jjiao/ESVO_result/upenn/" # CHANGE THIS PATH 56 | Dataset_Name: "upenn" 57 | 58 | # EMVS Mapping 59 | opts_dim_x: 0 60 | opts_dim_y: 0 61 | opts_dim_z: 100 62 | opts_depth_map_kernal_size: 5 # Size of the Gaussian kernel used for adaptive thresholding" 63 | opts_depth_map_threshold_c: 7 # A value in [0, 255]. The smaller the noisier and more dense reconstruction" 64 | opts_depth_map_median_filter_size: 15 # Size of the median filter used to clean the depth map" 65 | opts_depth_map_contrast_threshold: 1 # Threshold of the contrast to remove outliers in the mask 66 | opts_pc_radius_search: 0.03 67 | opts_pc_min_num_neighbors: 3 68 | opts_mapper_parallex: 5.0 69 | opts_mapper_PatchSize_X: 3 70 | opts_mapper_PatchSize_Y: 3 71 | opts_mapper_TS_score: 1.0 72 | KEYFRAME_LINEAR_DIS: 0.15 # m 73 | KEYFRAME_ORIENTATION_DIS: 5 # degree 74 | KEYFRAME_MEANDEPTH_DIS: 0.3 # percentage 75 | EMVS_Init_event: 50000 #200000 76 | EMVS_Keyframe_event: 200000 #200000 77 | stdVar_init: 0.05 -------------------------------------------------------------------------------- /esvo_core/cfg/mvstereo/mvstereo_rpg_stereo.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.2 2 | invDepth_max_range: 2 3 | residual_vis_threshold: 20 4 | stdVar_vis_threshold: 0.015 5 | age_max_range: 10 6 | age_vis_threshold: 1 7 | fusion_radius: 0 8 | FUSION_STRATEGY: "CONST_FRAMES" #"CONST_FRAMES" or "CONST_POINTS" 9 | maxNumFusionFrames: 40 10 | maxNumFusionPoints: 5000 11 | Denoising: True 12 | SmoothTimeSurface: False 13 | Regularization: True 14 | PROCESS_EVENT_NUM: 10000 15 | TS_HISTORY_LENGTH: 100 16 | mapping_rate_hz: 20 17 | # DepthProblemConfig 18 | patch_size_X: 15 19 | patch_size_Y: 7 20 | LSnorm: Tdist #(Tdist or l2) 21 | Tdist_nu: 2.1897 22 | Tdist_scale: 16.6397 23 | Tdist_stdvar: 56.5347 24 | # Event Matcher (EM) parameters 25 | EM_Slice_Thickness: 0.001 26 | EM_Time_THRESHOLD: 0.0005 27 | EM_EPIPOLAR_THRESHOLD: 1.0 28 | EM_TS_NCC_THRESHOLD: 0.1 29 | EM_NUM_EVENT_MATCHING: 3000 30 | EM_PATCH_INTENSITY_THRESHOLD: 10 31 | EM_PATCH_VALID_RATIO: 0.75 32 | # EventBM parameters 33 | BM_half_slice_thickness: 0.001 34 | BM_min_disparity: 1 35 | BM_max_disparity: 40 36 | BM_step: 1 37 | BM_ZNCC_Threshold: 0.1 38 | BM_bUpDownConfiguration: False 39 | # MVStereo Mode 40 | MVStereoMode: 3 41 | # PURE_EVENT_MATCHING //0 EM [26] 42 | # PURE_BLOCK_MATCHING //1 BM 43 | # EM_PLUS_ESTIMATION //2 EM [26] + nonlinear opt. 44 | # BM_PLUS_ESTIMATION //3 ESVO's mapper: BM + nonlinear opt. 45 | # SEMI_GLOBAL_MATCHING //4 [45] 46 | 47 | SAVE_RESULT: false 48 | PATH_TO_SAVE_RESULT: "/home/jjiao/ESVO_result/" # CHANGE THIS PATH 49 | Dataset_Name: "rpg_stereo" # rpg_desk, rpg_boxes, rpg_bin 50 | Sequence_Name: "rpg_boxes" 51 | EVENTS_SAVE_MAP: 5000000 52 | -------------------------------------------------------------------------------- /esvo_core/cfg/mvstereo/mvstereo_upenn.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.16 2 | invDepth_max_range: 1.0 3 | residual_vis_threshold: 20 4 | stdVar_vis_threshold: 0.15 5 | age_max_range: 10 6 | age_vis_threshold: 1 7 | fusion_radius: 0 8 | FUSION_STRATEGY: "CONST_POINTS" #"CONST_FRAMES" or "CONST_POINTS" 9 | maxNumFusionFrames: 40 10 | maxNumFusionPoints: 3000 11 | Denoising: False 12 | SmoothTimeSurface: False 13 | Regularization: False 14 | PROCESS_EVENT_NUM: 1000 15 | TS_HISTORY_LENGTH: 100 16 | mapping_rate_hz: 20 17 | # DepthProblemConfig 18 | patch_size_X: 15 19 | patch_size_Y: 7 20 | LSnorm: Tdist #(Tdist or l2) 21 | Tdist_nu: 2.182 22 | Tdist_scale: 17.277 23 | Tdist_stdvar: 59.763 24 | # Event Matcher (EM) parameters 25 | EM_Slice_Thickness: 0.001 26 | EM_Time_THRESHOLD: 0.0005 27 | EM_EPIPOLAR_THRESHOLD: 1.0 28 | EM_TS_NCC_THRESHOLD: 0.1 29 | EM_NUM_EVENT_MATCHING: 3000 30 | EM_PATCH_INTENSITY_THRESHOLD: 10 31 | EM_PATCH_VALID_RATIO: 0.75 32 | # EventBM parameters 33 | BM_half_slice_thickness: 0.001 34 | BM_min_disparity: 1 35 | BM_max_disparity: 40 36 | BM_step: 1 37 | BM_ZNCC_Threshold: 0.1 38 | BM_bUpDownConfiguration: False 39 | # MVStereo Mode 40 | MVStereoMode: 3 41 | # PURE_EVENT_MATCHING //0 EM [26] 42 | # PURE_BLOCK_MATCHING //1 BM 43 | # EM_PLUS_ESTIMATION //2 EM [26] + nonlinear opt. 44 | # BM_PLUS_ESTIMATION //3 ESVO's mapper 45 | # SEMI_GLOBAL_MATCHING //4 [45] -------------------------------------------------------------------------------- /esvo_core/cfg/time_surface/ts_parameters.yaml: -------------------------------------------------------------------------------- 1 | use_sim_time: True 2 | ignore_polarity: True 3 | time_surface_mode: 0 # 0: Backward; 1: Forward 4 | decay_ms: 30 5 | median_blur_kernel_size: 1 6 | max_event_queue_len: 20 -------------------------------------------------------------------------------- /esvo_core/cfg/tracking/tracking_hkust.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.25 2 | invDepth_max_range: 2 3 | TS_HISTORY_LENGTH: 100 4 | REF_HISTORY_LENGTH: 10 5 | tracking_rate_hz: 100 6 | patch_size_X: 1 7 | patch_size_Y: 1 8 | kernelSize: 5 9 | MAX_REGISTRATION_POINTS: 2000 10 | BATCH_SIZE: 500 11 | MAX_ITERATION: 10 12 | LSnorm: Huber #l2, Huber 13 | huber_threshold: 50 14 | MIN_NUM_EVENTS: 1000 15 | RegProblemType: 1 #( 0 numerical; 1 analytical) 16 | SAVE_TRAJECTORY: False 17 | PATH_TO_SAVE_TRAJECTORY: "/home/zhouyi/Desktop/ESVO_result/hkust_lab/" # CHANGE THIS PATH 18 | VISUALIZE_TRAJECTORY: True -------------------------------------------------------------------------------- /esvo_core/cfg/tracking/tracking_rpg_mono.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.5 2 | invDepth_max_range: 5 3 | TS_HISTORY_LENGTH: 100 4 | REF_HISTORY_LENGTH: 10 5 | tracking_rate_hz: 100 6 | patch_size_X: 1 7 | patch_size_Y: 1 8 | kernelSize: 5 9 | MAX_REGISTRATION_POINTS: 2000 10 | BATCH_SIZE: 300 11 | MAX_ITERATION: 10 12 | LSnorm: Huber #l2, Huber 13 | huber_threshold: 50 14 | MIN_NUM_EVENTS: 1000 15 | RegProblemType: 1 #( 0 numerical; 1 analytical) 16 | SAVE_TRAJECTORY: False 17 | PATH_TO_SAVE_TRAJECTORY: "/home/jjiao/ESVO_result/rpg_simu/" # CHANGE THIS PATH 18 | VISUALIZE_TRAJECTORY: True 19 | DATASET_NAME: "rpg_simu" -------------------------------------------------------------------------------- /esvo_core/cfg/tracking/tracking_rpg_stereo.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.2 2 | invDepth_max_range: 2.0 3 | TS_HISTORY_LENGTH: 100 4 | REF_HISTORY_LENGTH: 10 5 | tracking_rate_hz: 100 6 | patch_size_X: 1 7 | patch_size_Y: 1 8 | kernelSize: 5 9 | MAX_REGISTRATION_POINTS: 2000 10 | BATCH_SIZE: 300 11 | MAX_ITERATION: 10 12 | LSnorm: Huber #l2, Huber 13 | huber_threshold: 50 14 | MIN_NUM_EVENTS: 1000 15 | RegProblemType: 1 #( 0 numerical; 1 analytical) 16 | SAVE_TRAJECTORY: True 17 | PATH_TO_SAVE_TRAJECTORY: "/home/jjiao/ESVO_result/" # CHANGE THIS PATH 18 | VISUALIZE_TRAJECTORY: True 19 | -------------------------------------------------------------------------------- /esvo_core/cfg/tracking/tracking_simu.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.5 2 | invDepth_max_range: 2.0 3 | TS_HISTORY_LENGTH: 100 4 | REF_HISTORY_LENGTH: 10 5 | tracking_rate_hz: 100 6 | patch_size_X: 1 7 | patch_size_Y: 1 8 | kernelSize: 5 9 | MAX_REGISTRATION_POINTS: 2000 10 | BATCH_SIZE: 300 11 | MAX_ITERATION: 10 12 | LSnorm: Huber #l2, Huber 13 | huber_threshold: 50 14 | MIN_NUM_EVENTS: 1000 15 | RegProblemType: 1 #( 0 numerical; 1 analytical) 16 | SAVE_TRAJECTORY: True 17 | PATH_TO_SAVE_TRAJECTORY: "/home/jjiao/ESVO_result/" # CHANGE THIS PATH 18 | VISUALIZE_TRAJECTORY: True 19 | -------------------------------------------------------------------------------- /esvo_core/cfg/tracking/tracking_upenn.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.16 2 | invDepth_max_range: 1.0 3 | TS_HISTORY_LENGTH: 100 4 | REF_HISTORY_LENGTH: 10 5 | tracking_rate_hz: 100 6 | patch_size_X: 1 7 | patch_size_Y: 1 8 | kernelSize: 5 9 | MAX_REGISTRATION_POINTS: 2000 10 | BATCH_SIZE: 300 11 | MAX_ITERATION: 10 12 | LSnorm: Huber #Huber #l2 13 | huber_threshold: 50 14 | MIN_NUM_EVENTS: 1000 15 | RegProblemType: 1 #( 0 numerical; 1 analytical) 16 | SAVE_TRAJECTORY: True 17 | PATH_TO_SAVE_TRAJECTORY: "/home/jjiao/ESVO_result/" # CHANGE THIS PATH 18 | VISUALIZE_TRAJECTORY: True -------------------------------------------------------------------------------- /esvo_core/include/emvs_core/MedianFilter.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MEDIAN_FILTER_HPP_ 2 | #define MEDIAN_FILTER_HPP_ 3 | 4 | #include 5 | 6 | 7 | void huangMedianFilter(const cv::Mat& img, cv::Mat& out_img, const cv::Mat& mask, int patch_size); 8 | 9 | #endif -------------------------------------------------------------------------------- /esvo_core/include/emvs_core/Trajectory.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TRAJECTORY_HPP_ 2 | #define TRAJECTORY_HPP_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | // #define TRAJECTORY_LOG 13 | 14 | namespace EMVS 15 | { 16 | typedef std::map PoseMap; 17 | 18 | // return the first iterator which is greater than t 19 | inline PoseMap::const_iterator PoseMap_lower_bound( 20 | const PoseMap &stm, const ros::Time &t) 21 | { 22 | return std::lower_bound(stm.begin(), stm.end(), t, 23 | [](const std::pair &st, const ros::Time &t) { return st.first.toSec() < t.toSec(); }); 24 | } 25 | 26 | template 27 | class Trajectory 28 | { 29 | public: 30 | DerivedTrajectory &derived() 31 | { 32 | return static_cast(*this); 33 | } 34 | 35 | Trajectory() {} 36 | 37 | // Returns T_W_C (mapping points from C to the world frame W) 38 | bool getPoseAt(const PoseMap &pose_map, const ros::Time &t, Eigen::Matrix4d &T) const 39 | { 40 | return derived().getPoseAt(t, T); 41 | } 42 | 43 | void getFirstControlPose(const PoseMap &pose_map, Eigen::Matrix4d *T, ros::Time *t) const 44 | { 45 | *t = pose_map.begin()->first; 46 | *T = pose_map.begin()->second; 47 | } 48 | 49 | void getLastControlPose(const PoseMap &pose_map, Eigen::Matrix4d *T, ros::Time *t) const 50 | { 51 | *t = pose_map.rbegin()->first; 52 | *T = pose_map.rbegin()->second; 53 | } 54 | 55 | size_t getNumControlPoses(const PoseMap &pose_map) const 56 | { 57 | return pose_map.size(); 58 | } 59 | 60 | bool printAllPoses(const PoseMap &pose_map) const 61 | { 62 | size_t control_pose_idx = 0u; 63 | for (auto it : pose_map) 64 | { 65 | std::cout << "--Control pose #" << control_pose_idx++ << ". time = " << it.first << std::endl; 66 | std::cout << "--T = " << std::endl; 67 | std::cout << it.second << std::endl; 68 | } 69 | return true; 70 | } 71 | }; 72 | 73 | class LinearTrajectory : public Trajectory 74 | { 75 | public: 76 | LinearTrajectory() : Trajectory() {} 77 | 78 | bool getPoseAt(const PoseMap &pose_map, const ros::Time &t, Eigen::Matrix4d &T) const 79 | { 80 | ros::Time t0_, t1_; 81 | Eigen::Matrix4d T0_, T1_; 82 | 83 | // Check if it is between two known poses 84 | auto it1 = PoseMap_lower_bound(pose_map, t); 85 | if (it1 == pose_map.begin()) 86 | { 87 | #ifdef TRAJECTORY_LOG 88 | std::cout << "Cannot extrapolate in the past. Requested pose: " 89 | << t << " but the earliest pose available is at time: " 90 | << pose_map.begin()->first << std::endl; 91 | #endif 92 | return false; 93 | } 94 | else if (it1 == pose_map.end()) 95 | { 96 | #ifdef TRAJECTORY_LOG 97 | std::cout << "Cannot extrapolate in the future. Requested pose: " 98 | << t << " but the latest pose available is at time: " 99 | << (pose_map.rbegin())->first << std::endl 100 | #endif 101 | return false; 102 | } 103 | else 104 | { 105 | auto it0 = std::prev(it1); 106 | t0_ = (it0)->first; 107 | T0_ = (it0)->second; 108 | t1_ = (it1)->first; 109 | T1_ = (it1)->second; 110 | #ifdef TRAJECTORY_LOG 111 | printf("interpolation: %f < %f < %f\n\n", it0->first.toSec(), t.toSec(), it1->first.toSec()); 112 | #endif 113 | } 114 | 115 | // Linear interpolation in SE(3) 116 | const Eigen::Matrix4d T_relative = T0_.inverse() * T1_; 117 | const double delta_t = (t - t0_).toSec() / (t1_ - t0_).toSec(); 118 | T = T0_ * getLinearInterpolation(T_relative, delta_t); 119 | return true; 120 | } 121 | 122 | Eigen::Matrix4d getLinearInterpolation(const Eigen::Matrix4d &T_relative, const double &delta_t) const 123 | { 124 | Eigen::Matrix3d R_relative = T_relative.topLeftCorner<3, 3>(); 125 | Eigen::Quaterniond q_relative(R_relative); 126 | 127 | Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); 128 | T.topLeftCorner<3, 3>() = q_relative.slerp(delta_t, q_relative).toRotationMatrix(); 129 | T.topRightCorner<3, 1>() = delta_t * T_relative.topRightCorner<3, 1>(); 130 | return T; 131 | } 132 | }; 133 | } // namespace EMVS 134 | 135 | #endif -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/container/CameraSystem.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CONTAINER_CAMERASYSTEM_H 2 | #define ESVO_CORE_CONTAINER_CAMERASYSTEM_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace esvo_core 11 | { 12 | namespace container 13 | { 14 | class PerspectiveCamera 15 | { 16 | public: 17 | PerspectiveCamera(); 18 | virtual ~PerspectiveCamera(); 19 | using Ptr = std::shared_ptr; 20 | 21 | void setIntrinsicParameters( 22 | size_t width, size_t height, 23 | std::string &cameraName, 24 | std::string &distortion_model, 25 | std::vector &vD, 26 | std::vector &vK, 27 | std::vector &vRectMat, 28 | std::vector &vP); 29 | 30 | void preComputeRectifiedCoordinate(); 31 | 32 | Eigen::Matrix getRectifiedUndistortedCoordinate(int xcoor, int ycoor); 33 | 34 | void cam2World(const Eigen::Vector2d &x, double invDepth, Eigen::Vector3d &p); 35 | 36 | void world2Cam(const Eigen::Vector3d &p, Eigen::Vector2d &x); 37 | 38 | bool isValidPixel(const Eigen::Vector2d &p); 39 | 40 | public: 41 | size_t width_, height_; 42 | std::string cameraName_; 43 | std::string distortion_model_; 44 | Eigen::Matrix D_; 45 | Eigen::Matrix3d K_; 46 | Eigen::Matrix3d RectMat_; 47 | Eigen::Matrix P_; 48 | 49 | Eigen::Matrix2Xd precomputed_rectified_points_; 50 | cv::Mat undistort_map1_, undistort_map2_; 51 | Eigen::MatrixXi UndistortRectify_mask_; 52 | }; 53 | 54 | class CameraSystem 55 | { 56 | public: 57 | CameraSystem(const std::string &calibInfoDir, bool bPrintCalibInfo = false); 58 | virtual ~CameraSystem(); 59 | using Ptr = std::shared_ptr; 60 | 61 | void computeBaseline(); 62 | void loadCalibInfo(const std::string &cameraSystemDir, bool bPrintCalibInfo = false); 63 | void printCalibInfo(); 64 | 65 | PerspectiveCamera::Ptr cam_left_ptr_, cam_right_ptr_; // intrinsics 66 | Eigen::Matrix T_right_left_; // extrinsics 67 | double baseline_; 68 | }; 69 | } // namespace container 70 | } // namespace esvo_core 71 | 72 | #endif //ESVO_CORE_CONTAINER_CAMERASYSTEM_H -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/container/CameraSystemMono.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CONTAINER_CAMERASYSTEM_MONO_H 2 | #define ESVO_CORE_CONTAINER_CAMERASYSTEM_MONO_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace esvo_core 11 | { 12 | namespace container 13 | { 14 | class PerspectiveCamera 15 | { 16 | public: 17 | PerspectiveCamera(); 18 | virtual ~PerspectiveCamera(); 19 | using Ptr = std::shared_ptr; 20 | 21 | void setIntrinsicParameters( 22 | size_t width, size_t height, 23 | std::string &cameraName, 24 | std::string &distortion_model, 25 | std::vector &vD, 26 | std::vector &vK, 27 | std::vector &vRectMat, 28 | std::vector &vP); 29 | 30 | void preComputeRectifiedCoordinate(); 31 | 32 | Eigen::Matrix getRectifiedUndistortedCoordinate(int xcoor, int ycoor); 33 | 34 | void cam2World(const Eigen::Vector2d &x, double invDepth, Eigen::Vector3d &p); 35 | 36 | void world2Cam(const Eigen::Vector3d &p, Eigen::Vector2d &x); 37 | 38 | bool isValidPixel(const Eigen::Vector2d &p); 39 | 40 | public: 41 | size_t width_, height_; 42 | std::string cameraName_; 43 | std::string distortion_model_; 44 | Eigen::Matrix D_; 45 | Eigen::Matrix3d K_; 46 | Eigen::Matrix3d RectMat_; 47 | Eigen::Matrix P_; 48 | 49 | Eigen::Matrix2Xd precomputed_rectified_points_; 50 | cv::Mat undistort_map1_, undistort_map2_; 51 | Eigen::MatrixXi UndistortRectify_mask_; 52 | }; 53 | 54 | class CameraSystem 55 | { 56 | public: 57 | CameraSystem(const std::string &calibInfoDir, bool bPrintCalibInfo = false); 58 | virtual ~CameraSystem(); 59 | using Ptr = std::shared_ptr; 60 | 61 | void loadCalibInfo(const std::string &cameraSystemDir, bool bPrintCalibInfo = false); 62 | void printCalibInfo(); 63 | 64 | PerspectiveCamera::Ptr cam_left_ptr_; // intrinsics 65 | }; 66 | } // namespace container 67 | } // namespace esvo_core 68 | 69 | #endif //ESVO_CORE_CONTAINER_CAMERASYSTEM_MONO_H -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/container/DepthMap.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_DEPTHMAP_H 2 | #define ESVO_CORE_DEPTHMAP_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace esvo_core 9 | { 10 | using namespace tools; 11 | namespace container 12 | { 13 | using DepthMap = SmartGrid; // map points are stored in a grid, a container 14 | 15 | struct DepthFrame 16 | { 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | typedef std::shared_ptr Ptr; 19 | 20 | DepthFrame(size_t row, size_t col) 21 | { 22 | dMap_ = std::make_shared(row, col); 23 | id_ = 0; 24 | T_world_frame_.setIdentity(); 25 | } 26 | 27 | void setId(size_t id) 28 | { 29 | id_ = id; 30 | } 31 | 32 | void setTransformation(Transformation &T_world_frame) 33 | { 34 | T_world_frame_ = T_world_frame; 35 | } 36 | 37 | void clear() 38 | { 39 | dMap_->reset(); 40 | id_ = 0; 41 | T_world_frame_.setIdentity(); 42 | } 43 | 44 | DepthMap::Ptr dMap_; 45 | size_t id_; 46 | Transformation T_world_frame_; 47 | }; 48 | } 49 | } 50 | #endif //ESVO_CORE_DEPTHMAP_H 51 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/container/DepthPoint.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CONTAINER_DEPTHPOINT_H 2 | #define ESVO_CORE_CONTAINER_DEPTHPOINT_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace esvo_core 9 | { 10 | namespace container 11 | { 12 | class DepthPoint 13 | { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | typedef std::shared_ptr Ptr; 17 | 18 | DepthPoint(); 19 | 20 | DepthPoint(size_t row, size_t col); 21 | virtual ~DepthPoint(); 22 | 23 | size_t row() const; 24 | size_t col() const; 25 | 26 | void update_x(const Eigen::Vector2d &x); 27 | const Eigen::Vector2d &x() const; 28 | 29 | double &invDepth(); 30 | const double &invDepth() const; 31 | 32 | double &scaleSquared(); 33 | const double &scaleSquared() const; 34 | 35 | double &nu(); 36 | const double &nu() const; 37 | 38 | double &variance(); 39 | const double &variance() const; 40 | 41 | double &confidence(); 42 | const double &confidence() const; 43 | 44 | double &residual(); 45 | const double &residual() const; 46 | 47 | size_t &age(); 48 | const size_t &age() const; 49 | 50 | void boundVariance(); 51 | 52 | void update(double invDepth, double variance); // Gaussian distribution 53 | void update_studentT(double invDepth, double scale2, double variance, double nu); // T distribution 54 | void update_confidence(double invDepth, double confidence); 55 | 56 | void update_p_cam(const Eigen::Vector3d &p); 57 | const Eigen::Vector3d &p_cam() const; 58 | 59 | void updatePose(const Eigen::Matrix &T_world_cam); // used in the fusion of each newly estimate. 60 | // Therefore, it is not necessary to call updatePose for those created in the fusion. Because those share 61 | // the pose of the fused depthFrame. 62 | 63 | const Eigen::Matrix &T_world_cam() const; 64 | 65 | bool valid() const; 66 | bool valid(double var_threshold, 67 | double age_threshold, 68 | double invDepth_max, 69 | double invDepth_min) const; 70 | 71 | //copy an element without the location 72 | void copy(const DepthPoint ©); 73 | 74 | private: 75 | //coordinates in the image 76 | size_t row_; 77 | size_t col_; 78 | Eigen::Vector2d x_; 79 | 80 | //inverse depth parameters 81 | double invDepth_; 82 | double scaleSquared_; // squared scale 83 | double nu_; 84 | double variance_; 85 | double residual_; 86 | double confidence_; 87 | 88 | // count the number of fusion has been applied on a depth point 89 | size_t age_; 90 | 91 | //3D point (updated in reference frame before tracking) 92 | Eigen::Vector3d p_cam_; 93 | Eigen::Matrix T_world_cam_; 94 | }; 95 | } // namespace container 96 | } // namespace esvo_core 97 | 98 | #endif //ESVO_CORE_CONTAINER_DEPTHPOINT_H -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/container/EventMatchPair.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CORE_EVENTMATCHPAIR_H 2 | #define ESVO_CORE_CORE_EVENTMATCHPAIR_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace esvo_core 11 | { 12 | using namespace container; 13 | using namespace tools; 14 | namespace core 15 | { 16 | struct EventMatchPair 17 | { 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | 20 | EventMatchPair() {} 21 | 22 | // raw event coordinate 23 | Eigen::Vector2d x_left_raw_; 24 | // rectified_event coordinate (left, right) 25 | Eigen::Vector2d x_left_, x_right_; 26 | // timestamp 27 | ros::Time t_; 28 | // pose of virtual view (T_world_virtual) 29 | Transformation trans_; 30 | // inverse depth 31 | double invDepth_; 32 | // match cost 33 | double cost_; 34 | // gradient (left) 35 | double gx_, gy_; 36 | // disparity 37 | double disp_; 38 | }; 39 | } 40 | } 41 | 42 | #endif //ESVO_CORE_CORE_EVENTMATCHPAIR_H 43 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/container/ResidualItem.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CONTAINER_RESIDUALITEM_H 2 | #define ESVO_CORE_CONTAINER_RESIDUALITEM_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace esvo_core 9 | { 10 | namespace container 11 | { 12 | struct ResidualItem 13 | { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | Eigen::Vector3d p_;// 3D coordinate in the reference frame 17 | Eigen::Vector2d p_img_;// 2D coordinate in the image plane 18 | 19 | // double IRLS_weight_; 20 | // double variance_; 21 | Eigen::VectorXd residual_; 22 | // bool bOutlier_; 23 | 24 | ResidualItem(); 25 | ResidualItem(const double x,const double y,const double z); 26 | void initialize(const double x,const double y,const double z); 27 | }; 28 | 29 | using ResidualItems = std::vector >; 30 | }// namespace container 31 | }// namespace esvo_core 32 | 33 | #endif //ESVO_CORE_CONTAINER_RESIDUALITEM_H 34 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/core/DepthFusion.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CORE_DEPTHFUSION_H 2 | #define ESVO_CORE_CORE_DEPTHFUSION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace esvo_core 12 | { 13 | using namespace container; 14 | namespace core 15 | { 16 | class DepthFusion 17 | { 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | 21 | DepthFusion( 22 | CameraSystem::Ptr &camSysPtr, 23 | std::shared_ptr & dpConfigPtr); 24 | 25 | virtual ~DepthFusion(); 26 | 27 | bool propagate_one_point( 28 | DepthPoint &dp_prior, 29 | DepthPoint &dp_prop, 30 | Eigen::Matrix &T_prop_prior); 31 | 32 | int fusion( 33 | DepthPoint &dp_prop, 34 | DepthMap::Ptr &dm, 35 | int fusion_radius = 0); 36 | 37 | int update( 38 | std::vector &dp_obs, 39 | DepthFrame::Ptr &df, 40 | int fusion_radius = 0); 41 | 42 | bool boundaryCheck( 43 | double xcoor, 44 | double ycoor, 45 | size_t width, 46 | size_t height); 47 | 48 | bool chiSquareTest( 49 | double invD1, double invD2, 50 | double var1, double var2); 51 | 52 | bool studentTCompatibleTest( 53 | double invD1, double invD2, 54 | double var1, double var2); 55 | 56 | // Used by GTL and SGM for comparison, and also used in the initialization. 57 | void naive_propagation( 58 | std::vector &dp_obs, 59 | DepthFrame::Ptr &df); 60 | 61 | bool naive_propagate_one_point( 62 | DepthPoint &dp_prior, 63 | DepthPoint &dp_prop, 64 | Eigen::Matrix &T_prop_prior); 65 | 66 | private: 67 | CameraSystem::Ptr camSysPtr_; 68 | std::shared_ptr dpConfigPtr_; 69 | }; 70 | } 71 | } 72 | 73 | #endif //ESVO_CORE_CORE_DEPTHFUSION_H -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/core/DepthProblem.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CORE_DEPTHPROBLEM_H 2 | #define ESVO_CORE_CORE_DEPTHPROBLEM_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace esvo_core 11 | { 12 | using namespace container; 13 | using namespace tools; 14 | namespace core 15 | { 16 | struct DepthProblem : public optimization::OptimizationFunctor 17 | { 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | DepthProblem( 20 | const DepthProblemConfig::Ptr &dpConfig_ptr, 21 | const CameraSystem::Ptr &camSysPtr); 22 | 23 | void setProblem( 24 | Eigen::Vector2d &coor, 25 | Eigen::Matrix &T_world_virtual, 26 | StampedTimeSurfaceObs *pStampedTsObs); 27 | 28 | // function that is inherited from optimization::OptimizationFunctor 29 | int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const; 30 | 31 | // utils 32 | bool warping( 33 | const Eigen::Vector2d &x, 34 | double d, 35 | const Eigen::Matrix &T_left_virtual, 36 | Eigen::Vector2d &x1_s, 37 | Eigen::Vector2d &x2_s) const; 38 | 39 | bool patchInterpolation( 40 | const Eigen::MatrixXd &img, 41 | const Eigen::Vector2d &location, 42 | Eigen::MatrixXd &patch, 43 | bool debug = false) const; 44 | 45 | // variables 46 | CameraSystem::Ptr camSysPtr_; 47 | DepthProblemConfig::Ptr dpConfigPtr_; 48 | Eigen::Vector2d coordinate_; 49 | Eigen::Matrix T_world_virtual_; 50 | std::vector, 51 | Eigen::aligned_allocator>> 52 | vT_left_virtual_; 53 | StampedTimeSurfaceObs *pStampedTsObs_; 54 | }; 55 | } // namespace core 56 | } // namespace esvo_core 57 | 58 | #endif //ESVO_CORE_CORE_DEPTHPROBLEM_H 59 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/core/DepthProblemConfig.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CORE_DEPTHPROBLEM_CONFIG_H 2 | #define ESVO_CORE_CORE_DEPTHPROBLEM_CONFIG_H 3 | 4 | namespace esvo_core 5 | { 6 | namespace core 7 | { 8 | struct DepthProblemConfig 9 | { 10 | using Ptr = std::shared_ptr; 11 | DepthProblemConfig( 12 | size_t patchSize_X, 13 | size_t patchSize_Y, 14 | const std::string &LSnorm, 15 | double td_nu, 16 | double td_scale, 17 | const size_t MAX_ITERATION = 10) 18 | : patchSize_X_(patchSize_X), 19 | patchSize_Y_(patchSize_Y), 20 | LSnorm_(LSnorm), 21 | Noise_Model_(std::string("Gaussian")), 22 | td_nu_(td_nu), 23 | td_scale_(td_scale), 24 | td_scaleSquared_(pow(td_scale, 2)), 25 | td_stdvar_(sqrt(td_nu / (td_nu - 2) * td_scaleSquared_)), 26 | MAX_ITERATION_(MAX_ITERATION) {} 27 | 28 | size_t patchSize_X_, patchSize_Y_; 29 | std::string LSnorm_; 30 | std::string Noise_Model_; 31 | double td_nu_; 32 | double td_scale_; 33 | double td_scaleSquared_; // td_scale_^2 34 | double td_stdvar_; // sigma 35 | size_t MAX_ITERATION_; 36 | }; 37 | } // namespace core 38 | } // namespace esvo_core 39 | 40 | #endif -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/core/DepthProblemSolver.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CORE_DEPTHPROBLEMSOLVER2_H 2 | #define ESVO_CORE_CORE_DEPTHPROBLEMSOLVER2_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace esvo_core 12 | { 13 | namespace core 14 | { 15 | enum DepthProblemType 16 | { 17 | ANALYTICAL, 18 | NUMERICAL 19 | }; 20 | class DepthProblemSolver 21 | { 22 | struct Job 23 | { 24 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 25 | size_t i_thread_; 26 | std::vector *pvEMP_; 27 | StampedTimeSurfaceObs* pStamped_TS_obs_; 28 | std::shared_ptr dProblemPtr_; 29 | std::shared_ptr< Eigen::NumericalDiff > numDiff_dProblemPtr_; 30 | std::shared_ptr< std::vector > vdpPtr_; 31 | }; 32 | 33 | public: 34 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 35 | DepthProblemSolver( 36 | CameraSystem::Ptr & camSysPtr, 37 | std::shared_ptr & dpConfigPtr, 38 | DepthProblemType dpType = NUMERICAL, // we only provide the numerical (jacobian) solver here, because the analytical one will not show remarkable efficiency. 39 | size_t numThread = 1); 40 | 41 | virtual ~DepthProblemSolver(); 42 | 43 | void solve( 44 | std::vector* pvEMP, 45 | StampedTimeSurfaceObs* pStampedTsObs, 46 | std::vector &vdp ); 47 | 48 | void solve_multiple_problems(Job &job); 49 | 50 | bool solve_single_problem_numerical( 51 | double d_init, 52 | std::shared_ptr< Eigen::NumericalDiff > & dProblemPtr, 53 | double* result); 54 | 55 | void pointCulling( 56 | std::vector &vdp, 57 | double std_variance_threshold, 58 | double cost_threshold, 59 | double invDepth_min_range, 60 | double invDepth_max_range); 61 | 62 | DepthProblemType getProblemType(); 63 | 64 | private: 65 | CameraSystem::Ptr & camSysPtr_; 66 | std::shared_ptr dpConfigPtr_; 67 | size_t NUM_THREAD_; 68 | DepthProblemType dpType_; 69 | }; 70 | } 71 | } 72 | #endif //ESVO_CORE_CORE_DEPTHPROBLEMSOLVER2_H -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/core/DepthRegularization.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CORE_DEPTHREGULARIZATION_H 2 | #define ESVO_CORE_CORE_DEPTHREGULARIZATION_H 3 | 4 | #include 5 | #include 6 | #include 7 | namespace esvo_core 8 | { 9 | using namespace container; 10 | namespace core 11 | { 12 | class DepthRegularization 13 | { 14 | public: 15 | typedef std::shared_ptr Ptr; 16 | 17 | DepthRegularization(std::shared_ptr & dpConfigPtr); 18 | virtual ~DepthRegularization(); 19 | 20 | void apply( DepthMap::Ptr & depthMapPtr ); 21 | 22 | private: 23 | std::shared_ptr dpConfigPtr_; 24 | size_t _regularizationRadius; 25 | size_t _regularizationMinNeighbours; 26 | size_t _regularizationMinCloseNeighbours; 27 | }; 28 | }// core 29 | }// esvo_core 30 | 31 | #endif //ESVO_CORE_CORE_DEPTHREGULARIZATION_H -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/core/EventBM.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CORE_EVENTBM_H 2 | #define ESVO_CORE_CORE_EVENTBM_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace esvo_core 11 | { 12 | using namespace tools; 13 | using namespace container; 14 | namespace core 15 | { 16 | class EventBM 17 | { 18 | struct Job 19 | { 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 21 | size_t i_thread_; 22 | std::vector* pvEventPtr_; 23 | std::vector >* pvpDisparitySearchBound_; 24 | std::shared_ptr > pvEventMatchPair_; 25 | }; 26 | public: 27 | EventBM( 28 | CameraSystem::Ptr camSysPtr, 29 | size_t numThread = 1, 30 | bool bSmoothTS = false, 31 | size_t patch_size_X = 25, 32 | size_t patch_size_Y = 25, 33 | size_t min_disparity = 1, 34 | size_t max_disparity = 40, 35 | size_t step = 1, 36 | double ZNCC_Threshold = 0.1, 37 | bool bUpDownConfiguration = false);// bUpDownConfiguration determines the epipolar searching direction (UpDown or LeftRight). 38 | virtual ~EventBM(); 39 | 40 | void resetParameters( 41 | size_t patch_size_X, 42 | size_t patch_size_Y, 43 | size_t min_disparity, 44 | size_t max_disparity, 45 | size_t step, 46 | double ZNCC_Threshold, 47 | bool bDonwUpConfiguration); 48 | 49 | void createMatchProblem( 50 | StampedTimeSurfaceObs * pStampedTsObs, 51 | StampTransformationMap * pSt_map, 52 | std::vector* pvEventsPtr); 53 | 54 | bool match_an_event( 55 | dvs_msgs::Event *pEvent, 56 | std::pair& pDisparityBound, 57 | EventMatchPair &emPair); 58 | 59 | void match_all_SingleThread(std::vector &vEMP); 60 | void match_all_HyperThread(std::vector &vEMP); 61 | 62 | static double zncc_cost(Eigen::MatrixXd &patch_left, Eigen::MatrixXd &patch_right, bool normalized = false); 63 | 64 | private: 65 | void match(EventBM::Job &job); 66 | bool epipolarSearching(double& min_cost, Eigen::Vector2i& bestMatch, size_t& bestDisp, Eigen::MatrixXd& patch_dst, 67 | size_t searching_start_pos, size_t searching_end_pos, size_t searching_step, 68 | Eigen::Vector2i& x1, Eigen::MatrixXd& patch_src, bool bDownUpConfiguration = false); 69 | bool isValidPatch(Eigen::Vector2i& x, Eigen::Vector2i& left_top); 70 | private: 71 | CameraSystem::Ptr camSysPtr_; 72 | StampedTimeSurfaceObs* pStampedTsObs_; 73 | StampTransformationMap * pSt_map_; 74 | std::vector vEventsPtr_; 75 | std::vector > vpDisparitySearchBound_; 76 | Sobel sb_; 77 | 78 | size_t NUM_THREAD_; 79 | bool bSmoothTS_; 80 | size_t patch_size_X_; 81 | size_t patch_size_Y_; 82 | size_t min_disparity_; 83 | size_t max_disparity_; 84 | size_t step_; 85 | double ZNCC_Threshold_; 86 | double ZNCC_MAX_; 87 | bool bUpDownConfiguration_; 88 | 89 | size_t coarseSearchingFailNum_, fineSearchingFailNum_, infoNoiseRatioLowNum_; 90 | }; 91 | }// core 92 | }// esvo_core 93 | 94 | #endif //ESVO_CORE_CORE_EVENTBM_H 95 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/core/EventMatcher.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_EVENTMATCHER_H 2 | #define ESVO_CORE_EVENTMATCHER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace esvo_core 11 | { 12 | using namespace container; 13 | using namespace tools; 14 | namespace core 15 | { 16 | struct EventSlice 17 | { 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | EventSlice(double SLICE_THICKNESS = 2 * 1e-3) 20 | :SLICE_THICKNESS_(SLICE_THICKNESS){} 21 | 22 | size_t numEvents_; 23 | double SLICE_THICKNESS_; 24 | ros::Time t_median_;// used for Transformation iterpolation 25 | tools::Transformation transf_; 26 | std::vector::iterator it_begin_, it_end_; 27 | }; 28 | 29 | class EventMatcher 30 | { 31 | struct Job 32 | { 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 34 | size_t i_thread_; 35 | std::vector::iterator vEventPtr_it_begin_; 36 | std::vector* pvEventSlice_; 37 | std::vector* pvIndexEventSlice_; 38 | std::shared_ptr< std::vector > pvEventMatchPair_; 39 | }; 40 | public: 41 | EventMatcher( 42 | CameraSystem::Ptr camSysPtr, 43 | size_t numThread = 1, 44 | double Time_THRESHOLD = 10e-5,// 100us 45 | double EPIPOLAR_THRESHOLD = 0.5, 46 | double TS_NCC_THRESHOLD = 0.1, 47 | size_t patch_size_X = 25, 48 | size_t patch_size_Y = 5, 49 | size_t patch_intensity_threshold = 125, 50 | double patch_valid_ratio = 0.1); 51 | virtual ~EventMatcher(); 52 | 53 | void resetParameters( 54 | double Time_THRESHOLD, 55 | double EPIPOLAR_THRESHOLD, 56 | double TS_NCC_THRESHOLD, 57 | size_t patch_size_X, 58 | size_t patch_size_Y, 59 | size_t patch_intensity_threshold, 60 | double patch_valid_ratio); 61 | 62 | void createMatchProblem( 63 | StampedTimeSurfaceObs * pTS_obs, 64 | std::vector* vEventSlice_ptr, 65 | std::vector* vEventPtr_cand); 66 | 67 | bool match_an_event( 68 | dvs_msgs::Event* ev_ptr, 69 | Transformation &Trans_world_rv, 70 | EventMatchPair &emPair); 71 | 72 | void match_all_SingleThread(std::vector &vEMP); 73 | void match_all_HyperThread(std::vector &vEMP); 74 | void match(EventMatcher::Job& job); 75 | 76 | double zncc_cost(Eigen::MatrixXd &patch_left, Eigen::MatrixXd &patch_right); 77 | 78 | bool warping2( 79 | const Eigen::Vector2d &x, 80 | double invDepth, 81 | const Eigen::Matrix &T_left_rv, 82 | Eigen::Vector2d &x1_s, 83 | Eigen::Vector2d &x2_s); 84 | 85 | bool patchInterpolation2( 86 | const Eigen::MatrixXd &img, 87 | const Eigen::Vector2d &location, 88 | Eigen::MatrixXd &patch); 89 | private: 90 | container::CameraSystem::Ptr camSysPtr_; 91 | std::vector vEvents_left_itr_, vEvents_right_itr_; 92 | size_t NUM_THREAD_; 93 | double Time_THRESHOLD_, EPIPOLAR_THRESHOLD_, TS_NCC_THRESHOLD_; 94 | size_t patch_size_X_, patch_size_Y_; 95 | size_t patch_intensity_threshold_; 96 | double patch_valid_ratio_; 97 | 98 | StampedTimeSurfaceObs * pTS_obs_; 99 | std::vector* pvEventSlice_; 100 | std::vector* pvCandEventPtr_; 101 | }; 102 | } 103 | } 104 | #endif //ESVO_CORE_EVENTMATCHER_H 105 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/core/RegProblemSolverLM.h: -------------------------------------------------------------------------------- 1 | /** 2 | * LK method turtorial: Lucas-Kanade 20 Years On: A Unifying Framework 3 | **/ 4 | 5 | #ifndef ESVO_CORE_CORE_REGPROBLEMSOLVERLM_H 6 | #define ESVO_CORE_CORE_REGPROBLEMSOLVERLM_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | namespace esvo_core 23 | { 24 | namespace core 25 | { 26 | enum RegProblemType 27 | { 28 | REG_NUMERICAL, 29 | REG_ANALYTICAL 30 | }; 31 | 32 | struct LM_statics 33 | { 34 | size_t nPoints_; 35 | size_t nfev_; 36 | size_t nIter_; 37 | }; 38 | 39 | /** 40 | * @brief Solver for image-to-map registration problem. 41 | */ 42 | class RegProblemSolverLM 43 | { 44 | public: 45 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 46 | RegProblemSolverLM( 47 | CameraSystem::Ptr &camSysPtr, 48 | std::shared_ptr &rpConfigPtr, 49 | RegProblemType rpType = REG_NUMERICAL, 50 | size_t numThread = 1); 51 | virtual ~RegProblemSolverLM(); 52 | 53 | bool resetRegProblem(RefFrame *ref, CurFrame *cur); 54 | bool solve_numerical(); // relatively slower 55 | bool solve_analytical(); // faster 56 | bool evalDegeneracy(RefFrame *ref, CurFrame *cur, double &lambda, const int °enThreshold); 57 | 58 | // For test and visualization 59 | void setRegPublisher(image_transport::Publisher *reprojMap_pub); 60 | LM_statics lmStatics_; // record LevenburgMarquardt log. 61 | 62 | // variables 63 | private: 64 | CameraSystem::Ptr &camSysPtr_; 65 | std::shared_ptr rpConfigPtr_; 66 | size_t NUM_THREAD_; 67 | RegProblemType rpType_; 68 | 69 | std::shared_ptr regProblemPtr_; 70 | std::shared_ptr> numDiff_regProblemPtr_; 71 | 72 | // For test 73 | double z_min_, z_max_; 74 | image_transport::Publisher *reprojMap_pub_; 75 | Visualization visualizor_; 76 | bool bPrint_, bVisualize_; 77 | }; 78 | } //namespace core 79 | } //namespace esvo_core 80 | #endif //ESVO_CORE_CORE_REGPROBLEMSOLVER2_H -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/esvo_Tracking.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_TRACKING_H 2 | #define ESVO_CORE_TRACKING_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | namespace esvo_core 35 | { 36 | using namespace core; 37 | enum TrackingStatus 38 | { 39 | IDLE, 40 | WORKING 41 | }; 42 | 43 | class esvo_Tracking 44 | { 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | esvo_Tracking(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private); 48 | virtual ~esvo_Tracking(); 49 | 50 | // functions regarding tracking 51 | void TrackingLoop(); 52 | bool refDataTransferring(); 53 | bool curDataTransferring(); // These two data transferring functions are decoupled because the data are not updated at the same frequency. 54 | 55 | // topic callback functions 56 | void refMapCallback(const sensor_msgs::PointCloud2::ConstPtr &msg); 57 | void timeSurfaceCallback( 58 | const sensor_msgs::ImageConstPtr &time_surface_left, 59 | const sensor_msgs::ImageConstPtr &time_surface_right); 60 | void eventsCallback(const dvs_msgs::EventArray::ConstPtr &msg); 61 | 62 | // results 63 | void publishPose(const ros::Time &t, Transformation &tr); 64 | void publishPath(const ros::Time &t, Transformation &tr); 65 | void saveTrajectory(const std::string &resultDir); 66 | 67 | // utils 68 | void reset(); 69 | void clearEventQueue(); 70 | void stampedPoseCallback(const geometry_msgs::PoseStampedConstPtr &msg); 71 | bool getPoseAt( 72 | const ros::Time &t, 73 | esvo_core::Transformation &Tr, // T_world_something 74 | const std::string &source_frame); 75 | 76 | private: 77 | ros::NodeHandle nh_, pnh_; 78 | image_transport::ImageTransport it_; 79 | 80 | // subscribers and publishers 81 | ros::Subscriber events_left_sub_; 82 | ros::Subscriber map_sub_; 83 | message_filters::Subscriber TS_left_sub_, TS_right_sub_; 84 | ros::Subscriber stampedPose_sub_; 85 | image_transport::Publisher reprojMap_pub_left_; 86 | 87 | // publishers 88 | ros::Publisher pose_pub_, path_pub_; 89 | 90 | // results 91 | nav_msgs::Path path_; 92 | std::list, Eigen::aligned_allocator>> lPose_; 93 | std::list lTimestamp_; 94 | 95 | // Time Surface sync policy 96 | typedef message_filters::sync_policies::ExactTime ExactSyncPolicy; 97 | message_filters::Synchronizer TS_sync_; 98 | 99 | // offline data 100 | std::string dvs_frame_id_; 101 | std::string world_frame_id_; 102 | std::string calibInfoDir_; 103 | CameraSystem::Ptr camSysPtr_; 104 | 105 | // inter-thread management 106 | std::mutex data_mutex_; 107 | 108 | // online data 109 | EventQueue events_left_; 110 | TimeSurfaceHistory TS_history_; 111 | size_t TS_id_; 112 | std::shared_ptr tf_; 113 | RefPointCloudMap refPCMap_; 114 | RefFrame ref_; 115 | CurFrame cur_; 116 | 117 | /**** offline parameters ***/ 118 | size_t tracking_rate_hz_; 119 | size_t TS_HISTORY_LENGTH_; 120 | size_t REF_HISTORY_LENGTH_; 121 | bool bSaveTrajectory_; 122 | bool bVisualizeTrajectory_; 123 | std::string resultPath_; 124 | 125 | Eigen::Matrix T_world_ref_; 126 | Eigen::Matrix T_world_cur_; 127 | 128 | /*** system objects ***/ 129 | RegProblemType rpType_; 130 | TrackingStatus ets_; 131 | std::string ESVO_System_Status_; 132 | RegProblemConfig::Ptr rpConfigPtr_; 133 | RegProblemSolverLM rpSolver_; 134 | }; 135 | } // namespace esvo_core 136 | 137 | #endif //ESVO_CORE_TRACKING_H -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/optimization/OptimizationFunctor.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_OPTIMIZATION_OPTIMIZATIONFUNCTOR_H 2 | #define ESVO_CORE_OPTIMIZATION_OPTIMIZATIONFUNCTOR_H 3 | 4 | #include 5 | #include 6 | 7 | namespace esvo_core 8 | { 9 | namespace optimization 10 | { 11 | 12 | /** 13 | * Generic functor base for use with the Eigen-nonlinear optimization 14 | * toolbox. Please refer to the Eigen-documentation for further information. 15 | */ 16 | template 17 | struct OptimizationFunctor 18 | { 19 | /** undocumented */ 20 | typedef _Scalar Scalar; 21 | /** undocumented */ 22 | enum 23 | { 24 | InputsAtCompileTime = NX, 25 | ValuesAtCompileTime = NY 26 | }; 27 | /** undocumented */ 28 | typedef Eigen::Matrix InputType; 29 | /** undocumented */ 30 | typedef Eigen::Matrix ValueType; 31 | /** undocumented */ 32 | typedef Eigen::Matrix JacobianType; 33 | 34 | /** undocumented */ 35 | const int m_inputs; 36 | /** undocumented */ 37 | int m_values; 38 | 39 | /** undocumented */ 40 | OptimizationFunctor() : 41 | m_inputs(InputsAtCompileTime), 42 | m_values(ValuesAtCompileTime) {} 43 | /** undocumented */ 44 | OptimizationFunctor(int inputs, int values) : 45 | m_inputs(inputs), 46 | m_values(values) {} 47 | 48 | /** undocumented */ 49 | int inputs() const 50 | { 51 | return m_inputs; 52 | } 53 | /** undocumented */ 54 | int values() const 55 | { 56 | return m_values; 57 | } 58 | 59 | void resetNumberValues( int values ) 60 | { 61 | m_values = values; 62 | } 63 | }; 64 | 65 | } 66 | } 67 | #endif //ESVO_CORE_OPTIMIZATION_OPTIMIZATIONFUNCTOR_H 68 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/tools/TicToc.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_TOOLS_TICTOC_H 2 | #define ESVO_CORE_TOOLS_TICTOC_H 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace esvo_core 11 | { 12 | namespace tools 13 | { 14 | class TicToc 15 | { 16 | public: 17 | TicToc() 18 | { 19 | tic(); 20 | } 21 | 22 | void tic() 23 | { 24 | start = std::chrono::system_clock::now(); 25 | } 26 | 27 | double toc() 28 | { 29 | end = std::chrono::system_clock::now(); 30 | std::chrono::duration elapsed_seconds = end - start; 31 | return elapsed_seconds.count() * 1000; // returns millisec 32 | } 33 | 34 | private: 35 | std::chrono::time_point start, end; 36 | }; 37 | } 38 | } 39 | #endif //ESVO_CORE_TOOLS_TICTOC_H 40 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/tools/Visualization.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_TOOLS_VISUALIZATION_H 2 | #define ESVO_CORE_TOOLS_VISUALIZATION_H 3 | 4 | #include 5 | 6 | namespace esvo_core 7 | { 8 | using namespace container; 9 | namespace tools 10 | { 11 | enum VisMapType 12 | { 13 | InvDepthMap, 14 | StdVarMap, 15 | CostMap, 16 | AgeMap 17 | }; 18 | class Visualization 19 | { 20 | public: 21 | Visualization(); 22 | 23 | virtual ~Visualization(); 24 | 25 | void plot_map( 26 | DepthMap::Ptr &depthMapPtr, 27 | VisMapType vmType, 28 | cv::Mat &img, 29 | double max_range, 30 | double min_range, 31 | double visualization_threshold1, 32 | double visualization_threshold2 = 0.0); 33 | 34 | void plot_eventMap( 35 | std::vector& vEventPtr, 36 | cv::Mat & eventMap, 37 | size_t row, size_t col); 38 | 39 | void plot_events( 40 | std::vector, 41 | Eigen::aligned_allocator > > & vEvents, 42 | cv::Mat & event_img, 43 | size_t row, size_t col); 44 | 45 | void DrawPoint( 46 | double val, 47 | double max_range, 48 | double min_range, 49 | const Eigen::Vector2d &location, 50 | cv::Mat &img ); 51 | 52 | public: 53 | //the rgb values for a jet colormap with 256 values 54 | static const float r[]; 55 | static const float g[]; 56 | static const float b[]; 57 | }; 58 | } 59 | } 60 | 61 | #endif //ESVO_CORE_TOOLS_VISUALIZATION_H 62 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/tools/cayley.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_TOOLS_CAYLEY_H 2 | #define ESVO_CORE_TOOLS_CAYLEY_H 3 | 4 | #include 5 | namespace esvo_core 6 | { 7 | namespace tools 8 | { 9 | Eigen::Matrix3d cayley2rot( const Eigen::Vector3d & cayley); 10 | Eigen::Vector3d rot2cayley( const Eigen::Matrix3d & R ); 11 | }// namespace tools 12 | }// namespace esvo_core 13 | 14 | 15 | #endif //ESVO_CORE_TOOLS_CAYLEY_H 16 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/tools/params_helper.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_PARAMS_HELPER_H 2 | #define ESVO_CORE_PARAMS_HELPER_H 3 | 4 | #pragma once 5 | #include 6 | #include 7 | 8 | namespace esvo_core 9 | { 10 | namespace tools 11 | { 12 | //inline 13 | //bool hasParam(const std::string &name) 14 | //{ 15 | // return ros::param::has(name); 16 | //} 17 | // 18 | //template 19 | //T getParam(const std::string &name, const T &defaultValue) 20 | //{ 21 | // T v; 22 | // if (ros::param::get(name, v)) 23 | // { 24 | // ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v); 25 | // return v; 26 | // } 27 | // else 28 | // ROS_WARN_STREAM("Cannot find value for parameter: " << name << ", assigning default: " << defaultValue); 29 | // return defaultValue; 30 | //} 31 | // 32 | //template 33 | //T getParam(const std::string &name) 34 | //{ 35 | // T v; 36 | // if (ros::param::get(name, v)) 37 | // { 38 | // ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v); 39 | // return v; 40 | // } 41 | // else 42 | // ROS_ERROR_STREAM("Cannot find value for parameter: " << name); 43 | // return T(); 44 | //} 45 | 46 | template 47 | T param(const ros::NodeHandle &nh, const std::string &name, const T &defaultValue) 48 | { 49 | if (nh.hasParam(name)) 50 | { 51 | T v; 52 | nh.param(name, v, defaultValue); 53 | ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v); 54 | return v; 55 | } 56 | ROS_WARN_STREAM("Cannot find value for parameter: " << name << ", assigning default: " << defaultValue); 57 | return defaultValue; 58 | } 59 | } // tools 60 | } // esvo_core 61 | 62 | #endif //ESVO_CORE_PARAMS_HELPER_H 63 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/tools/sobel.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_TOOLS_SOBEL_H 2 | #define ESVO_CORE_TOOLS_SOBEL_H 3 | 4 | #include 5 | #include 6 | 7 | using namespace std; 8 | namespace esvo_core 9 | { 10 | namespace tools 11 | { 12 | class Sobel 13 | { 14 | public: 15 | Sobel(size_t kernel_size); 16 | virtual ~Sobel(); 17 | 18 | double grad_x(Eigen::Matrix3d& src); 19 | double grad_y(Eigen::Matrix3d& src); 20 | void grad_xy(Eigen::Matrix3d& src, Eigen::Vector2d& grad); 21 | double convolve( 22 | const Eigen::Matrix3d& kernel, 23 | const Eigen::Matrix3d& src); 24 | private: 25 | size_t kernel_size_; 26 | static const Eigen::Matrix3d sobel_3x3_x, sobel_3x3_y; 27 | static const Eigen::Matrix sobel_5x5_x, sobel_5x5_y; 28 | }; 29 | } 30 | } 31 | #endif //ESVO_CORE_TOOLS_SOBEL_H -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/tools/utils.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_TOOLS_UTILS_H 2 | #define ESVO_CORE_TOOLS_UTILS_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | using namespace std; 30 | namespace esvo_core 31 | { 32 | namespace tools 33 | { 34 | // TUNE this according to your platform's computational capability. 35 | #define NUM_THREAD_TRACKING 2 36 | #define NUM_THREAD_MAPPING 4 37 | 38 | typedef pcl::PointCloud PointCloud; 39 | using RefPointCloudMap = std::map; 40 | 41 | using Transformation = kindr::minimal::QuatTransformation; 42 | 43 | inline static std::vector::iterator EventVecPtr_lower_bound( 44 | std::vector &vEventPtr, ros::Time &t) 45 | { 46 | return std::lower_bound(vEventPtr.begin(), vEventPtr.end(), t, 47 | [](const dvs_msgs::Event *e, const ros::Time &t) { return e->ts.toSec() < t.toSec(); }); 48 | } 49 | 50 | using EventQueue = std::deque; 51 | inline static EventQueue::iterator EventBuffer_lower_bound( 52 | EventQueue &eb, ros::Time &t) 53 | { 54 | return std::lower_bound(eb.begin(), eb.end(), t, 55 | [](const dvs_msgs::Event &e, const ros::Time &t) { return e.ts.toSec() < t.toSec(); }); 56 | } 57 | 58 | inline static EventQueue::iterator EventBuffer_upper_bound( 59 | EventQueue &eb, ros::Time &t) 60 | { 61 | return std::upper_bound(eb.begin(), eb.end(), t, 62 | [](const ros::Time &t, const dvs_msgs::Event &e) { return t.toSec() < e.ts.toSec(); }); 63 | } 64 | 65 | using StampTransformationMap = std::map; 66 | inline static StampTransformationMap::iterator StampTransformationMap_lower_bound( 67 | StampTransformationMap &stm, ros::Time &t) 68 | { 69 | return std::lower_bound(stm.begin(), stm.end(), t, 70 | [](const std::pair &st, const ros::Time &t) { return st.first.toSec() < t.toSec(); }); 71 | } 72 | 73 | /******************* Used by Block Match ********************/ 74 | static inline void meanStdDev( 75 | Eigen::MatrixXd &patch, 76 | double &mean, double &sigma) 77 | { 78 | size_t numElement = patch.rows() * patch.cols(); 79 | mean = patch.array().sum() / numElement; 80 | Eigen::MatrixXd sub = patch.array() - mean; 81 | sigma = sqrt((sub.array() * sub.array()).sum() / numElement) + 1e-6; 82 | } 83 | 84 | static inline void normalizePatch( 85 | Eigen::MatrixXd &patch_src, 86 | Eigen::MatrixXd &patch_dst) 87 | { 88 | double mean = 0; 89 | double sigma = 0; 90 | meanStdDev(patch_src, mean, sigma); 91 | patch_dst = (patch_src.array() - mean) / sigma; 92 | } 93 | 94 | // recursively create a directory 95 | static inline void _mkdir(const char *dir) 96 | { 97 | char tmp[256]; 98 | char *p = NULL; 99 | size_t len; 100 | 101 | snprintf(tmp, sizeof(tmp), "%s", dir); 102 | len = strlen(tmp); 103 | if (tmp[len - 1] == '/') 104 | tmp[len - 1] = 0; 105 | for (p = tmp + 1; *p; p++) 106 | if (*p == '/') 107 | { 108 | *p = 0; 109 | mkdir(tmp, S_IRWXU); 110 | *p = '/'; 111 | } 112 | mkdir(tmp, S_IRWXU); 113 | } 114 | 115 | } // namespace tools 116 | } // namespace esvo_core 117 | #endif //ESVO_CORE_TOOLS_UTILS_H 118 | -------------------------------------------------------------------------------- /esvo_core/include/initial/MonoInitializer.h: -------------------------------------------------------------------------------- 1 | #ifndef MONO_INITIALIZER_H_ 2 | #define MONO_INITIALIZER_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | // #include 16 | #include 17 | // #include 18 | // #include 19 | 20 | template 21 | void reduceVector(std::vector &v, std::vector status) 22 | { 23 | int j = 0; 24 | for (int i = 0; i < int(v.size()); i++) 25 | if (status[i]) 26 | v[j++] = v[i]; 27 | v.resize(j); 28 | } 29 | 30 | class MonoInitializer 31 | { 32 | public: 33 | MonoInitializer(); 34 | ~MonoInitializer() = default; 35 | 36 | void setMask(); 37 | bool inBorder(const cv::Point2f &pt); 38 | void trackImage(const double &t, const cv::Mat &img, const Eigen::Matrix3d &K); 39 | 40 | void addFeatureCheckParallax(const size_t &frame_count); 41 | bool initialize(const Eigen::Matrix3d &K); 42 | void relativePose(Eigen::Matrix3d &relative_R, Eigen::Vector3d &relative_T, int &l); 43 | 44 | void drawTrack(); 45 | inline cv::Mat getTrackImage() const 46 | { 47 | return img_track_vis_; 48 | } 49 | 50 | private: 51 | int row_, col_; 52 | cv::Mat mask_; 53 | cv::Mat prev_img_, cur_img_; 54 | cv::Mat img_track_vis_; 55 | std::vector n_pts_; 56 | std::vector prev_pts_, cur_pts_; 57 | std::vector prev_un_pts_, cur_un_pts_; 58 | std::vector ids_; 59 | std::vector track_cnt_; 60 | 61 | std::map>>> feature_vec_; 62 | double prev_time_, cur_time_; 63 | bool has_prediction_; 64 | int n_id_; 65 | 66 | int focal_length_; 67 | int frame_count_; 68 | // FeatureManager f_manager_; 69 | // MotionEstimator m_estimator_; 70 | 71 | // ModelSelector modelSelector_; 72 | 73 | bool isLargeParallax_; 74 | }; 75 | 76 | #endif -------------------------------------------------------------------------------- /esvo_core/include/initial/initial_sfm.h: -------------------------------------------------------------------------------- 1 | /******************************************************* 2 | * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology 3 | * 4 | * This file is part of VINS. 5 | * 6 | * Licensed under the GNU General Public License v3.0; 7 | * you may not use this file except in compliance with the License. 8 | * 9 | * Author: Qin Tong (qintonguav@gmail.com) 10 | *******************************************************/ 11 | 12 | #ifndef INITIAL_SFM_H_ 13 | #define INITIAL_SFM_H_ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | namespace eloam 28 | { 29 | struct SFMFeature 30 | { 31 | bool state; 32 | int id; 33 | std::vector> observation; 34 | double position[3]; 35 | double depth; 36 | }; 37 | 38 | struct ReprojectionError3D 39 | { 40 | ReprojectionError3D(double observed_u, double observed_v) 41 | : observed_u(observed_u), observed_v(observed_v) 42 | { 43 | } 44 | 45 | template 46 | bool operator()(const T *const camera_R, const T *const camera_T, const T *point, T *residuals) const 47 | { 48 | T p[3]; 49 | ceres::QuaternionRotatePoint(camera_R, point, p); 50 | p[0] += camera_T[0]; 51 | p[1] += camera_T[1]; 52 | p[2] += camera_T[2]; 53 | T xp = p[0] / p[2]; 54 | T yp = p[1] / p[2]; 55 | residuals[0] = xp - T(observed_u); 56 | residuals[1] = yp - T(observed_v); 57 | return true; 58 | } 59 | 60 | static ceres::CostFunction *Create(const double observed_x, 61 | const double observed_y) 62 | { 63 | return (new ceres::AutoDiffCostFunction< 64 | ReprojectionError3D, 2, 4, 3, 3>( 65 | new ReprojectionError3D(observed_x, observed_y))); 66 | } 67 | 68 | double observed_u; 69 | double observed_v; 70 | }; 71 | 72 | class GlobalSFM 73 | { 74 | public: 75 | GlobalSFM(); 76 | bool construct(int frame_num, Eigen::Quaterniond *q, Eigen::Vector3d *T, int l, 77 | const Eigen::Matrix3d relative_R, const Eigen::Vector3d relative_T, 78 | std::vector &sfm_f, std::map &sfm_tracked_points, 79 | double &depth_min, double &depth_median); 80 | 81 | private: 82 | bool solveFrameByPnP(Eigen::Matrix3d &R_initial, Eigen::Vector3d &P_initial, int i, std::vector &sfm_f); 83 | void triangulatePoint(Eigen::Matrix &Pose0, Eigen::Matrix &Pose1, 84 | Eigen::Vector2d &point0, Eigen::Vector2d &point1, Eigen::Vector3d &point_3d); 85 | void triangulateTwoFrames(int frame0, Eigen::Matrix &Pose0, 86 | int frame1, Eigen::Matrix &Pose1, 87 | std::vector &sfm_f); 88 | 89 | int feature_num; 90 | 91 | std::vector depth_vec; 92 | }; 93 | } // namespace eloam 94 | 95 | #endif -------------------------------------------------------------------------------- /esvo_core/include/initial/solve_5pts.h: -------------------------------------------------------------------------------- 1 | /******************************************************* 2 | * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology 3 | * 4 | * This file is part of VINS. 5 | * 6 | * Licensed under the GNU General Public License v3.0; 7 | * you may not use this file except in compliance with the License. 8 | * 9 | * Author: Qin Tong (qintonguav@gmail.com) 10 | *******************************************************/ 11 | 12 | #ifndef SOLVER_5PTS_H_ 13 | #define SOLVER_5PTS_H_ 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | //#include 20 | #include 21 | 22 | #include "eloam/utility/parameters.h" 23 | 24 | class MotionEstimator 25 | { 26 | public: 27 | bool solveRelativeRT(const std::vector> &corres, Eigen::Matrix3d &R, Eigen::Vector3d &T); 28 | 29 | private: 30 | double testTriangulation(const std::vector &l, 31 | const std::vector &r, 32 | cv::Mat_ R, cv::Mat_ t); 33 | void decomposeE(cv::Mat E, 34 | cv::Mat_ &R1, cv::Mat_ &R2, 35 | cv::Mat_ &t1, cv::Mat_ &t2); 36 | }; 37 | 38 | #endif -------------------------------------------------------------------------------- /esvo_core/launch/monosystem/monosystem_rpg_stereo.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | "dvs" 40 | "map" 41 | $(arg calibInfoDirStr) 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | "dvs" 54 | "map" 55 | $(arg calibInfoDirStr) 56 | 57 | 58 | 59 | 60 | 64 | 65 | -------------------------------------------------------------------------------- /esvo_core/launch/monosystem/monosystem_simu.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | "dvs" 35 | "map" 36 | $(arg calibInfoDirStr) 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | "dvs" 48 | "map" 49 | $(arg calibInfoDirStr) 50 | 51 | 52 | 53 | 54 | 58 | 59 | -------------------------------------------------------------------------------- /esvo_core/launch/mvsmono/mvsmono_rpg_mono.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | "dvs" 29 | "map" 30 | $(arg calibInfoDirStr) 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 46 | 47 | -------------------------------------------------------------------------------- /esvo_core/launch/mvsmono/mvsmono_rpg_slider.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | "dvs" 29 | "map" 30 | $(arg calibInfoDirStr) 31 | 32 | 33 | 34 | 35 | 36 | 49 | 50 | 51 | 55 | 56 | -------------------------------------------------------------------------------- /esvo_core/launch/mvsmono/mvsmono_simu.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | "cam0" 27 | "map" 28 | $(arg calibInfoDirStr) 29 | 30 | 31 | 32 | 33 | 37 | 38 | -------------------------------------------------------------------------------- /esvo_core/launch/mvstereo/mvstereo_rpg_stereo.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | "marker" 42 | "map" 43 | $(arg calibInfoDirStr) 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 60 | 61 | -------------------------------------------------------------------------------- /esvo_core/launch/mvstereo/mvstereo_upenn.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | "dvs" 40 | "map" 41 | $(arg calibInfoDirStr) 42 | 43 | 44 | 45 | 46 | 47 | 49 | 51 | 52 | -------------------------------------------------------------------------------- /esvo_core/launch/system/system_hkust.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | "dvs" 41 | "map" 42 | $(arg calibInfoDirStr) 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | "dvs" 57 | "map" 58 | $(arg calibInfoDirStr) 59 | 60 | 61 | 62 | 63 | 64 | 66 | 68 | 69 | -------------------------------------------------------------------------------- /esvo_core/launch/system/system_simu.launch: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | true 13 | 14 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | "dvs" 49 | "map" 50 | $(arg calibInfoDirStr) 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | "dvs" 62 | "map" 63 | $(arg calibInfoDirStr) 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /esvo_core/launch/tracking/tracking_rpg_mono.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | "dvs" 36 | "map" 37 | $(arg calibInfoDirStr) 38 | 39 | 40 | 41 | 42 | 43 | 47 | 48 | -------------------------------------------------------------------------------- /esvo_core/launch/tracking/tracking_rpg_stereo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | true 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | "dvs" 41 | "map" 42 | $(arg calibInfoDirStr) 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | "dvs" 56 | "map" 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 70 | 71 | -------------------------------------------------------------------------------- /esvo_core/launch/tracking/tracking_simu.launch: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | true 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | "dvs" 41 | "map" 42 | $(arg calibInfoDirStr) 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | "dvs" 56 | "map" 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 70 | 71 | -------------------------------------------------------------------------------- /esvo_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | esvo_core 4 | 0.0.0 5 | The esvo_core package. 6 | 7 | 8 | 9 | Yi Zhou 10 | 11 | 12 | 13 | 14 | GPLv3 15 | 16 | catkin 17 | catkin_simple 18 | 19 | dvs_msgs 20 | glog_catkin 21 | gflags_catkin 22 | cv_bridge 23 | minkindr 24 | minkindr_conversions 25 | image_geometry 26 | image_transport 27 | pcl_ros 28 | tf_conversions 29 | tf2 30 | tf2_ros 31 | roscpp 32 | dynamic_reconfigure 33 | esvo_cartesian3dgrid 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /esvo_core/script/fn_constants.py: -------------------------------------------------------------------------------- 1 | # ! /usr/bin/python2 2 | 3 | seqsToDatasetsMapping = { 4 | 'rpg_stereo': ['rpg_monitor', 'rpg_box', 'rpg_bin', 'rpg_desk'], 5 | 'upenn': ['indoor_flying1', 'indoor_flying2', 'indoor_flying3'], 6 | 'simu': ['simu_poster_planar', 'simu_checkerboard_planar', 'simu_office_planar', \ 7 | 'simu_poster_6dof', 'simu_checkerboard_6dof', 'simu_office_6dof', \ 8 | 'simu_poster_6dof_fast', 'simu_checkerboard_6dof_fast', 'simu_office_6dof_fast'] 9 | } -------------------------------------------------------------------------------- /esvo_core/script/fn_constants.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gogojjh/ESVO_extension/a860c9bd6f5d7ea0c277440d3c30d37b9df6521c/esvo_core/script/fn_constants.pyc -------------------------------------------------------------------------------- /esvo_core/src/TrackingNode.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char** argv) 4 | { 5 | ros::init(argc, argv, "Tracking"); 6 | ros::NodeHandle nh; 7 | ros::NodeHandle nh_private("~"); 8 | 9 | esvo_core::Tracking tracker(nh, nh_private); 10 | ros::spin(); 11 | return 0; 12 | } 13 | 14 | -------------------------------------------------------------------------------- /esvo_core/src/container/EventPoint.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace esvo_core 4 | { 5 | namespace container 6 | { 7 | EventPoint::EventPoint() 8 | { 9 | row_ = 0; 10 | col_ = 0; 11 | ts_ = ros::Time(); 12 | polarity_ = 0; 13 | } 14 | 15 | EventPoint::EventPoint(size_t row, size_t col) 16 | { 17 | row_ = row; 18 | col_ = col; 19 | ts_ = ros::Time(); 20 | polarity_ = 0; 21 | } 22 | 23 | EventPoint::EventPoint(size_t row, size_t col, ros::Time &ts, uint8_t polarity) 24 | { 25 | row_ = row; 26 | col_ = col; 27 | ts_ = ts; 28 | polarity_ = polarity; 29 | } 30 | 31 | EventPoint::~EventPoint() 32 | {} 33 | 34 | size_t 35 | EventPoint::row() const 36 | { 37 | return row_; 38 | } 39 | 40 | size_t 41 | EventPoint::col() const 42 | { 43 | return col_; 44 | } 45 | 46 | ros::Time 47 | EventPoint::ts() const 48 | { 49 | return ts_; 50 | } 51 | 52 | uint8_t 53 | EventPoint::polarity() const 54 | { 55 | return polarity_; 56 | } 57 | 58 | bool 59 | EventPoint::valid() const 60 | { 61 | return ts_.toSec() > 0; 62 | } 63 | 64 | void 65 | EventPoint::copy(const EventPoint ©) 66 | { 67 | ts_= copy.ts_; 68 | polarity_ = copy.polarity_; 69 | } 70 | 71 | } 72 | } -------------------------------------------------------------------------------- /esvo_core/src/container/ResidualItem.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | esvo_core::container::ResidualItem::ResidualItem() = default; 4 | 5 | esvo_core::container::ResidualItem::ResidualItem( 6 | const double x, 7 | const double y, 8 | const double z) 9 | { 10 | initialize(x,y,z); 11 | } 12 | 13 | void esvo_core::container::ResidualItem::initialize( 14 | const double x, 15 | const double y, 16 | const double z) 17 | { 18 | p_ = Eigen::Vector3d(x,y,z); 19 | // bOutlier_ = false; 20 | // variance_ = 1.0; 21 | } -------------------------------------------------------------------------------- /esvo_core/src/emvs_core/MedianFilter.cpp: -------------------------------------------------------------------------------- 1 | #include "emvs_core/MedianFilter.hpp" 2 | 3 | #include 4 | 5 | #define FORWARD 1 6 | #define BACKWARD (-1) 7 | 8 | int compute_median_histogram(const int h[], int num_elements) 9 | { 10 | int middle = (num_elements+1)/2; 11 | int m, n; 12 | for(m=0,n=0; n < 256; n++) 13 | { 14 | m += h[n]; 15 | if(m >= middle) 16 | break; 17 | } 18 | return n; 19 | } 20 | 21 | 22 | inline int get_value(const cv::Mat& img, const cv::Mat& mask, int row, int col) 23 | { 24 | if(row >= 0 && col >= 0 && row < img.rows && col < img.cols && mask.at(row,col) > 0) 25 | return img.at(row,col); 26 | else 27 | return -1; 28 | } 29 | 30 | 31 | // 2D median filter. Complexity: O(p) where p is the patch size 32 | // "T. Huang, G. Yang, and G. Tang, "A Fast Two-Dimensional Median Filtering Algorithm" 33 | // Code adapted from: http://www.sergejusz.com/engineering_tips/median_filter.htm 34 | void huangMedianFilter(const cv::Mat& img, cv::Mat& out_img, const cv::Mat& mask, int patch_size) 35 | { 36 | CHECK(img.data); 37 | CHECK(mask.data); 38 | CHECK_GT(img.rows, 0); 39 | CHECK_GT(img.cols, 0); 40 | CHECK_EQ(img.rows, mask.rows); 41 | CHECK_EQ(img.cols, mask.cols); 42 | CHECK_EQ(img.type(), CV_8U); 43 | CHECK_EQ(patch_size % 2, 1); 44 | 45 | out_img = img.clone(); 46 | 47 | int p = patch_size / 2; 48 | 49 | int med; 50 | int prev, next; 51 | int h[256]; 52 | int direction = FORWARD; 53 | int row1, row2, col1, col2; 54 | int row, col, r, c; 55 | 56 | memset(h, 0, sizeof(h)); 57 | 58 | int num_elements = 0; 59 | 60 | // Histogram For (0,0)-element 61 | for(row = -p; row <= p; row++) 62 | { 63 | for(col = -p; col <= p; col++) 64 | { 65 | int val = get_value(img, mask, row, col); 66 | if(val >= 0) 67 | { 68 | h[val]++; 69 | num_elements++; 70 | } 71 | } 72 | } 73 | 74 | // Median 75 | med = compute_median_histogram(h, num_elements); 76 | 77 | // Now, Median Is Defined For (0,0)-element 78 | // Begin Scanning: direction - FORWARD 79 | out_img.at(0,0)=med; 80 | 81 | // main loop 82 | for(col=1, row=0; row=0 && col= 0 && value_out == value_in) 104 | continue; 105 | if(value_out >= 0) 106 | { 107 | h[value_out]--; 108 | num_elements--; 109 | } 110 | if(value_in >= 0) 111 | { 112 | h[value_in]++; 113 | num_elements++; 114 | } 115 | } 116 | 117 | // Update new median 118 | med = compute_median_histogram(h, num_elements); 119 | out_img.at(row,col) = med; 120 | } // end of column loop 121 | 122 | if(row == img.rows-1) 123 | return; 124 | 125 | // go back to the last/first pixel of the line 126 | col -= direction; 127 | // change direction to the opposite 128 | direction *= -1; 129 | 130 | // Shift Down One Line 131 | prev = row1; 132 | next = row2+1; 133 | 134 | col1 = col - p; 135 | col2 = col + p; 136 | 137 | for(c=col1; c<=col2; c++) 138 | { 139 | int value_out = get_value(img, mask, prev, c); 140 | int value_in = get_value(img, mask, next, c); 141 | if(value_in >= 0 && value_out == value_in) 142 | continue; 143 | if(value_out >= 0) 144 | { 145 | h[value_out]--; 146 | num_elements--; 147 | } 148 | if(value_in >= 0) 149 | { 150 | h[value_in]++; 151 | num_elements++; 152 | } 153 | } 154 | 155 | med = compute_median_histogram(h, num_elements); 156 | out_img.at(row+1,col) = med; 157 | col += direction; 158 | } 159 | } 160 | -------------------------------------------------------------------------------- /esvo_core/src/esvo_MVSMonoNode.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char** argv) 4 | { 5 | ros::init(argc, argv, "esvo_MVSMono"); 6 | ros::NodeHandle nh; 7 | ros::NodeHandle nh_private("~"); 8 | 9 | esvo_core::esvo_MVSMono mvs(nh, nh_private); 10 | ros::spin(); 11 | return 0; 12 | } -------------------------------------------------------------------------------- /esvo_core/src/esvo_MVStereoNode.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char** argv) 4 | { 5 | ros::init(argc, argv, "esvo_MVStereo"); 6 | ros::NodeHandle nh; 7 | ros::NodeHandle nh_private("~"); 8 | 9 | esvo_core::esvo_MVStereo mvs(nh, nh_private); 10 | ros::spin(); 11 | return 0; 12 | } -------------------------------------------------------------------------------- /esvo_core/src/esvo_MappingNode.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char** argv) 4 | { 5 | ros::init(argc, argv, "esvo_Mapping"); 6 | ros::NodeHandle nh; 7 | ros::NodeHandle nh_private("~"); 8 | 9 | esvo_core::esvo_Mapping mapper(nh, nh_private); 10 | ros::spin(); 11 | return 0; 12 | } 13 | 14 | -------------------------------------------------------------------------------- /esvo_core/src/esvo_MonoMappingNode.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char** argv) 4 | { 5 | ros::init(argc, argv, "esvo_MonoMapping"); 6 | ros::NodeHandle nh; 7 | ros::NodeHandle nh_private("~"); 8 | 9 | esvo_core::esvo_MonoMapping mapper(nh, nh_private); 10 | ros::spin(); 11 | return 0; 12 | } 13 | 14 | -------------------------------------------------------------------------------- /esvo_core/src/esvo_TrackingNode.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char** argv) 4 | { 5 | ros::init(argc, argv, "esvo_Tracking"); 6 | ros::NodeHandle nh; 7 | ros::NodeHandle nh_private("~"); 8 | 9 | esvo_core::esvo_Tracking tracker(nh, nh_private); 10 | ros::spin(); 11 | return 0; 12 | } 13 | 14 | -------------------------------------------------------------------------------- /esvo_core/src/tools/cayley.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | Eigen::Matrix3d 4 | esvo_core::tools::cayley2rot(const Eigen::Vector3d &cayley) 5 | { 6 | Eigen::Matrix3d R; 7 | double scale = 1+pow(cayley[0],2)+pow(cayley[1],2)+pow(cayley[2],2); 8 | 9 | R(0,0) = 1+pow(cayley[0],2)-pow(cayley[1],2)-pow(cayley[2],2); 10 | R(0,1) = 2*(cayley[0]*cayley[1]-cayley[2]); 11 | R(0,2) = 2*(cayley[0]*cayley[2]+cayley[1]); 12 | R(1,0) = 2*(cayley[0]*cayley[1]+cayley[2]); 13 | R(1,1) = 1-pow(cayley[0],2)+pow(cayley[1],2)-pow(cayley[2],2); 14 | R(1,2) = 2*(cayley[1]*cayley[2]-cayley[0]); 15 | R(2,0) = 2*(cayley[0]*cayley[2]-cayley[1]); 16 | R(2,1) = 2*(cayley[1]*cayley[2]+cayley[0]); 17 | R(2,2) = 1-pow(cayley[0],2)-pow(cayley[1],2)+pow(cayley[2],2); 18 | 19 | R = (1/scale) * R; 20 | return R; 21 | } 22 | 23 | Eigen::Vector3d 24 | esvo_core::tools::rot2cayley(const Eigen::Matrix3d &R) 25 | { 26 | Eigen::Matrix3d C1; 27 | Eigen::Matrix3d C2; 28 | Eigen::Matrix3d C; 29 | C1 = R-Eigen::Matrix3d::Identity(); 30 | C2 = R+Eigen::Matrix3d::Identity(); 31 | C = C1 * C2.inverse(); 32 | 33 | Eigen::Vector3d cayley; 34 | cayley[0] = -C(1,2); 35 | cayley[1] = C(0,2); 36 | cayley[2] = -C(0,1); 37 | 38 | return cayley; 39 | } 40 | -------------------------------------------------------------------------------- /esvo_core/src/tools/publishMap.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | pcl::PointCloud::Ptr PC_ptr; 14 | 15 | std::string resultPath; 16 | std::string strDataset; 17 | std::string strSequence; 18 | size_t mapping_rate_hz; 19 | std::string world_frame_id, dvs_frame_id; 20 | std::string strRep; 21 | size_t TS_start, Event_start; 22 | 23 | ros::Time TS_latest_timestamp, EM_latest_timestamp; 24 | size_t TS_number = 0, event_number = 0; 25 | std::deque TS_time_buf; 26 | double start_time = 0.0; 27 | std::shared_ptr tf_ptr; 28 | 29 | template 30 | T param(const ros::NodeHandle &nh, const std::string &name, const T &defaultValue) 31 | { 32 | if (nh.hasParam(name)) 33 | { 34 | T v; 35 | nh.param(name, v, defaultValue); 36 | ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v); 37 | return v; 38 | } 39 | ROS_WARN_STREAM("Cannot find value for parameter: " << name << ", assigning default: " << defaultValue); 40 | return defaultValue; 41 | } 42 | 43 | void timeSurfaceCallback(const sensor_msgs::ImageConstPtr &time_surface_left) 44 | { 45 | TS_latest_timestamp = time_surface_left->header.stamp; 46 | TS_number++; 47 | } 48 | 49 | void eventsCallback(const dvs_msgs::EventArray::ConstPtr &msg) 50 | { 51 | EM_latest_timestamp = msg->header.stamp; 52 | event_number += msg->events.size(); 53 | } 54 | 55 | int main(int argc, char **argv) 56 | { 57 | ros::init(argc, argv, "publishMap"); 58 | ros::NodeHandle nh; 59 | ros::NodeHandle nh_private("~"); 60 | 61 | resultPath = param(nh_private, "PATH_TO_SAVE_TRAJECTORY", std::string("/tmp/")); 62 | strDataset = param(nh_private, "Dataset_Name", std::string("rpg")); 63 | strSequence = param(nh_private, "Sequence_Name", std::string("shapes_poster")); 64 | mapping_rate_hz = param(nh_private, "mapping_rate_hz", 20); 65 | world_frame_id = param(nh_private, "world_frame_id", std::string("map")); 66 | dvs_frame_id = param(nh_private, "dvs_frame_id", std::string("dvs")); 67 | strRep = param(nh_private, "Representation_Name", std::string("TS")); 68 | TS_start = param(nh_private, "TS_start", 20); 69 | Event_start = param(nh_private, "Event_start", 10000); 70 | 71 | ros::Subscriber TS_left_sub_ = nh.subscribe("time_surface_left", 10, timeSurfaceCallback); 72 | ros::Subscriber events_left_sub = nh.subscribe("events_left", 10, eventsCallback); 73 | ros::Publisher pc_pub = nh.advertise("/publishMap/pointcloud_local", 1); 74 | 75 | tf_ptr = std::make_shared(true, ros::Duration(100.0)); 76 | 77 | std::string pcdName = resultPath + "/" + strDataset + "/" + strSequence + ".pcd"; 78 | PC_ptr.reset(new pcl::PointCloud()); 79 | pcl::io::loadPCDFile(pcdName, *PC_ptr); 80 | PC_ptr->header.frame_id = world_frame_id; 81 | std::cout << "Loading point cloud: " << PC_ptr->size() << std::endl; 82 | TS_time_buf.clear(); 83 | 84 | ros::Rate r(mapping_rate_hz); 85 | while (ros::ok()) 86 | { 87 | ros::spinOnce(); 88 | r.sleep(); 89 | if (!strRep.compare("TS") && TS_number < TS_start) // tracking initialize 90 | { 91 | continue; 92 | } 93 | else if (!strRep.compare("EM") && event_number < Event_start) 94 | { 95 | continue; 96 | } 97 | 98 | sensor_msgs::PointCloud2::Ptr pc_to_publish(new sensor_msgs::PointCloud2); 99 | if (!PC_ptr->empty()) 100 | { 101 | pcl::toROSMsg(*PC_ptr, *pc_to_publish); 102 | if (!strRep.compare("TS")) 103 | { 104 | if (TS_number != 0) 105 | { 106 | pc_to_publish->header.stamp = TS_latest_timestamp; 107 | } 108 | } 109 | else if (!strRep.compare("EM")) 110 | { 111 | if (event_number != 0) 112 | { 113 | pc_to_publish->header.stamp = EM_latest_timestamp; 114 | } 115 | } 116 | pc_pub.publish(pc_to_publish); 117 | } 118 | } 119 | return 0; 120 | } 121 | 122 | 123 | 124 | 125 | -------------------------------------------------------------------------------- /esvo_core/src/tools/saveMap.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | size_t N_accumEvents; 13 | size_t accumulateEvent; 14 | pcl::PointCloud::Ptr PC_ptr; 15 | 16 | std::string resultPath; 17 | std::string strDataset; 18 | std::string strSequence; 19 | 20 | template 21 | T param(const ros::NodeHandle &nh, const std::string &name, const T &defaultValue) 22 | { 23 | if (nh.hasParam(name)) 24 | { 25 | T v; 26 | nh.param(name, v, defaultValue); 27 | ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v); 28 | return v; 29 | } 30 | ROS_WARN_STREAM("Cannot find value for parameter: " << name << ", assigning default: " << defaultValue); 31 | return defaultValue; 32 | } 33 | 34 | void eventsCallback(const dvs_msgs::EventArray::ConstPtr &msg) 35 | { 36 | accumulateEvent += msg->events.size(); 37 | } 38 | 39 | void refMapCallback(const sensor_msgs::PointCloud2::ConstPtr &msg) 40 | { 41 | pcl::fromROSMsg(*msg, *PC_ptr); 42 | if (accumulateEvent > N_accumEvents) 43 | { 44 | std::string pcdName = resultPath + strDataset + "/" + strSequence + ".pcd"; 45 | std::cout << "Saving point cloud: " << PC_ptr->size() << " to " << pcdName << std::endl; 46 | pcl::io::savePCDFileASCII(pcdName, *PC_ptr); 47 | ros::shutdown(); 48 | } 49 | } 50 | 51 | int main(int argc, char **argv) 52 | { 53 | ros::init(argc, argv, "saveMap"); 54 | ros::NodeHandle nh; 55 | ros::NodeHandle nh_private("~"); 56 | 57 | N_accumEvents = param(nh_private, "EVENTS_SAVE_MAP", 5e6); 58 | resultPath = param(nh_private, "PATH_TO_SAVE_RESULT", std::string("/tmp/")); 59 | strDataset = param(nh_private, "Dataset_Name", std::string("rpg")); 60 | strSequence = param(nh_private, "Sequence_Name", std::string("shapes_poster")); 61 | 62 | ros::Subscriber events_sub = nh.subscribe("events", 0, eventsCallback); 63 | ros::Subscriber map_sub = nh.subscribe("pointcloud", 0, refMapCallback); 64 | PC_ptr.reset(new pcl::PointCloud()); 65 | accumulateEvent = 0; 66 | 67 | ros::Rate r(100); 68 | while (ros::ok()) 69 | { 70 | ros::spinOnce(); 71 | r.sleep(); 72 | } 73 | return 0; 74 | } 75 | -------------------------------------------------------------------------------- /esvo_core/src/tools/sobel.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace esvo_core 4 | { 5 | namespace tools 6 | { 7 | Sobel::Sobel(size_t kernel_size):kernel_size_(kernel_size) 8 | {} 9 | 10 | Sobel::~Sobel() 11 | {} 12 | 13 | double Sobel::grad_x(Eigen::Matrix3d& src) 14 | { 15 | return convolve(sobel_3x3_x, src) / 8; 16 | } 17 | 18 | double Sobel::grad_y(Eigen::Matrix3d& src) 19 | { 20 | return convolve(sobel_3x3_y, src) / 8; 21 | } 22 | 23 | void Sobel::grad_xy(Eigen::Matrix3d& src, Eigen::Vector2d& grad) 24 | { 25 | grad << grad_x(src), grad_y(src); 26 | } 27 | 28 | double Sobel::convolve( 29 | const Eigen::Matrix3d& kernel, 30 | const Eigen::Matrix3d& src) 31 | { 32 | return kernel.cwiseProduct(src).sum(); 33 | } 34 | 35 | const Eigen::Matrix3d Sobel::sobel_3x3_x 36 | = (Eigen::Matrix3d() << -1, 0, 1, 37 | -2, 0, 2, 38 | -1, 0, 1).finished(); 39 | const Eigen::Matrix3d Sobel::sobel_3x3_y 40 | = (Eigen::Matrix3d() << -1, -2, -1, 41 | 0, 0, 0, 42 | 1, 2, 1).finished(); 43 | const Eigen::Matrix Sobel::sobel_5x5_x 44 | = (Eigen::Matrix() << -5, -4, 0, 4, 5, 45 | -8, -10, 0, 10, 8, 46 | -10, -20, 0, 20, 10, 47 | -8, -10, 0, 10, 8, 48 | -5, -4, 0, 4, 5).finished();; 49 | const Eigen::Matrix Sobel::sobel_5x5_y 50 | = (Eigen::Matrix() << -5, -8, -10, -8, -5, 51 | -4, -10, -20, -10, -4, 52 | 0, 0, 0, 0, 0, 53 | 4, 10, 20, 10, 4, 54 | 5, 8, 10, 8, 5).finished(); 55 | }//tools 56 | }//esvo_core -------------------------------------------------------------------------------- /esvo_core/test/test_transform.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | using namespace std; 10 | 11 | int main(int argc, char **argv) 12 | { 13 | ros::init(argc, argv, "test_transform"); 14 | ros::NodeHandle nh; 15 | 16 | ros::Rate r(10); 17 | 18 | ros::Time t_begin = ros::Time::now(); 19 | tf::Transformer tf_; 20 | for (size_t i = 0; i <= 10; i++) 21 | { 22 | if (!ros::ok()) 23 | break; 24 | tf::Transform tf( 25 | tf::Quaternion( 26 | 0, 27 | 0, 28 | 0, 29 | 1), 30 | tf::Vector3( 31 | i * 1, 32 | 0, 33 | 0)); 34 | ros::Time t_cur = ros::Time::now(); 35 | // printf("t_cur: %fs\n", t_cur.toSec()); 36 | tf::StampedTransform st(tf, t_cur, "world", "base_link"); 37 | tf_.setTransform(st); 38 | static tf::TransformBroadcaster br; 39 | br.sendTransform(st); 40 | // ros::Time t_test = ros::Time(t_cur.toSec() - 0.3); 41 | // printf("t_test: %fs\n", t_test.toSec()); 42 | // std::string *err_msg = new std::string(); 43 | // if (!tf_.canTransform("world", "base_link", t_test, err_msg)) 44 | // { 45 | // std::cout << *err_msg << std::endl; 46 | // delete err_msg; 47 | // // continue; 48 | // } 49 | // else 50 | // { 51 | // tf::StampedTransform st; 52 | // tf_.lookupTransform("world", "base_link", t_test, st); 53 | // tf::Quaternion q; 54 | // tf::Vector3 t; 55 | // t = st.getOrigin(); 56 | // q = st.getRotation(); 57 | // std::cout << t.x() << " " << t.y() << " " << t.z() << std::endl; 58 | // } 59 | r.sleep(); 60 | // sleep(1); 61 | } 62 | 63 | ros::Time t_end = ros::Time::now(); 64 | double dt = (t_end.toSec() - t_begin.toSec()) / 20; 65 | printf("t_begin: %fs\n", t_begin.toSec()); 66 | printf("t_end: %fs\n", t_end.toSec()); 67 | printf("dt: %fs\n", dt); 68 | for (size_t i = 0; i < 20; i++) 69 | { 70 | ros::Time t_cur = ros::Time::now().fromSec(t_begin.toSec() + dt * i); 71 | std::string *err_msg = new std::string(); 72 | if (!tf_.canTransform("world", "base_link", t_cur, err_msg)) 73 | { 74 | std::cout << *err_msg << std::endl; 75 | delete err_msg; 76 | continue; 77 | } 78 | else 79 | { 80 | tf::StampedTransform st; 81 | tf_.lookupTransform("world", "base_link", t_cur, st); 82 | tf::Quaternion q; 83 | tf::Vector3 t; 84 | t = st.getOrigin(); 85 | q = st.getRotation(); 86 | std::cout << i << ": " << t.x() << " " << t.y() << " " << t.z() << std::endl; 87 | } 88 | } 89 | 90 | return 0; 91 | } -------------------------------------------------------------------------------- /esvo_time_surface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(esvo_time_surface) 3 | 4 | # To be consistent with the configuration in esvo_core 5 | set(CMAKE_CXX_STANDARD 14) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | # SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") 8 | set(CMAKE_CXX_FLAGS "-O3") 9 | 10 | find_package(catkin_simple REQUIRED) 11 | catkin_simple(ALL_DEPS_REQUIRED) 12 | 13 | # SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") 14 | 15 | find_package(OpenCV REQUIRED) 16 | 17 | include_directories(include ${catkin_INCLUDE_DIRS}) 18 | 19 | # make the executable 20 | cs_add_executable(esvo_time_surface 21 | src/TimeSurface.cpp 22 | src/TimeSurface_node.cpp 23 | ) 24 | 25 | # link the executable to the necesarry libs 26 | target_link_libraries(esvo_time_surface 27 | ${catkin_LIBRARIES} 28 | ${OpenCV_LIBRARIES} 29 | ) 30 | 31 | # make the executable 32 | cs_add_executable(TimeSurface_global_timer 33 | src/TimeSurface_global_timer.cpp 34 | ) 35 | 36 | # link the executable to the necesarry libs 37 | target_link_libraries(TimeSurface_global_timer 38 | ${catkin_LIBRARIES} 39 | ) 40 | 41 | 42 | cs_install() -------------------------------------------------------------------------------- /esvo_time_surface/cfg/parameters.yaml: -------------------------------------------------------------------------------- 1 | use_sim_time: True 2 | ignore_polarity: True 3 | time_surface_mode: 0 # 0: Backward; 1: Forward 4 | decay_ms: 30 5 | median_blur_kernel_size: 1 6 | max_event_queue_len: 20 # the maximum events that are stored in a pixel -------------------------------------------------------------------------------- /esvo_time_surface/include/esvo_time_surface/TicToc.h: -------------------------------------------------------------------------------- 1 | #ifndef esvo_tictoc_H_ 2 | #define esvo_tictoc_H_ 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace esvo_time_surface 11 | { 12 | class TicToc 13 | { 14 | public: 15 | TicToc() 16 | { 17 | tic(); 18 | } 19 | 20 | void tic() 21 | { 22 | start = std::chrono::system_clock::now(); 23 | } 24 | 25 | double toc() 26 | { 27 | end = std::chrono::system_clock::now(); 28 | std::chrono::duration elapsed_seconds = end - start; 29 | return elapsed_seconds.count() * 1000; 30 | } 31 | 32 | private: 33 | std::chrono::time_point start, end; 34 | }; 35 | } 36 | 37 | #endif // esvo_tictoc_H_ 38 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/mono_time_surface.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 20 | 21 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/hkust/hkust_calib_info.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 13 | 14 | 24 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/hkust/hkust_lab.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/rpg_mono/rpg_boxes_6dof.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/rpg_mono/rpg_dynamic_6dof.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/rpg_simu/rpg_checkerboard_6dof.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/rpg_simu/rpg_office_6dof.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/rpg_simu/rpg_shapes_poster_6dof.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/rpg_slider/rpg_calib_info.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 13 | 14 | 24 | 25 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/rpg_slider/rpg_slider_depth.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/rpg_slider/rpg_slider_far.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/rpg_stereo/rpg_calib_info.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 13 | 14 | 24 | 25 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/rpg_stereo/rpg_stereo_bin.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/rpg_stereo/rpg_stereo_boxes.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/rpg_stereo/rpg_stereo_desk.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/rpg_stereo/rpg_stereo_monitor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/upenn/upenn_indoor_flying1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/upenn/upenn_indoor_flying3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/stereo_time_surface.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /esvo_time_surface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | esvo_time_surface 4 | 0.0.0 5 | The esvo_time_surface package. 6 | Yi Zhou 7 | 8 | 9 | 10 | 11 | GPLv3 12 | 13 | catkin 14 | catkin_simple 15 | 16 | roscpp 17 | eigen_catkin 18 | sensor_msgs 19 | cv_bridge 20 | image_transport 21 | glog_catkin 22 | gflags_catkin 23 | dvs_msgs 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /esvo_time_surface/src/TimeSurface_global_timer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #define TIME_SURFACE_SYNC 14 | 15 | int newEvents; 16 | 17 | template 18 | T param(const ros::NodeHandle &nh, const std::string &name, const T &defaultValue) 19 | { 20 | if (nh.hasParam(name)) 21 | { 22 | T v; 23 | nh.param(name, v, defaultValue); 24 | ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v); 25 | return v; 26 | } 27 | ROS_WARN_STREAM("Cannot find value for parameter: " << name << ", assigning default: " << defaultValue); 28 | return defaultValue; 29 | } 30 | 31 | void eventsCallback(const dvs_msgs::EventArray::ConstPtr &msg) 32 | { 33 | newEvents += msg->events.size(); 34 | } 35 | 36 | int main(int argc, char *argv[]) 37 | { 38 | ros::init(argc, argv, "esvo_time_surface_global_timer"); 39 | 40 | ros::NodeHandle nh; 41 | ros::NodeHandle nh_private("~"); 42 | 43 | const int MINIMUM_EVENTS = param(nh_private, "minimum_events", static_cast(1000)); 44 | const int FREQUENCY_TIMER = param(nh_private, "frequency_timer", static_cast(100)); 45 | const int MINIMUM_FREQUENCY_TIMER = param(nh_private, "minimum_frequency_timer", static_cast(40)); 46 | ros::Subscriber event_sub = nh.subscribe("events", 0, eventsCallback); 47 | ros::Publisher global_time_pub = nh.advertise("/sync", 1); 48 | 49 | newEvents = 0; 50 | double timestampLast = ros::WallTime::now().toSec(); 51 | while (ros::ok()) 52 | { 53 | ros::spinOnce(); 54 | #ifdef TIME_SURFACE_SYNC 55 | if (ros::WallTime::now().toSec() - timestampLast >= 1.0 / FREQUENCY_TIMER) 56 | { 57 | std_msgs::Time msg; 58 | msg.data = ros::Time(ros::WallTime::now().toSec()); 59 | global_time_pub.publish(msg); 60 | timestampLast = msg.data.toSec(); 61 | newEvents = 0; 62 | } 63 | #else 64 | if ((ros::WallTime::now().toSec() - timestampLast >= 1.0 / FREQUENCY_TIMER) && (newEvents >= MINIMUM_EVENTS) 65 | || (ros::WallTime::now().toSec() - timestampLast >= 1.0 / MINIMUM_FREQUENCY_TIMER)) 66 | { 67 | // LOG_EVERY_N(INFO, 10) << (ros::WallTime::now().toSec() - timestampLast) * 1000 << " ms"; 68 | std_msgs::Time msg; 69 | msg.data = ros::Time(ros::WallTime::now().toSec()); 70 | global_time_pub.publish(msg); 71 | timestampLast = msg.data.toSec(); 72 | newEvents = 0; 73 | } 74 | #endif 75 | usleep(1e3); // 1ms 76 | } 77 | return 0; 78 | } 79 | -------------------------------------------------------------------------------- /esvo_time_surface/src/TimeSurface_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char* argv[]) 4 | { 5 | ros::init(argc, argv, "esvo_time_surface"); 6 | 7 | ros::NodeHandle nh; 8 | ros::NodeHandle nh_private("~"); 9 | 10 | esvo_time_surface::TimeSurface ts(nh, nh_private); 11 | 12 | ros::spin(); 13 | 14 | return 0; 15 | } 16 | -------------------------------------------------------------------------------- /events_repacking_helper/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(events_repacking_helper) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin_simple REQUIRED COMPONENTS 11 | rosbag 12 | roscpp 13 | rospy 14 | std_msgs 15 | sensor_msgs 16 | geometry_msgs 17 | ) 18 | 19 | catkin_simple() 20 | 21 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O0") 22 | 23 | # make the executable 24 | cs_add_executable(EventMessageEditor src/EventMessageEditor.cpp) 25 | target_link_libraries(EventMessageEditor ${catkin_LIBARIES}) 26 | 27 | cs_add_executable(EventMessageEditor_Mono src/EventMessageEditor_Mono.cpp) 28 | target_link_libraries(EventMessageEditor_Mono ${catkin_LIBARIES}) -------------------------------------------------------------------------------- /events_repacking_helper/README.md: -------------------------------------------------------------------------------- 1 | # events_repacking_helper 2 | 3 | This package provides an example of preparing a bag file for ESVO. A bag file recorded by a stereo event camera (e.g., a pair of DAVIS sensors) typically consists of the following topics: 4 | 5 | -- /davis/left/events 6 | -- /davis/right/events 7 | 8 | -- /davis/left/imu 9 | -- /davis/right/imu 10 | 11 | -- /davis/left/camera_info 12 | -- /davis/right/camera_info 13 | 14 | -- /davis/left/image_raw (optional for visualization) 15 | -- /davis/right/image_raw (optional for visualization) 16 | 17 | ## Preparation 18 | 19 | ### 1. Extract event messages from the original bag, and change the streaming rate to 1000 Hz. 20 | 21 | Set the input and output paths as arguments in the file `repacking.launch`, and then run 22 | 23 | `$ roslaunch events_repacking_helper repacking.launch` 24 | 25 | This command will return a bag file (e.g., output.bag.events) which only contains the re-packed stereo event messages. 26 | 27 | ### 2. Extract other needed topics from the original bag using [srv_tools](https://github.com/srv/srv_tools). 28 | 29 | `$ cd path_to_/srv_tools/bag_tools/scripts` 30 | 31 | `$ python extract_topics.py source_bag_name output_bag_name [topic names]` 32 | 33 | This command will return a bag file (e.g., output.bag.extracted) which contains other necessary topics except for events. 34 | 35 | ### 3. Merge above output bags. 36 | 37 | `$ python merge.py output.bag.events output.bag.extracted --output output_bag_name` 38 | 39 | ### 4. Remove hot pixels using [DVS_Hot_Pixel_Filter](https://github.com/cedric-scheerlinck/dvs_tools/tree/master/dvs_hot_pixel_filter) (optional). 40 | 41 | `$ rosrun dvs_hot_pixel_filter hot_pixel_filter path_to_input.bag` 42 | 43 | This command will generate a bag file named xxx.bag.filtered, which is good to feed to ESVO. 44 | 45 | ### 5. Or directly run the scripts 46 | 47 | 48 | -------------------------------------------------------------------------------- /events_repacking_helper/launch/repacking.launch: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /events_repacking_helper/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | events_repacking_helper 4 | 0.0.0 5 | The events_repacking_helper package 6 | 7 | 8 | zhouyi 9 | 10 | 11 | 12 | 13 | GPLv3 14 | 15 | catkin 16 | rosbag 17 | roscpp 18 | rospy 19 | std_msgs 20 | dvs_msgs 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /events_repacking_helper/script/extract_topics.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | """ 3 | Copyright (c) 2012, 4 | Systems, Robotics and Vision Group 5 | University of the Balearican Islands 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of Systems, Robotics and Vision Group, University of 16 | the Balearican Islands nor the names of its contributors may be used to 17 | endorse or promote products derived from this software without specific 18 | prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | 33 | PKG = 'bag_tools' # this package name 34 | 35 | import roslib; roslib.load_manifest(PKG) 36 | import rospy 37 | import rosbag 38 | import os 39 | import sys 40 | import argparse 41 | 42 | def extract_topics(inbag,outbag,topics): 43 | rospy.loginfo(' Processing input bagfile: %s', inbag) 44 | rospy.loginfo(' Writing to output bagfile: %s', outbag) 45 | rospy.loginfo(' Extracting topics: %s', topics) 46 | 47 | outbag = rosbag.Bag(outbag,'w') 48 | for topic, msg, t in rosbag.Bag(inbag,'r').read_messages(): 49 | if topic in topics: 50 | outbag.write(topic, msg, t) 51 | rospy.loginfo('Closing output bagfile and exit...') 52 | outbag.close(); 53 | 54 | if __name__ == "__main__": 55 | rospy.init_node('extract_topics') 56 | parser = argparse.ArgumentParser( 57 | description='Extracts topics from a bagfile into another bagfile.') 58 | parser.add_argument('inbag', help='input bagfile') 59 | parser.add_argument('outbag', help='output bagfile') 60 | parser.add_argument('topics', nargs='+', help='topics to extract') 61 | args = parser.parse_args() 62 | 63 | try: 64 | extract_topics(args.inbag,args.outbag,args.topics) 65 | except Exception, e: 66 | import traceback 67 | traceback.print_exc() 68 | 69 | -------------------------------------------------------------------------------- /events_repacking_helper/script/ijrr_rpg_bag_edited.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | if [[ $1 == "" ]];then 4 | echo -e "--------------------Deployment script------------------" 5 | echo -e "One argument is needed. Usage: \n" 6 | echo -e " ./bag_edited.sh \n" 7 | echo -e "Example: \n" 8 | echo -e " ./bag_edited.sh /Monster/dataset/event_camera/ijrr_rpg_dataset slider_depth dvs \n" 9 | echo -e "exiting" 10 | echo -e "------------------------------------------------------------------" 11 | exit 1 12 | fi 13 | FOLDER_NAME=$1 14 | BAG_NAME=$2 15 | CAM_NAME=$3 16 | 17 | echo -e "Start editing bags" 18 | rosrun events_repacking_helper EventMessageEditor_Mono \ 19 | $FOLDER_NAME/$BAG_NAME.bag $FOLDER_NAME/$BAG_NAME.bag.events /$CAM_NAME/events 20 | 21 | python extract_topics.py \ 22 | $FOLDER_NAME/$BAG_NAME.bag $FOLDER_NAME/$BAG_NAME.bag.extracted \ 23 | /$CAM_NAME/camera_info /$CAM_NAME/image_raw /optitrack/davis 24 | 25 | python merge.py \ 26 | $FOLDER_NAME/$BAG_NAME.bag.events $FOLDER_NAME/$BAG_NAME.bag.extracted \ 27 | --output $FOLDER_NAME/$BAG_NAME\_edited.bag 28 | echo -e "Finish editing bags" 29 | 30 | rm $FOLDER_NAME/$BAG_NAME.bag.events 31 | rm $FOLDER_NAME/$BAG_NAME.bag.extracted 32 | echo -e "Removing temporary files" -------------------------------------------------------------------------------- /events_repacking_helper/script/merge.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | """ 3 | Copyright (c) 2015, 4 | Enrique Fernandez Perdomo 5 | Clearpath Robotics, Inc. 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of Systems, Robotics and Vision Group, University of 16 | the Balearican Islands nor the names of its contributors may be used to 17 | endorse or promote products derived from this software without specific 18 | prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | from __future__ import print_function 33 | 34 | import rosbag 35 | 36 | import argparse 37 | import os 38 | import sys 39 | 40 | def merge(inbags, outbag='output.bag', topics=None, exclude_topics=[], raw=True): 41 | # Open output bag file: 42 | try: 43 | out = rosbag.Bag(outbag, 'a' if os.path.exists(outbag) else 'w') 44 | except IOError as e: 45 | print('Failed to open output bag file %s!: %s' % (outbag, e.message), file=sys.stderr) 46 | return 127 47 | 48 | # Write the messages from the input bag files into the output one: 49 | for inbag in inbags: 50 | try: 51 | print(' Processing input bagfile: %s' % inbag) 52 | for topic, msg, t in rosbag.Bag(inbag, 'r').read_messages(topics=topics, raw=raw): 53 | if topic not in args.exclude_topics: 54 | out.write(topic, msg, t, raw=raw) 55 | except IOError as e: 56 | print('Failed to open input bag file %s!: %s' % (inbag, e.message), file=sys.stderr) 57 | return 127 58 | 59 | print(' Saving output bag file: %s' % outbag) 60 | out.close() 61 | 62 | return 0 63 | 64 | 65 | if __name__ == "__main__": 66 | parser = argparse.ArgumentParser( 67 | description='Merge multiple bag files into a single one.') 68 | parser.add_argument('inbag', help='input bagfile(s)', nargs='+') 69 | parser.add_argument('--output', help='output bag file', default='output.bag') 70 | parser.add_argument('--topics', help='topics to merge from the input bag files', nargs='+', default=None) 71 | parser.add_argument('--exclude_topics', help='topics not to merge from the input bag files', nargs='+', default=[]) 72 | args = parser.parse_args() 73 | 74 | try: 75 | sys.exit(merge(args.inbag, args.output, args.topics, args.exclude_topics)) 76 | except Exception, e: 77 | import traceback 78 | traceback.print_exc() 79 | -------------------------------------------------------------------------------- /events_repacking_helper/script/simu_bag_edited.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | if [[ $1 == "" ]];then 4 | echo -e "--------------------Deployment script------------------" 5 | echo -e "One argument is needed. Usage: \n" 6 | echo -e " ./bag_edited.sh ...\n" 7 | echo -e "Example: \n" 8 | echo -e " ./bag_edited.sh /Monster/dataset/event_camera/ijrr_rpg_dataset slider_depth dvs \n" 9 | echo -e "exiting" 10 | echo -e "------------------------------------------------------------------" 11 | exit 1 12 | fi 13 | FOLDER_NAME=$1 14 | BAG_NAME=$2 15 | CAM_NAME1=$3 16 | CAM_NAME2=$4 17 | 18 | echo -e "Start editing bags" 19 | rosrun events_repacking_helper EventMessageEditor \ 20 | $FOLDER_NAME/$BAG_NAME.bag $FOLDER_NAME/$BAG_NAME.bag.events /$CAM_NAME1/events /$CAM_NAME2/events 21 | 22 | python extract_topics.py \ 23 | $FOLDER_NAME/$BAG_NAME.bag $FOLDER_NAME/$BAG_NAME.bag.extracted \ 24 | /$CAM_NAME1/camera_info /$CAM_NAME1/depthmap /$CAM_NAME1/image_corrupted \ 25 | /$CAM_NAME1/image_raw /$CAM_NAME1/optic_flow /$CAM_NAME1/pointcloud \ 26 | /$CAM_NAME1/pose /$CAM_NAME1/twist \ 27 | /$CAM_NAME2/camera_info /$CAM_NAME2/depthmap /$CAM_NAME2/image_corrupted \ 28 | /$CAM_NAME2/image_raw /$CAM_NAME2/optic_flow /$CAM_NAME2/pointcloud \ 29 | /$CAM_NAME2/pose /$CAM_NAME2/twist \ 30 | /imu 31 | 32 | python merge.py \ 33 | $FOLDER_NAME/$BAG_NAME.bag.events $FOLDER_NAME/$BAG_NAME.bag.extracted \ 34 | --output $FOLDER_NAME/$BAG_NAME\_edited.bag 35 | echo -e "Finish editing bags" 36 | 37 | rm $FOLDER_NAME/$BAG_NAME.bag.events 38 | rm $FOLDER_NAME/$BAG_NAME.bag.extracted 39 | echo -e "Removing temporary files" -------------------------------------------------------------------------------- /events_repacking_helper/src/EventMessageEditor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | struct EventMessageEditor 9 | { 10 | EventMessageEditor( 11 | double frequency, 12 | std::string & messageTopic) 13 | :bFirstMessage_(true) 14 | { 15 | eArray_ = dvs_msgs::EventArray(); 16 | duration_threshold_ = 1 / frequency; 17 | message_topic_ = messageTopic; 18 | } 19 | 20 | void resetBuffer(ros::Time startTimeStamp) 21 | { 22 | start_time_ = startTimeStamp; 23 | end_time_ = ros::Time(start_time_.toSec() + duration_threshold_); 24 | eArray_.events.clear(); 25 | eArray_.header.stamp = end_time_; 26 | } 27 | 28 | void resetArraySize(size_t width, size_t height) 29 | { 30 | eArray_.width = width; 31 | eArray_.height = height; 32 | } 33 | 34 | void insertEvent( 35 | dvs_msgs::Event & e, 36 | rosbag::Bag* bag) 37 | { 38 | if(bFirstMessage_) 39 | { 40 | resetBuffer(e.ts); 41 | bFirstMessage_ = false; 42 | } 43 | 44 | if(e.ts.toSec() >= end_time_.toSec()) 45 | { 46 | bag->write(message_topic_.c_str(), eArray_.header.stamp, eArray_); 47 | resetBuffer(end_time_); 48 | } 49 | eArray_.events.push_back(e); 50 | } 51 | 52 | // variables 53 | dvs_msgs::EventArray eArray_; 54 | double duration_threshold_; 55 | ros::Time start_time_, end_time_; 56 | bool bFirstMessage_; 57 | std::string message_topic_; 58 | }; 59 | 60 | 61 | int main(int argc, char* argv[]) 62 | { 63 | ros::init(argc, argv, "event_message_editor"); 64 | 65 | std::string src_bag_path(argv[1]); 66 | std::string dst_bag_path(argv[2]); 67 | rosbag::Bag bag_src, bag_dst; 68 | bag_src.open(src_bag_path.c_str(), rosbag::bagmode::Read); 69 | bag_dst.open(dst_bag_path.c_str(), rosbag::bagmode::Write); 70 | 71 | if(!bag_src.isOpen()) 72 | { 73 | ROS_INFO("No rosbag is found in the give path."); 74 | exit(-1); 75 | } 76 | else 77 | { 78 | ROS_INFO("***********Input Bag File Name ***********"); 79 | ROS_INFO(argv[1]); 80 | ROS_INFO("******************************************"); 81 | } 82 | 83 | if(!bag_dst.isOpen()) 84 | { 85 | ROS_INFO("The dst bag is not opened."); 86 | exit(-1); 87 | } 88 | else 89 | { 90 | ROS_INFO("***********Output Bag File Name ***********"); 91 | ROS_INFO(argv[2]); 92 | ROS_INFO("***************************"); 93 | } 94 | 95 | const double frequency = 1000; 96 | 97 | // process events 98 | std::vector topics; 99 | topics.push_back(std::string(argv[3])); 100 | topics.push_back(std::string(argv[4])); 101 | std::vector topics_rename; 102 | topics_rename.push_back(std::string(argv[3])); 103 | topics_rename.push_back(std::string(argv[4])); 104 | for(size_t i = 0;i < topics.size(); i++) 105 | { 106 | rosbag::View view(bag_src, rosbag::TopicQuery(topics[i])); 107 | EventMessageEditor eArrayEditor(frequency, topics_rename[i]); 108 | 109 | // topic loop 110 | for(rosbag::MessageInstance const m: view) 111 | { 112 | dvs_msgs::EventArray::ConstPtr msg = m.instantiate(); 113 | eArrayEditor.resetArraySize(msg->width, msg->height); 114 | // message loop 115 | for(const dvs_msgs::Event& e : msg->events) 116 | { 117 | eArrayEditor.insertEvent(const_cast(e), &bag_dst); 118 | } 119 | } 120 | } 121 | 122 | bag_src.close(); 123 | bag_dst.close(); 124 | return 0; 125 | } 126 | 127 | 128 | -------------------------------------------------------------------------------- /events_repacking_helper/src/EventMessageEditor_Mono.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | struct EventMessageEditor 12 | { 13 | EventMessageEditor( 14 | double frequency, 15 | std::string &messageTopic) 16 | : bFirstMessage_(true) 17 | { 18 | eArray_ = dvs_msgs::EventArray(); 19 | duration_threshold_ = 1 / frequency; 20 | message_topic_ = messageTopic; 21 | } 22 | 23 | void resetBuffer(ros::Time startTimeStamp) 24 | { 25 | start_time_ = startTimeStamp; 26 | end_time_ = ros::Time(start_time_.toSec() + duration_threshold_); 27 | eArray_.events.clear(); 28 | eArray_.header.stamp = end_time_; 29 | } 30 | 31 | void resetArraySize(size_t width, size_t height) 32 | { 33 | eArray_.width = width; 34 | eArray_.height = height; 35 | } 36 | 37 | void insertEvent( 38 | dvs_msgs::Event &e, 39 | rosbag::Bag *bag) 40 | { 41 | if (bFirstMessage_) 42 | { 43 | resetBuffer(e.ts); 44 | bFirstMessage_ = false; 45 | } 46 | 47 | if (e.ts.toSec() >= end_time_.toSec()) 48 | { 49 | bag->write(message_topic_.c_str(), eArray_.header.stamp, eArray_); 50 | resetBuffer(end_time_); 51 | } 52 | eArray_.events.push_back(e); 53 | } 54 | 55 | // variables 56 | dvs_msgs::EventArray eArray_; 57 | double duration_threshold_; 58 | ros::Time start_time_, end_time_; 59 | bool bFirstMessage_; 60 | std::string message_topic_; 61 | }; 62 | 63 | int main(int argc, char *argv[]) 64 | { 65 | ros::init(argc, argv, "event_message_editor"); 66 | 67 | std::string src_bag_path(argv[1]); 68 | std::string dst_bag_path(argv[2]); 69 | rosbag::Bag bag_src, bag_dst; 70 | bag_src.open(src_bag_path.c_str(), rosbag::bagmode::Read); 71 | bag_dst.open(dst_bag_path.c_str(), rosbag::bagmode::Write); 72 | 73 | if (!bag_src.isOpen()) 74 | { 75 | ROS_INFO("No rosbag is found in the give path."); 76 | exit(-1); 77 | } 78 | else 79 | { 80 | ROS_INFO("***********Input Bag File Name ***********"); 81 | ROS_INFO(argv[1]); 82 | ROS_INFO("******************************************"); 83 | } 84 | 85 | if (!bag_dst.isOpen()) 86 | { 87 | ROS_INFO("The dst bag is not opened."); 88 | exit(-1); 89 | } 90 | else 91 | { 92 | ROS_INFO("***********Output Bag File Name ***********"); 93 | ROS_INFO(argv[2]); 94 | ROS_INFO("***************************"); 95 | } 96 | 97 | const double frequency = 1000; 98 | 99 | // process events 100 | std::vector topics; 101 | topics.push_back(std::string(argv[3])); 102 | std::vector topics_rename; 103 | topics_rename.push_back(std::string(argv[3])); 104 | for (size_t i = 0; i < topics.size(); i++) 105 | { 106 | rosbag::View view(bag_src, rosbag::TopicQuery(topics[i])); 107 | EventMessageEditor eArrayEditor(frequency, topics_rename[i]); 108 | 109 | // topic loop 110 | for (rosbag::MessageInstance const m : view) 111 | { 112 | dvs_msgs::EventArray::ConstPtr msg = m.instantiate(); 113 | eArrayEditor.resetArraySize(msg->width, msg->height); 114 | // message loop 115 | for (const dvs_msgs::Event &e : msg->events) 116 | { 117 | eArrayEditor.insertEvent(const_cast(e), &bag_dst); 118 | } 119 | } 120 | } 121 | 122 | bag_src.close(); 123 | bag_dst.close(); 124 | return 0; 125 | } 126 | -------------------------------------------------------------------------------- /pict/MVSMONO_simu_office_planar.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gogojjh/ESVO_extension/a860c9bd6f5d7ea0c277440d3c30d37b9df6521c/pict/MVSMONO_simu_office_planar.gif -------------------------------------------------------------------------------- /pict/Tracking_upenn_flying3.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gogojjh/ESVO_extension/a860c9bd6f5d7ea0c277440d3c30d37b9df6521c/pict/Tracking_upenn_flying3.gif -------------------------------------------------------------------------------- /pict/tracker_comparison.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gogojjh/ESVO_extension/a860c9bd6f5d7ea0c277440d3c30d37b9df6521c/pict/tracker_comparison.png -------------------------------------------------------------------------------- /stereo_rig_design/Stereo_Rig_Base.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gogojjh/ESVO_extension/a860c9bd6f5d7ea0c277440d3c30d37b9df6521c/stereo_rig_design/Stereo_Rig_Base.pdf -------------------------------------------------------------------------------- /stereo_rig_design/camMount.SLDPRT: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gogojjh/ESVO_extension/a860c9bd6f5d7ea0c277440d3c30d37b9df6521c/stereo_rig_design/camMount.SLDPRT --------------------------------------------------------------------------------