├── CMakeLists.txt ├── LICENSE ├── README.md ├── TS_bluredTS_OSTS.png ├── cover_of_video.png ├── dependencies.yaml ├── dv_ros_msgs ├── CMakeLists.txt ├── README.md ├── msg │ ├── Event.msg │ ├── EventArray.msg │ └── Trigger.msg └── package.xml ├── esvo2_core ├── CMakeLists.txt ├── calib │ ├── dsec │ │ └── zurich_city_04_a │ │ │ ├── left.yaml │ │ │ └── right.yaml │ ├── dvx_7.11 │ │ ├── left.yaml │ │ └── right.yaml │ ├── dvx_7.12 │ │ ├── left.yaml │ │ └── right.yaml │ ├── dvx_7.15 │ │ ├── left.yaml │ │ └── right.yaml │ ├── dvx_7.9 │ │ ├── left.yaml │ │ └── right.yaml │ ├── rpg │ │ ├── left.yaml │ │ └── right.yaml │ ├── tum_DoF │ │ ├── left.yaml │ │ └── right.yaml │ ├── tum_desk │ │ ├── left.yaml │ │ └── right.yaml │ ├── upenn │ │ ├── left.yaml │ │ └── right.yaml │ └── vector │ │ ├── left.yaml │ │ └── right.yaml ├── cfg │ ├── DVS_MappingStereo.cfg │ ├── mapping │ │ ├── mapping_dsec_AA.yaml │ │ ├── mapping_dvx_AA_mapping.yaml │ │ ├── mapping_dvx_AA_tracking.yaml │ │ ├── mapping_rpg_AA.yaml │ │ ├── mapping_tum_AA.yaml │ │ ├── mapping_upenn_AA.yaml │ │ └── mapping_vector_AA.yaml │ └── tracking │ │ ├── tracking_dsec_AA.yaml │ │ ├── tracking_dvx_AA_mapping.yaml │ │ ├── tracking_dvx_AA_tracking.yaml │ │ ├── tracking_online_AA.yaml │ │ ├── tracking_rpg_AA.yaml │ │ ├── tracking_tum_AA.yaml │ │ ├── tracking_upenn_AA.yaml │ │ └── tracking_vector_AA.yaml ├── esvo2_mvstereo.perspective ├── esvo2_mvstereo.rviz ├── esvo2_system.perspective ├── esvo2_system.rviz ├── esvo2_system_DSEC.perspective ├── esvo2_system_DSEC.rviz ├── include │ ├── esvo2_core │ │ ├── container │ │ │ ├── CameraSystem.h │ │ │ ├── DepthMap.h │ │ │ ├── DepthPoint.h │ │ │ ├── EventMatchPair.h │ │ │ ├── ResidualItem.h │ │ │ ├── SmartGrid.h │ │ │ └── TimeSurfaceObservation.h │ │ ├── core │ │ │ ├── BackendOptimization.h │ │ │ ├── DepthFusion.h │ │ │ ├── DepthProblem.h │ │ │ ├── DepthProblemSolver.h │ │ │ ├── DepthRegularization.h │ │ │ ├── EventBM.h │ │ │ ├── RegProblemLM.h │ │ │ └── RegProblemSolverLM.h │ │ ├── esvo2_Mapping.h │ │ ├── esvo2_Tracking.h │ │ ├── factor │ │ │ ├── OptimizationFunctor.h │ │ │ ├── imu_factor.h │ │ │ ├── imu_integration.h │ │ │ ├── pose_local_parameterization.h │ │ │ └── utility.h │ │ └── tools │ │ │ ├── TicToc.h │ │ │ ├── Visualization.h │ │ │ ├── cayley.h │ │ │ ├── params_helper.h │ │ │ ├── sobel.h │ │ │ └── utils.h │ └── sophus │ │ ├── se3.hpp │ │ ├── so3.hpp │ │ └── sophus.hpp ├── launch │ └── system │ │ ├── system_dsec.launch │ │ ├── system_dvx_mapping.launch │ │ ├── system_dvx_tracking.launch │ │ ├── system_rpg.launch │ │ ├── system_tum_DoF.launch │ │ ├── system_tum_desk.launch │ │ ├── system_upenn.launch │ │ └── system_vector.launch ├── package.xml └── src │ ├── container │ ├── CameraSystem.cpp │ ├── DepthPoint.cpp │ ├── EventPoint.cpp │ └── ResidualItem.cpp │ ├── core │ ├── BackendOptimization.cpp │ ├── DepthFusion.cpp │ ├── DepthProblem.cpp │ ├── DepthProblemSolver.cpp │ ├── DepthRegularization.cpp │ ├── EventBM.cpp │ ├── RegProblemLM.cpp │ └── RegProblemSolverLM.cpp │ ├── esvo2_Mapping.cpp │ ├── esvo2_MappingNode.cpp │ ├── esvo2_Tracking.cpp │ ├── esvo2_TrackingNode.cpp │ ├── factor │ └── pose_local_parameterization.cpp │ └── tools │ ├── Visualization.cpp │ ├── cayley.cpp │ └── sobel.cpp ├── events_repacking_tool ├── CMakeLists.txt ├── README.md ├── launch │ └── repacking.launch ├── msg │ └── V_ba_bg.msg ├── package.xml └── src │ └── EventMessageEditor.cpp ├── image_representation ├── .gitignore ├── CMakeLists.txt ├── cfg │ ├── image_representation_fast.yaml │ ├── image_representation_fast_40hz.yaml │ ├── image_representation_fast_r.yaml │ └── image_representation_fast_r_40hz.yaml ├── image_representation.perspective ├── include │ └── image_representation │ │ ├── ImageRepresentation.h │ │ └── TicToc.h ├── launch │ └── image_representation.launch ├── package.xml └── src │ ├── ImageRepresentation.cpp │ └── ImageRepresentation_node.cpp └── results ├── gt ├── dsec │ ├── 04a.txt │ ├── 04b.txt │ ├── 04c.txt │ ├── 04d.txt │ ├── 04e.txt │ ├── 04f.txt │ ├── 11a.txt │ └── 11b.txt ├── rpg │ ├── bin.txt │ ├── box2.txt │ ├── desk2.txt │ ├── monitor.txt │ └── reader.txt ├── tum_vie │ ├── 1d_trans.txt │ ├── 3d_trans.txt │ ├── 6d_trans.txt │ ├── desk.txt │ └── desk2.txt ├── upenn │ ├── stamped_groundtruth_of_indoorflying1.txt │ ├── stamped_groundtruth_of_indoorflying2.txt │ ├── stamped_groundtruth_of_indoorflying3.txt │ └── stamped_groundtruth_of_indoorflying4.txt └── vector │ ├── corner_slow.txt │ ├── desk_normal.txt │ ├── hdr_normal.txt │ ├── robot_normal.txt │ └── sofa_normal.txt └── ours ├── ours_dsec ├── stamped_traj_estimate04a.txt ├── stamped_traj_estimate04b.txt ├── stamped_traj_estimate04c.txt ├── stamped_traj_estimate04d.txt ├── stamped_traj_estimate04e.txt ├── stamped_traj_estimate04f.txt ├── stamped_traj_estimate11a.txt └── stamped_traj_estimate11b.txt ├── ours_rpg ├── stamped_traj_estimate_bin.txt ├── stamped_traj_estimate_box2.txt ├── stamped_traj_estimate_desk2.txt ├── stamped_traj_estimate_monitor.txt └── stamped_traj_estimate_reader.txt ├── ours_tum_vie ├── 1d_trans.txt ├── 3d_trans.txt ├── 6d_trans.txt ├── desk.txt └── desk2.txt ├── ours_upenn ├── stamped_traj_indoor1.txt ├── stamped_traj_indoor2.txt ├── stamped_traj_indoor3.txt └── stamped_traj_indoor4.txt └── ours_vector ├── corner_slow.txt ├── desk_normal.txt ├── hdr_normal.txt ├── robot_normal.txt └── sofa_normal.txt /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # toplevel CMakeLists.txt for a catkin workspace 2 | # catkin/cmake/toplevel.cmake 3 | 4 | cmake_minimum_required(VERSION 3.0.2) 5 | 6 | project(Project) 7 | 8 | set(CATKIN_TOPLEVEL TRUE) 9 | 10 | # search for catkin within the workspace 11 | set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}") 12 | execute_process(COMMAND ${_cmd} 13 | RESULT_VARIABLE _res 14 | OUTPUT_VARIABLE _out 15 | ERROR_VARIABLE _err 16 | OUTPUT_STRIP_TRAILING_WHITESPACE 17 | ERROR_STRIP_TRAILING_WHITESPACE 18 | ) 19 | if(NOT _res EQUAL 0 AND NOT _res EQUAL 2) 20 | # searching fot catkin resulted in an error 21 | string(REPLACE ";" " " _cmd_str "${_cmd}") 22 | message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}") 23 | endif() 24 | 25 | # include catkin from workspace or via find_package() 26 | if(_res EQUAL 0) 27 | set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake") 28 | # include all.cmake without add_subdirectory to let it operate in same scope 29 | include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE) 30 | add_subdirectory("${_out}") 31 | 32 | else() 33 | # use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument 34 | # or CMAKE_PREFIX_PATH from the environment 35 | if(NOT DEFINED CMAKE_PREFIX_PATH) 36 | if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "") 37 | if(NOT WIN32) 38 | string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) 39 | else() 40 | set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) 41 | endif() 42 | endif() 43 | endif() 44 | 45 | # list of catkin workspaces 46 | set(catkin_search_path "") 47 | foreach(path ${CMAKE_PREFIX_PATH}) 48 | if(EXISTS "${path}/.catkin") 49 | list(FIND catkin_search_path ${path} _index) 50 | if(_index EQUAL -1) 51 | list(APPEND catkin_search_path ${path}) 52 | endif() 53 | endif() 54 | endforeach() 55 | 56 | # search for catkin in all workspaces 57 | set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE) 58 | find_package(catkin QUIET 59 | NO_POLICY_SCOPE 60 | PATHS ${catkin_search_path} 61 | NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) 62 | unset(CATKIN_TOPLEVEL_FIND_PACKAGE) 63 | 64 | if(NOT catkin_FOUND) 65 | message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.") 66 | endif() 67 | endif() 68 | 69 | catkin_workspace() 70 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Neuromorphic Automation and Intelligence Lab 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /TS_bluredTS_OSTS.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NAIL-HNU/ESVO2/f9545bed9001aac2f440e735e292a7a03ae0005f/TS_bluredTS_OSTS.png -------------------------------------------------------------------------------- /cover_of_video.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NAIL-HNU/ESVO2/f9545bed9001aac2f440e735e292a7a03ae0005f/cover_of_video.png -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /dv_ros_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14) 2 | project(dv_ros_msgs VERSION 1.2.0) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs 5 | message_generation) 6 | 7 | add_message_files(FILES Trigger.msg Event.msg EventArray.msg) 8 | 9 | generate_messages(DEPENDENCIES std_msgs sensor_msgs) 10 | 11 | catkin_package(CATKIN_DEPENDS message_runtime roscpp std_msgs sensor_msgs) 12 | 13 | include_directories(${catkin_INCLUDE_DIRS}) 14 | -------------------------------------------------------------------------------- /dv_ros_msgs/README.md: -------------------------------------------------------------------------------- 1 | # DV ROS Msgs 2 | 3 | This project provides the most basic data structures needed to publish and subscribe event data. Event and EventArray 4 | are basic structure to describe a single event and an event packet. Additional type is a Trigger message, which is used 5 | to communicate various signals (internal and external) from the camera. 6 | -------------------------------------------------------------------------------- /dv_ros_msgs/msg/Event.msg: -------------------------------------------------------------------------------- 1 | # A DVS event 2 | uint16 x 3 | uint16 y 4 | time ts 5 | bool polarity 6 | -------------------------------------------------------------------------------- /dv_ros_msgs/msg/EventArray.msg: -------------------------------------------------------------------------------- 1 | # This message contains an array of events 2 | # (0, 0) is at top-left corner of image 3 | # 4 | 5 | Header header 6 | 7 | uint32 height # image height, that is, number of rows 8 | uint32 width # image width, that is, number of columns 9 | 10 | # an array of events 11 | Event[] events 12 | -------------------------------------------------------------------------------- /dv_ros_msgs/msg/Trigger.msg: -------------------------------------------------------------------------------- 1 | # A DVS trigger 2 | 3 | # Numeric value representing the type of trigger signal received 4 | int8 type 5 | # Time of occurence 6 | time timestamp 7 | 8 | # Below are constant numeric values describing eah type of trigger 9 | # A timestamp reset occurred. 10 | int8 TRIGGER_TYPE_TIMESTAMP_RESET=0 11 | 12 | # A rising edge was detected (External Input module on device). 13 | int8 TRIGGER_TYPE_EXTERNAL_SIGNAL_RISING_EDGE=1 14 | 15 | # A falling edge was detected (External Input module on device). 16 | int8 TRIGGER_TYPE_EXTERNAL_SIGNAL_FALLING_EDGE=2 17 | 18 | # A pulse was detected (External Input module on device). 19 | int8 TRIGGER_TYPE_EXTERNAL_SIGNAL_PULSE=3 20 | 21 | # A rising edge was generated (External Generator module on device). 22 | int8 TRIGGER_TYPE_EXTERNAL_GENERATOR_RISING_EDGE=4 23 | 24 | # A falling edge was generated (External Generator module on device). 25 | int8 TRIGGER_TYPE_EXTERNAL_GENERATOR_FALLING_EDGE=5 26 | 27 | # An APS frame capture has started (Frame Event will follow). 28 | int8 TRIGGER_TYPE_APS_FRAME_START=6 29 | 30 | # An APS frame capture has completed (Frame Event is contemporary). 31 | int8 TRIGGER_TYPE_APS_FRAME_END=7 32 | 33 | # An APS frame exposure has started (Frame Event will follow). 34 | int8 TRIGGER_APS_EXPOSURE_START=8 35 | 36 | # An APS frame exposure has completed (Frame Event will follow). 37 | int8 TRIGGER_APS_EXPOSURE_END=9 38 | -------------------------------------------------------------------------------- /dv_ros_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dv_ros_msgs 4 | 1.3.0 5 | The dv_ros_msgs package contains messages types from dv-processing library for event camera data publishing. 6 | 7 | iniVation 8 | 9 | Apache2.0 10 | 11 | https://dv-processing.inivation.com/ 12 | 13 | catkin 14 | roscpp 15 | std_msgs 16 | sensor_msgs 17 | std_msgs 18 | message_generation 19 | 20 | roscpp 21 | std_msgs 22 | 23 | roscpp 24 | std_msgs 25 | sensor_msgs 26 | message_runtime 27 | 28 | -------------------------------------------------------------------------------- /esvo2_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(esvo2_core) 3 | # explicitly set std=c++14 to remove errors from pcl library 4 | 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 | set(CMAKE_BUILD_TYPE "Release") 11 | # set(CMAKE_CXX_FLAGS "-std=c++14") 12 | 13 | # set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 14 | 15 | 16 | 17 | find_package(catkin_simple REQUIRED) 18 | catkin_simple(ALL_DEPS_REQUIRED) 19 | 20 | find_package(OpenCV REQUIRED) 21 | 22 | # FIND_PACKAGE(Sophus REQUIRED) 23 | # INCLUDE_DIRECTORIES(${Sophus_INCLUDE_DIRS}) 24 | 25 | find_package(fmt REQUIRED) 26 | include_directories(${fmt_INCLUDE_DIRS}) 27 | 28 | find_package(Ceres REQUIRED) 29 | include_directories(${CERES_INCLUDE_DIRS}) 30 | 31 | include_directories( 32 | ${PROJECT_SOURCE_DIR}/include) 33 | ########### 34 | ## Build ## 35 | ########### 36 | 37 | ## Specify additional locations of header files 38 | ## Your package locations should be listed before other locations 39 | include_directories(include) 40 | 41 | set(HEADERS 42 | include/esvo2_core/core/DepthFusion.h 43 | include/esvo2_core/core/DepthRegularization.h 44 | include/esvo2_core/core/DepthProblem.h 45 | include/esvo2_core/core/DepthProblemSolver.h 46 | include/esvo2_core/core/EventBM.h 47 | include/esvo2_core/core/RegProblemLM.h 48 | include/esvo2_core/core/RegProblemSolverLM.h 49 | include/esvo2_core/core/BackendOptimization.h 50 | include/esvo2_core/factor/OptimizationFunctor.h 51 | include/esvo2_core/container/CameraSystem.h 52 | include/esvo2_core/container/DepthPoint.h 53 | include/esvo2_core/container/EventMatchPair.h 54 | include/esvo2_core/container/SmartGrid.h 55 | include/esvo2_core/container/DepthMap.h 56 | include/esvo2_core/container/ResidualItem.h 57 | include/esvo2_core/container/TimeSurfaceObservation.h 58 | include/esvo2_core/tools/Visualization.h 59 | include/esvo2_core/tools/utils.h 60 | include/esvo2_core/tools/TicToc.h 61 | include/esvo2_core/tools/sobel.h 62 | include/esvo2_core/tools/cayley.h 63 | include/esvo2_core/tools/params_helper.h 64 | 65 | include/esvo2_core/factor/utility.h 66 | 67 | include/esvo2_core/factor/pose_local_parameterization.h 68 | ) 69 | 70 | set(SOURCES 71 | src/core/DepthFusion.cpp 72 | src/core/DepthRegularization.cpp 73 | src/core/DepthProblem.cpp 74 | src/core/DepthProblemSolver.cpp 75 | src/core/EventBM.cpp 76 | src/core/RegProblemLM.cpp 77 | src/core/RegProblemSolverLM.cpp 78 | src/core/BackendOptimization.cpp 79 | src/container/CameraSystem.cpp 80 | src/container/DepthPoint.cpp 81 | src/container/ResidualItem.cpp 82 | src/tools/Visualization.cpp 83 | src/tools/sobel.cpp 84 | src/tools/cayley.cpp 85 | ) 86 | 87 | cs_add_library(${PROJECT_NAME}_LIB ${SOURCES} ${HEADERS}) 88 | target_link_libraries(${PROJECT_NAME}_LIB ${CERES_LIBRARIES}) 89 | 90 | 91 | # Node esvo2_Mapping 92 | cs_add_executable(esvo2_Mapping src/esvo2_MappingNode.cpp 93 | src/esvo2_Mapping.cpp include/esvo2_core/esvo2_Mapping.h) 94 | target_link_libraries(esvo2_Mapping ${PROJECT_NAME}_LIB 95 | ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${CERES_LIBRARIES} yaml-cpp) 96 | 97 | # Node esvo2_Tracking 98 | cs_add_executable(esvo2_Tracking src/esvo2_TrackingNode.cpp 99 | src/esvo2_Tracking.cpp include/esvo2_core/esvo2_Tracking.h) 100 | target_link_libraries(esvo2_Tracking ${PROJECT_NAME}_LIB 101 | ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} yaml-cpp) 102 | -------------------------------------------------------------------------------- /esvo2_core/calib/dsec/zurich_city_04_a/left.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: RPG_DSEC_Prophesee_Gen3.1M 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [553.469, 0, 346.653, 8 | 0, 553.399, 216.521, 9 | 0, 0, 1] 10 | distortion_model: plumb_bob 11 | distortion_coefficients: 12 | rows: 1 13 | cols: 4 14 | data: [-0.0935648, 0.194458, 7.64243e-05, 0.00195639] 15 | rectification_matrix: 16 | rows: 3 17 | cols: 3 18 | data: [0.999866, -0.00319364, 0.0160517, 19 | 0.00322964, 0.999992, -0.00221712, 20 | -0.0160445, 0.00226867, 0.999869] 21 | projection_matrix: 22 | rows: 3 23 | cols: 4 24 | data: [536.0578704071853, 0, 336.2674980163574, 0, 25 | 0, 536.0578704071853, 222.2888011932373, 0, 26 | 0, 0, 1, 0] 27 | T_right_left: 28 | rows: 3 29 | cols: 4 30 | data: [0.999759349174819, -0.01136184588494952, 0.01876571862791739, 31 | 0.01144476919322272, 0.9999251858525768, -0.004317401509758799, 32 | -0.01871526103610019, 0.004531131841964332, 0.9998145867352477] 33 | T_b_c: 34 | rows: 3 35 | cols: 4 36 | data: [-1, 0, 0, 0, 37 | 0, -1, 0, 0, 38 | 0, 0, 1, 0] -------------------------------------------------------------------------------- /esvo2_core/calib/dsec/zurich_city_04_a/right.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: RPG_DSEC_Prophesee_Gen3.1M 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [552.182, 0, 336.874, 8 | 0, 551.445, 226.326, 9 | 0, 0, 1] 10 | distortion_model: plumb_bob 11 | distortion_coefficients: 12 | rows: 1 13 | cols: 4 14 | data: [-0.0949368, 0.202115, 0.000582129, 0.00145529] 15 | rectification_matrix: 16 | rows: 3 17 | cols: 3 18 | data: [0.999963, 0.00818053, -0.00267849, 19 | -0.0081745, 0.999964, 0.00225394, 20 | 0.00269683, -0.00223196, 0.999994] 21 | projection_matrix: 22 | rows: 3 23 | cols: 4 24 | data: [536.0578704071853, 0, 336.2674980163574, -321.1164388243399, 25 | 0, 536.0578704071853, 222.2888011932373, 0, 26 | 0, 0, 1, 0] 27 | T_right_left: 28 | rows: 3 29 | cols: 4 30 | data: [0.999759349174819, -0.01136184588494952, 0.01876571862791739, 31 | 0.01144476919322272, 0.9999251858525768, -0.004317401509758799, 32 | -0.01871526103610019, 0.004531131841964332, 0.9998145867352477] 33 | T_b_c: 34 | rows: 3 35 | cols: 4 36 | data: [-1, 0, 0, 0, 37 | 0, -1, 0, 0, 38 | 0, 0, 1, 0] 39 | -------------------------------------------------------------------------------- /esvo2_core/calib/dvx_7.11/left.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: RPG_DSEC_Prophesee_Gen3.1M 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [434.1778, 0, 328.6492, 0, 433.8335, 232.6556, 0, 0, 1] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 4 12 | data: [0.04334948, -0.06016529, 0, 0] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.9998832813933003, 0.004103351036722398, -0.01471686449063877, -0.004121981625352883, 0.9999907409590998, -0.001235825048533487, 0.01471165720259442, 0.001296343449768941, 0.9998909373706782] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [448.4773332253563, 0, 331.2306709289551, 0, 0, 448.4773332253563, 236.0885963439941, 0, 0, 0, 1, 0] 21 | T_right_left: 22 | rows: 3 23 | cols: 4 24 | data: [0.999760252, 0.013379681, -0.017332725, -0.51159033332, -0.013422442, 0.999907146, -0.002353056, 0.00474945463, 0.017299632, 0.002585139, 0.999847008, -0.00132692634] 25 | T_b_c: 26 | rows: 3 27 | cols: 4 28 | data: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0] -------------------------------------------------------------------------------- /esvo2_core/calib/dvx_7.11/right.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: RPG_DSEC_Prophesee_Gen3.1M 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [434.1872, 0, 324.3261, 0, 434.0652, 239.8637, 0, 0, 1] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 4 12 | data: [0.05184187, -0.07847694, 0, 0] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.9999535459183549, -0.009283275482605657, 0.002593607838996671, 0.00927998418159965, 0.9999561231242929, 0.001278170498442819, -0.002605359648438675, -0.001254042482486944, 0.9999958197305399] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [448.4773332253563, 0, 331.2306709289551, -222.7643952971784, 0, 448.4773332253563, 236.0885963439941, 0, 0, 0, 1, 0] 21 | T_right_left: 22 | rows: 3 23 | cols: 4 24 | data: [0.999760252, 0.013379681, -0.017332725, -0.51159033332, -0.013422442, 0.999907146, -0.002353056, 0.00474945463, 0.017299632, 0.002585139, 0.999847008, -0.00132692634] 25 | T_b_c: 26 | rows: 3 27 | cols: 4 28 | data: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0] -------------------------------------------------------------------------------- /esvo2_core/calib/dvx_7.12/left.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: RPG_DSEC_Prophesee_Gen3.1M 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [432.7812, 0, 328.4005, 0, 432.7748, 232.4376, 0, 0, 1] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 4 12 | data: [0.0445071, -0.06356757, 0, 0] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.9998539461716247, 0.003694298096812298, -0.01668647615890179, -0.003729593459353143, 0.9999908722793747, -0.002084584026459531, 0.01667862277460759, 0.002146513337323832, 0.9998585980141567] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [450.5047898814922, 0, 332.9536323547363, 0, 0, 450.5047898814922, 238.4309101104736, 0, 0, 0, 1, 0] 21 | T_right_left: 22 | rows: 3 23 | cols: 4 24 | data: [0.999736436, 0.013393934, -0.018645677, -0.5120856507029999, -0.013471273, 0.999901143, -0.004028423, 0.004971649021999999, 0.018589877, 0.004278542, 0.999818039, -0.0009830110739999999] 25 | T_b_c: 26 | rows: 3 27 | cols: 4 28 | data: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0] -------------------------------------------------------------------------------- /esvo2_core/calib/dvx_7.12/right.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: RPG_DSEC_Prophesee_Gen3.1M 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [434.0541, 0, 325.9281, 0, 434.0786, 244.4503, 0, 0, 1] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 4 12 | data: [0.05821453, -0.08425602, 0, 0] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.999951032394347, -0.009708152465168582, 0.001919528377636773, 0.009704069684654541, 0.9999506565588225, 0.002124965678543425, -0.00194006315229174, -0.00210623438692375, 0.9999958999574311] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [450.5047898814922, 0, 332.9536323547363, -230.7083357060859, 0, 450.5047898814922, 238.4309101104736, 0, 0, 0, 1, 0] 21 | T_right_left: 22 | rows: 3 23 | cols: 4 24 | data: [0.999736436, 0.013393934, -0.018645677, -0.5120856507029999, -0.013471273, 0.999901143, -0.004028423, 0.004971649021999999, 0.018589877, 0.004278542, 0.999818039, -0.0009830110739999999] 25 | T_b_c: 26 | rows: 3 27 | cols: 4 28 | data: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0] -------------------------------------------------------------------------------- /esvo2_core/calib/dvx_7.15/left.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: RPG_DSEC_Prophesee_Gen3.1M 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [422.6183, 0, 326.7147, 0, 422.575, 231.9814, 0, 0, 1] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 4 12 | data: [0.04000976, -0.05895489, 0, 0] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.9997420607778356, 0.003075691061873218, -0.02250226735638845, -0.003118297237261123, 0.9999934109423682, -0.001858573159199178, 0.0224964026907984, 0.001928262518413717, 0.9997450643387211] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [443.2937499920903, 0, 334.158504486084, 0, 0, 443.2937499920903, 239.9048557281494, 0, 0, 0, 1, 0] 21 | T_right_left: 22 | rows: 3 23 | cols: 4 24 | data: [0.9996268699999999, 0.013476739, -0.023759164, -0.519410817, -0.013563895, 0.999901842, -0.003510948, 0.005405287, 0.023709515, 0.003831905, 0.999711546, -0.000633409] 25 | T_b_c: 26 | rows: 3 27 | cols: 4 28 | data: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0] -------------------------------------------------------------------------------- /esvo2_core/calib/dvx_7.15/right.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: RPG_DSEC_Prophesee_Gen3.1M 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [424.2381, 0, 325.6551, 0, 424.0411, 247.5604, 0, 0, 1] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 4 12 | data: [0.05575131, -0.08181416, 0, 0] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.9999451125693434, -0.0104060026106206, 0.001219409017047227, 0.01040367492339396, 0.999944075382641, 0.001899909272396437, -0.001239111282913046, -0.001887118656245959, 0.999997451689956] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [443.2937499920903, 0, 334.158504486084, -230.2642074651054, 0, 443.2937499920903, 239.9048557281494, 0, 0, 0, 1, 0] 21 | T_right_left: 22 | rows: 3 23 | cols: 4 24 | data: [0.9996268699999999, 0.013476739, -0.023759164, -0.519410817, -0.013563895, 0.999901842, -0.003510948, 0.005405287, 0.023709515, 0.003831905, 0.999711546, -0.000633409] 25 | T_b_c: 26 | rows: 3 27 | cols: 4 28 | data: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0] -------------------------------------------------------------------------------- /esvo2_core/calib/dvx_7.9/left.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: RPG_DSEC_Prophesee_Gen3.1M 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [434.9484516, 0, 318.4569369, 8 | 0, 434.8979906, 236.3369857, 9 | 0, 0, 1] 10 | distortion_model: plumb_bob 11 | distortion_coefficients: 12 | rows: 1 13 | cols: 4 14 | data: [0.053739769, -0.08123554, 0, 0] 15 | rectification_matrix: 16 | rows: 3 17 | cols: 3 18 | data: [0.9999105105358901, 0.002873549036556365, -0.01306574283345767, 19 | -0.002915109492186892, 0.9999907489291623, -0.003162940521385146, 20 | 0.01305653309665725, 0.003200745542489167, 0.9999096370029985] 21 | projection_matrix: #use 0 to calib 22 | rows: 3 23 | cols: 4 24 | data: [449.2300511093084, 0, 325.7455711364746, 0, 25 | 0, 449.2300511093084, 242.9759902954102, 0, 26 | 0, 0, 1, 0] 27 | T_right_left: 28 | rows: 3 29 | cols: 4 30 | data: [0.999735249, 0.012800124, -0.019120367, -0.4942509919, 31 | -0.012920476, 0.999897403, -0.006184228, 0.004925674698, 32 | 0.019039246, 0.006429635, 0.999798063, -0.002962203921] 33 | T_b_c: 34 | rows: 3 35 | cols: 4 36 | data: [1, 0, 0, 0, 37 | 0, 1, 0, 0, 38 | 0, 0, 1, 0] -------------------------------------------------------------------------------- /esvo2_core/calib/dvx_7.9/right.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: RPG_DSEC_Prophesee_Gen3.1M 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [433.6833886, 0, 327.6565328, 8 | 0, 433.6727372, 249.1875723, 9 | 0, 0, 1] 10 | distortion_model: plumb_bob 11 | distortion_coefficients: 12 | rows: 1 13 | cols: 4 14 | data: [0.043339754, -0.065281539, 0, 0] 15 | rectification_matrix: 16 | rows: 3 17 | cols: 3 18 | data: [0.999932386961623, -0.009965264084205682, 0.005992913895851983, 19 | 0.00994614437504703, 0.9999453778738928, 0.003211772206806913, 20 | -0.006024592708372559, -0.003151948662294061, 0.9999768844840007] 21 | projection_matrix: #use 0 to calib 22 | rows: 3 23 | cols: 4 24 | data: [449.2300511093084, 0, 325.7455711364746, -246.4474116522289, 25 | 0, 449.2300511093084, 242.9759902954102, 0, 26 | 0, 0, 1, 0] 27 | T_right_left: 28 | rows: 3 29 | cols: 4 30 | data: [0.999735249, 0.012800124, -0.019120367, -0.4942509919, 31 | -0.012920476, 0.999897403, -0.006184228, 0.004925674698, 32 | 0.019039246, 0.006429635, 0.999798063, -0.002962203921] 33 | T_b_c: 34 | rows: 3 35 | cols: 4 36 | data: [1, 0, 0, 0, 37 | 0, 1, 0, 0, 38 | 0, 0, 1, 0] 39 | -------------------------------------------------------------------------------- /esvo2_core/calib/rpg/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] 27 | T_b_c: 28 | rows: 3 29 | cols: 4 30 | data: [0.999905117246, 0.0121780171243, -0.00643835413146, 0, 31 | -0.0122374970355, 0.999882045134, -0.00928113597812, 0, 32 | 0.00632456886338, 0.00935904469797, 0.99993620202, 0] -------------------------------------------------------------------------------- /esvo2_core/calib/rpg/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, 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] 27 | T_b_c: 28 | rows: 3 29 | cols: 4 30 | data: [0.999905117246, 0.0121780171243, -0.00643835413146, 0, 31 | -0.0122374970355, 0.999882045134, -0.00928113597812, 0, 32 | 0.00632456886338, 0.00935904469797, 0.99993620202, 0] -------------------------------------------------------------------------------- /esvo2_core/calib/tum_DoF/left.yaml: -------------------------------------------------------------------------------- 1 | image_width: 1280 2 | image_height: 720 3 | camera_name: RPG_DSEC_Prophesee_Gen3.1M 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [1051.317, 0, 636.1348, 0, 1051.795, 265.7059, 0, 0, 1] 8 | distortion_model: equidistant 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 4 12 | data: [-0.131047, 0.07279845, -0.211093, 0.1849343] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.9996801027846643, -0.01425548616909317, -0.02089194128188449, 0.01403938405407294, 0.9998467906279263, -0.01045423198604381, 0.02103777060016484, 0.01015757771905148, 0.9997270806690477] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [978.82227434271, 0, 662.7080004688397, 0, 0, 978.82227434271, 251.0649540442805, 0, 0, 0, 1, 0] 21 | T_right_left: 22 | rows: 3 23 | cols: 4 24 | data: [0.9999139238730244, 0.00275304520871895, -0.0128283119283708, -0.1187973925652208, -0.003015363491210488, 0.9997858361647609, -0.02047411505649691, 0.002000794259366445, 0.01276919840352954, 0.02051103474741225, 0.9997080799041906, 0.0009996342780319578] 25 | T_b_c: 26 | rows: 3 27 | cols: 4 28 | data: [-0.0160003, -0.999846, -0.00716287, -0.0351382, -0.9998, 0.0159127, 0.0121157, 0.0594199, -0.0119998, 0.00735529, -0.999901, -0.0325031] -------------------------------------------------------------------------------- /esvo2_core/calib/tum_DoF/right.yaml: -------------------------------------------------------------------------------- 1 | image_width: 1280 2 | image_height: 720 3 | camera_name: RPG_DSEC_Prophesee_Gen3.1M 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [1049.048, 0, 652.4781, 0, 1049.574, 262.0072, 0, 0, 1] 8 | distortion_model: equidistant 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 4 12 | data: [-0.1216066, 0.01165121, -0.04866892, 0.03971039] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.9998228165348589, -0.01683908803476893, -0.00841312370402582, 0.01692490916354675, 0.9998043669069874, 0.01023598375945926, 0.008239113186965713, -0.01037656146685995, 0.9999122181401812] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [978.82227434271, 0, 669.4652931589101, -116.3021407929822, 0, 978.82227434271, 251.0649540442805, 0, 0, 0, 1, 0] 21 | T_right_left: 22 | rows: 3 23 | cols: 4 24 | data: [0.9999139238730244, 0.00275304520871895, -0.0128283119283708, -0.1187973925652208, -0.003015363491210488, 0.9997858361647609, -0.02047411505649691, 0.002000794259366445, 0.01276919840352954, 0.02051103474741225, 0.9997080799041905, 0.0009996342780319578] 25 | T_b_c: 26 | rows: 3 27 | cols: 4 28 | data: [-0.0160003, -0.999846, -0.00716287, -0.0351382, -0.9998, 0.0159127, 0.0121157, 0.0594199, -0.0119998, 0.00735529, -0.999901, -0.0325031] -------------------------------------------------------------------------------- /esvo2_core/calib/tum_desk/left.yaml: -------------------------------------------------------------------------------- 1 | image_width: 1280 2 | image_height: 720 3 | camera_name: RPG_DSEC_Prophesee_Gen3.1M 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [1049.583, 0, 634.7184, 0, 1049.423, 263.4698, 0, 0, 1] 8 | distortion_model: equidistant 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 4 12 | data: [-0.1151966, -0.06222183, 0.2168261, -0.2352862] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.999754826676689, -0.01266626048306591, -0.01816183861944204, 0.01247096879510851, 0.9998636259452682, -0.01082610035478137, 0.01829648802297759, 0.01059695036109404, 0.9997764465964727] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [974.8248485687409, 0, 657.1452947326158, 0, 0, 974.8248485687409, 249.0317449660879, 0, 0, 0, 1, 0] 21 | T_right_left: 22 | rows: 3 23 | cols: 4 24 | data: [0.9999139238730244, 0.00275304520871895, -0.0128283119283708, -0.1187973925652208, -0.003015363491210488, 0.9997858361647609, -0.02047411505649691, 0.002000794259366445, 0.01276919840352954, 0.02051103474741225, 0.9997080799041905, 0.0009996342780319578] 25 | T_b_c: 26 | rows: 3 27 | cols: 4 28 | data: [-0.0156758, -0.999861, -0.00567473, -0.0348034, -0.999798, 0.0156027, 0.0127008, 0.0597177, -0.0126105, 0.00587268, -0.999903, -0.0369473] -------------------------------------------------------------------------------- /esvo2_core/calib/tum_desk/right.yaml: -------------------------------------------------------------------------------- 1 | image_width: 1280 2 | image_height: 720 3 | camera_name: RPG_DSEC_Prophesee_Gen3.1M 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [1047.668, 0, 652.9083, 0, 1047.523, 260.541, 0, 0, 1] 8 | distortion_model: equidistant 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 4 12 | data: [-0.1246107, 0.006075037, 0.03566192, -0.08888043] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.9998695874466074, -0.01531690352425632, -0.005118648823660143, 0.01537085831336992, 0.9998248945376152, 0.0106731896617947, 0.004954272303945287, -0.01075047576970244, 0.999929938774044] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [974.8248485687409, 0, 665.7830960074431, -115.4845433975721, 0, 974.8248485687409, 249.0317449660879, 0, 0, 0, 1, 0] 21 | T_right_left: 22 | rows: 3 23 | cols: 4 24 | data: [0.9999067813752269, 0.002756653661083337, -0.0133727117828264, -0.1184515176576849, -0.003041059118927757, 0.999768629994468, -0.02129409397977534, 0.001814547107986286, 0.01331091729629842, 0.02133277618073071, 0.9996838160844428, 0.0006063908024923279] 25 | T_b_c: 26 | rows: 3 27 | cols: 4 28 | data: [-0.0156758, -0.999861, -0.00567473, -0.0348034, 29 | -0.999798, 0.0156027, 0.0127008, 0.0597177, 30 | -0.0126105, 0.00587268, -0.999903, -0.0369473] -------------------------------------------------------------------------------- /esvo2_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] 31 | T_b_c: 32 | rows: 3 33 | cols: 4 34 | data: [1, 0, 0, 0, 35 | 0, 1, 0, 0, 36 | 0, 0, 1, 0] 37 | # data: [0.9998771896957381, 0.015081690576531692, 0.004260062852482415, 0.00286126, 38 | # -0.015128404695721132, 0.9998233340761795, 0.011154909598696812, 0.00180892, 39 | # -0.004091075349566317, -0.011217987615716542, 0.99992871, 0.0177984] 40 | -------------------------------------------------------------------------------- /esvo2_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] 31 | T_b_c: 32 | rows: 3 33 | cols: 4 34 | data: [1, 0, 0, 0, 35 | 0, 1, 0, 0, 36 | 0, 0, 1, 0] -------------------------------------------------------------------------------- /esvo2_core/calib/vector/left.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: RPG_DSEC_Prophesee_Gen3.1M 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [327.32749, 0, 304.97749, 8 | 0, 327.46184, 235.37621, 9 | 0, 0, 1] 10 | distortion_model: plumb_bob 11 | distortion_coefficients: 12 | rows: 1 13 | cols: 4 14 | data: [-0.031982, 0.041966, -0.000507, -0.001031, 0.000000] 15 | rectification_matrix: 16 | rows: 3 17 | cols: 3 18 | data: [1.0, 0.0, 0.0, 19 | 0.0, 1.0, 0.0, 20 | 0.0, 0.0, 1.0] 21 | projection_matrix: 22 | rows: 3 23 | cols: 4 24 | data: [257.7465659912339, 0, 317.5881580988773, 0, 25 | 0, 257.7465659912339, 237.3116097643339, 0, 26 | 0, 0, 1, 0] 27 | T_right_left: 28 | rows: 3 29 | cols: 4 30 | data: [1.000000, 0.000000, 0.000000, -0.170000, 31 | 0.000000, 1.000000, 0.000000, 0.000000, 32 | 0.000000, 0.000000, 1.000000, 0.000000] 33 | T_b_c: 34 | rows: 3 35 | cols: 4 36 | data: [0.000000, 0.000000, 1.000000, 0.103080, 37 | -1.000000, 0.000000, 0.000000, 0.082500, 38 | 0.000000, -1.000000, 0.000000, 0.025000] -------------------------------------------------------------------------------- /esvo2_core/calib/vector/right.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: RPG_DSEC_Prophesee_Gen3.1M 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [327.48497, 0.0, 318.53477, 8 | 0.0, 327.55395, 230.96356, 9 | 0.0, 0.0, 1.0] 10 | distortion_model: plumb_bob 11 | distortion_coefficients: 12 | rows: 1 13 | cols: 4 14 | data: [-0.026300, 0.037995, -0.000513, 0.000167, 0.000000] 15 | rectification_matrix: 16 | rows: 3 17 | cols: 3 18 | data: [1.0, 0.0, 0.0, 19 | 0.0, 1.0, 0.0, 20 | 0.0, 0.0, 1.0] 21 | projection_matrix: 22 | rows: 3 23 | cols: 4 24 | data: [257.7465659912339, 0, 317.5881580988773, -43.81691621850976, 25 | 0, 257.7465659912339, 237.3116097643339, 0, 26 | 0, 0, 1, 0] 27 | T_right_left: 28 | rows: 3 29 | cols: 4 30 | data: [1.000000, 0.000000, 0.000000, -0.170000, 31 | 0.000000, 1.000000, 0.000000, 0.000000, 32 | 0.000000, 0.000000, 1.000000, 0.000000] 33 | T_b_c: 34 | rows: 3 35 | cols: 4 36 | data: [0.000000, 0.000000, 1.000000, 0.103080, 37 | -1.000000, 0.000000, 0.000000, 0.082500, 38 | 0.000000, -1.000000, 0.000000, 0.025000] 39 | -------------------------------------------------------------------------------- /esvo2_core/cfg/DVS_MappingStereo.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "esvo2_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, 50000) 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, 150) 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 esvo2_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, 1000000) 28 | gen.add("stdVar_vis_threshold", double_t, 0, "StdVariance_VIS_THRESHOLD", 0.12, 0.0, 10000000) 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", 1, 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, 20000) 36 | gen.add("PROCESS_EVENT_NUM", int_t, 0, "#processed event", 100, 0, 100000) 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 esvo2_System 44 | gen.add("ResetButton", bool_t, 0, "reset system (used as a button...)", False) 45 | exit(gen.generate(PACKAGE, "esvo2_core", "DVS_MappingStereo")) -------------------------------------------------------------------------------- /esvo2_core/cfg/mapping/mapping_dsec_AA.yaml: -------------------------------------------------------------------------------- 1 | # Configuration for depth estimation 2 | invDepth_min_range: 0.001 3 | invDepth_max_range: 0.25 4 | residual_vis_threshold: 30 #20 #30 #20 #20 5 | residual_vis_threshold_ln: 30 6 | stdVar_vis_threshold: 1 #0.1 #0.2 7 | stdVar_vis_threshold_ln: 1 #0.1 #0.2 8 | age_max_range: 10 9 | age_vis_threshold: 1 10 | # patch size of static BM 11 | patch_size_X: 15 12 | patch_size_Y: 7 13 | # patch size of temporal BM 14 | patch_size_X_2: 5 15 | patch_size_Y_2: 31 16 | # EventBM parameters 17 | BM_half_slice_thickness: 0.001 18 | BM_min_disparity: 0 19 | BM_max_disparity: 150 20 | BM_step: 3 21 | BM_ZNCC_Threshold: 0.2 22 | BM_bUpDownConfiguration: False 23 | distance_from_last_frame: 0.20 24 | SmoothTimeSurface: True #False 25 | 26 | # Configuration for fusion 27 | fusion_radius: 1 28 | FUSION_STRATEGY: CONST_FRAMES #"CONST_POINTS" # "CONST_FRAMES" 29 | maxNumFusionFrames: 5 #40 30 | maxNumFusionFrames_ln: 5 31 | maxNumFusionPoints: 20000 #8000 #3000 32 | LSnorm: Tdist #Tdist #Tdist #Tdist # l2 33 | Tdist_nu: 2.182 #2.1897 34 | Tdist_scale: 17.277 #16.6397 35 | Tdist_stdvar: 59.763 #56.5347 36 | LSnorm_ln: Tdist #Tdist #Tdist #Tdist # l2 37 | Tdist_nu_ln: 2.182 38 | Tdist_scale_ln: 17.277 39 | Tdist_stdvar_ln: 59.763 40 | 41 | # Configuration for point sampling 42 | Denoising: False 43 | PROCESS_EVENT_NUM: 5000 #20000 44 | PROCESS_EVENT_NUM_AA: 5000 #20000 45 | x_patches: 8 46 | y_patches: 6 47 | select_points_from_AA: True 48 | eta_for_select_points: 0.2 49 | 50 | # Configuration for visualization 51 | Regularization: False 52 | RegularizationRadius: 20 53 | RegularizationMinNeighbours: 32 54 | RegularizationMinCloseNeighbours: 32 55 | bVisualizeGlobalPC: True 56 | visualizeGPC_interval: 0.5 #2[second] 57 | NumGPC_added_per_refresh: 10000 58 | visualize_range: 30 59 | 60 | # Configuration for mapping system 61 | TS_HISTORY_LENGTH: 100 62 | USE_IMU: True 63 | INIT_SGM_DP_NUM_THRESHOLD: 500 64 | mapping_rate_hz: 20 65 | large_scale: True -------------------------------------------------------------------------------- /esvo2_core/cfg/mapping/mapping_dvx_AA_mapping.yaml: -------------------------------------------------------------------------------- 1 | # Configuration for depth estimation 2 | invDepth_min_range: 0.05 3 | invDepth_max_range: 0.33 4 | residual_vis_threshold: 30 #20 #30 #20 #20 5 | residual_vis_threshold_ln: 30 6 | stdVar_vis_threshold: 1 #0.1 #0.2 7 | stdVar_vis_threshold_ln: 1 #0.1 #0.2 8 | age_max_range: 10 9 | age_vis_threshold: 1 10 | # patch size of static BM 11 | patch_size_X: 15 12 | patch_size_Y: 7 13 | # patch size of temporal BM 14 | patch_size_X_2: 5 15 | patch_size_Y_2: 31 16 | # EventBM parameters 17 | BM_half_slice_thickness: 0.001 18 | BM_min_disparity: 0 19 | BM_max_disparity: 150 20 | BM_step: 3 21 | BM_ZNCC_Threshold: 0.2 22 | BM_bUpDownConfiguration: False 23 | distance_from_last_frame: 0.20 24 | SmoothTimeSurface: True #False 25 | 26 | # Configuration for fusion 27 | fusion_radius: 2 28 | FUSION_STRATEGY: CONST_FRAMES #"CONST_POINTS" # "CONST_FRAMES" 29 | maxNumFusionFrames: 5 #40 30 | maxNumFusionFrames_ln: 5 31 | maxNumFusionPoints: 20000 #8000 #3000 32 | LSnorm: Tdist #Tdist #Tdist #Tdist # l2 33 | Tdist_nu: 2.182 #2.1897 34 | Tdist_scale: 17.277 #16.6397 35 | Tdist_stdvar: 59.763 #56.5347 36 | LSnorm_ln: Tdist #Tdist #Tdist #Tdist # l2 37 | Tdist_nu_ln: 2.182 38 | Tdist_scale_ln: 17.277 39 | Tdist_stdvar_ln: 59.763 40 | 41 | # Configuration for point sampling 42 | Denoising: False 43 | PROCESS_EVENT_NUM: 10000 #20000 44 | PROCESS_EVENT_NUM_AA: 10000 #20000 45 | x_patches: 8 46 | y_patches: 6 47 | select_points_from_AA: True 48 | eta_for_select_points: 0.1 49 | 50 | # Configuration for visualization 51 | Regularization: True 52 | RegularizationRadius: 20 53 | RegularizationMinNeighbours: 32 54 | RegularizationMinCloseNeighbours: 32 55 | bVisualizeGlobalPC: True 56 | visualizeGPC_interval: 0.5 #2[second] 57 | NumGPC_added_per_refresh: 10000 58 | visualize_range: 30 59 | 60 | # Configuration for mapping system 61 | TS_HISTORY_LENGTH: 100 62 | USE_IMU: True 63 | INIT_SGM_DP_NUM_THRESHOLD: 500 64 | mapping_rate_hz: 20 65 | large_scale: True -------------------------------------------------------------------------------- /esvo2_core/cfg/mapping/mapping_dvx_AA_tracking.yaml: -------------------------------------------------------------------------------- 1 | # Configuration for depth estimation 2 | invDepth_min_range: 0.02 3 | invDepth_max_range: 0.33 4 | residual_vis_threshold: 30 #20 #30 #20 #20 5 | residual_vis_threshold_ln: 30 6 | stdVar_vis_threshold: 1 #0.1 #0.2 7 | stdVar_vis_threshold_ln: 1 #0.1 #0.2 8 | age_max_range: 10 9 | age_vis_threshold: 1 10 | # patch size of static BM 11 | patch_size_X: 15 12 | patch_size_Y: 7 13 | # patch size of temporal BM 14 | patch_size_X_2: 5 15 | patch_size_Y_2: 31 16 | # EventBM parameters 17 | BM_half_slice_thickness: 0.001 18 | BM_min_disparity: 0 19 | BM_max_disparity: 150 20 | BM_step: 3 21 | BM_ZNCC_Threshold: 0.2 22 | BM_bUpDownConfiguration: False 23 | distance_from_last_frame: 0.20 24 | SmoothTimeSurface: True #False 25 | 26 | # Configuration for fusion 27 | fusion_radius: 2 28 | FUSION_STRATEGY: CONST_FRAMES #"CONST_POINTS" # "CONST_FRAMES" 29 | maxNumFusionFrames: 5 #40 30 | maxNumFusionFrames_ln: 5 31 | maxNumFusionPoints: 20000 #8000 #3000 32 | LSnorm: Tdist #Tdist #Tdist #Tdist # l2 33 | Tdist_nu: 2.182 #2.1897 34 | Tdist_scale: 17.277 #16.6397 35 | Tdist_stdvar: 59.763 #56.5347 36 | LSnorm_ln: Tdist #Tdist #Tdist #Tdist # l2 37 | Tdist_nu_ln: 2.182 38 | Tdist_scale_ln: 17.277 39 | Tdist_stdvar_ln: 59.763 40 | 41 | # Configuration for point sampling 42 | Denoising: False 43 | PROCESS_EVENT_NUM: 10000 #20000 44 | PROCESS_EVENT_NUM_AA: 10000 #20000 45 | x_patches: 8 46 | y_patches: 6 47 | select_points_from_AA: True 48 | eta_for_select_points: 0.1 49 | 50 | # Configuration for visualization 51 | Regularization: True 52 | RegularizationRadius: 20 53 | RegularizationMinNeighbours: 32 54 | RegularizationMinCloseNeighbours: 32 55 | bVisualizeGlobalPC: True 56 | visualizeGPC_interval: 0.5 #2[second] 57 | NumGPC_added_per_refresh: 10000 58 | visualize_range: 30 59 | 60 | # Configuration for mapping system 61 | TS_HISTORY_LENGTH: 100 62 | USE_IMU: True 63 | INIT_SGM_DP_NUM_THRESHOLD: 500 64 | mapping_rate_hz: 20 65 | large_scale: True -------------------------------------------------------------------------------- /esvo2_core/cfg/mapping/mapping_rpg_AA.yaml: -------------------------------------------------------------------------------- 1 | # Configuration for depth estimation 2 | invDepth_min_range: 0.2 3 | invDepth_max_range: 2 4 | residual_vis_threshold: 30 #20 #30 #20 #20 5 | residual_vis_threshold_ln: 30 6 | stdVar_vis_threshold: 0.1 #0.1 #0.2 7 | stdVar_vis_threshold_ln: 0.1 #0.1 #0.2 8 | age_max_range: 10 9 | age_vis_threshold: 2 10 | # patch size of static BM 11 | patch_size_X: 15 12 | patch_size_Y: 7 13 | # patch size of temporal BM 14 | patch_size_X_2: 7 15 | patch_size_Y_2: 21 16 | # EventBM parameters 17 | BM_half_slice_thickness: 0.001 18 | BM_min_disparity: 1 19 | BM_max_disparity: 40 20 | BM_step: 1 21 | BM_ZNCC_Threshold: 0.3 22 | BM_bUpDownConfiguration: False 23 | distance_from_last_frame: 0.04 24 | SmoothTimeSurface: True #False 25 | 26 | # Configuration for fusion 27 | fusion_radius: 0 28 | FUSION_STRATEGY: CONST_POINTS #"CONST_POINTS" # "CONST_FRAMES" 29 | maxNumFusionFrames: 40 #40 30 | maxNumFusionFrames_ln: 40 31 | maxNumFusionPoints: 8000 #8000 #3000 32 | LSnorm: Tdist #Tdist #Tdist #Tdist # l2 33 | Tdist_nu: 2.182 #2.1897 34 | Tdist_scale: 17.277 #16.6397 35 | Tdist_stdvar: 59.763 #56.5347 36 | LSnorm_ln: Tdist #Tdist #Tdist #Tdist # l2 37 | Tdist_nu_ln: 2.182 38 | Tdist_scale_ln: 17.277 39 | Tdist_stdvar_ln: 59.763 40 | 41 | # Configuration for point sampling 42 | Denoising: True 43 | PROCESS_EVENT_NUM: 6000 44 | PROCESS_EVENT_NUM_AA: 6000 45 | x_patches: 4 46 | y_patches: 3 47 | select_points_from_AA: True 48 | eta_for_select_points: 0.1 49 | 50 | # Configuration for visualization 51 | Regularization: True 52 | RegularizationRadius: 5 53 | RegularizationMinNeighbours: 8 54 | RegularizationMinCloseNeighbours: 8 55 | 56 | bVisualizeGlobalPC: True 57 | visualizeGPC_interval: 2 #2[second] 58 | NumGPC_added_per_refresh: 3000 59 | visualize_range: 5.0 60 | 61 | # Configuration for mapping system 62 | TS_HISTORY_LENGTH: 100 63 | USE_IMU: True 64 | INIT_SGM_DP_NUM_THRESHOLD: 1500 65 | mapping_rate_hz: 10 66 | large_scale: False -------------------------------------------------------------------------------- /esvo2_core/cfg/mapping/mapping_tum_AA.yaml: -------------------------------------------------------------------------------- 1 | # Configuration for depth estimation 2 | invDepth_min_range: 1 3 | invDepth_max_range: 3 4 | residual_vis_threshold: 30 #20 #30 #20 #20 5 | residual_vis_threshold_ln: 30 6 | stdVar_vis_threshold: 1 #0.1 #0.2 7 | stdVar_vis_threshold_ln: 1 #0.1 #0.2 8 | age_max_range: 10 9 | age_vis_threshold: 1 10 | # patch size of static BM 11 | patch_size_X: 51 12 | patch_size_Y: 21 13 | # patch size of temporal BM 14 | patch_size_X_2: 11 15 | patch_size_Y_2: 41 16 | # EventBM parameters 17 | BM_half_slice_thickness: 0.001 18 | BM_min_disparity: 0 19 | BM_max_disparity: 300 20 | BM_step: 1 21 | BM_ZNCC_Threshold: 0.2 22 | BM_bUpDownConfiguration: False 23 | distance_from_last_frame: 0.04 24 | SmoothTimeSurface: True #False 25 | 26 | # Configuration for fusion 27 | fusion_radius: 2 28 | FUSION_STRATEGY: CONST_FRAMES #"CONST_POINTS" # "CONST_FRAMES" 29 | maxNumFusionFrames: 5 #40 30 | maxNumFusionFrames_ln: 5 31 | maxNumFusionPoints: 30000 #8000 #3000 32 | LSnorm: Tdist #Tdist #Tdist #Tdist # l2 33 | Tdist_nu: 2.182 #2.1897 34 | Tdist_scale: 17.277 #16.6397 35 | Tdist_stdvar: 59.763 #56.5347 36 | LSnorm_ln: Tdist #Tdist #Tdist #Tdist # l2 37 | Tdist_nu_ln: 2.182 38 | Tdist_scale_ln: 17.277 39 | Tdist_stdvar_ln: 59.763 40 | 41 | # Configuration for point sampling 42 | Denoising: False 43 | PROCESS_EVENT_NUM: 10000 #20000 44 | PROCESS_EVENT_NUM_AA: 10000 #20000 45 | x_patches: 16 46 | y_patches: 12 47 | select_points_from_AA: True 48 | eta_for_select_points: 0.0 49 | 50 | # Configuration for visualization 51 | Regularization: True 52 | RegularizationRadius: 5 53 | RegularizationMinNeighbours: 8 54 | RegularizationMinCloseNeighbours: 8 55 | bVisualizeGlobalPC: True 56 | visualizeGPC_interval: 0.5 #2[second] 57 | NumGPC_added_per_refresh: 5000 58 | visualize_range: 30 59 | 60 | # Configuration for mapping system 61 | TS_HISTORY_LENGTH: 100 62 | USE_IMU: True 63 | INIT_SGM_DP_NUM_THRESHOLD: 2000 64 | mapping_rate_hz: 10 65 | large_scale: False -------------------------------------------------------------------------------- /esvo2_core/cfg/mapping/mapping_upenn_AA.yaml: -------------------------------------------------------------------------------- 1 | # Configuration for depth estimation 2 | invDepth_min_range: 0.16 3 | invDepth_max_range: 1 4 | residual_vis_threshold: 20 #20 #30 #20 #20 5 | residual_vis_threshold_ln: 30 6 | stdVar_vis_threshold: 0.15 #0.1 #0.2 7 | stdVar_vis_threshold_ln: 1 #0.1 #0.2 8 | age_max_range: 10 9 | age_vis_threshold: 1 10 | # patch size of static BM 11 | patch_size_X: 15 12 | patch_size_Y: 7 13 | # patch size of temporal BM 14 | patch_size_X_2: 5 15 | patch_size_Y_2: 31 16 | # EventBM parameters 17 | BM_half_slice_thickness: 0.001 18 | BM_min_disparity: 1 19 | BM_max_disparity: 40 20 | BM_step: 1 21 | BM_ZNCC_Threshold: 0.1 22 | BM_bUpDownConfiguration: False 23 | distance_from_last_frame: 0.04 24 | SmoothTimeSurface: False #False 25 | 26 | # Configuration for fusion 27 | fusion_radius: 0 28 | FUSION_STRATEGY: CONST_POINTS #"CONST_POINTS" # "CONST_FRAMES" 29 | maxNumFusionFrames: 40 #40 30 | maxNumFusionFrames_ln: 40 31 | maxNumFusionPoints: 8000 #8000 #3000 32 | LSnorm: Tdist #Tdist #Tdist #Tdist # l2 33 | Tdist_nu: 2.182 #2.1897 34 | Tdist_scale: 17.277 #16.6397 35 | Tdist_stdvar: 59.763 #56.5347 36 | LSnorm_ln: Tdist #Tdist #Tdist #Tdist # l2 37 | Tdist_nu_ln: 2.182 38 | Tdist_scale_ln: 17.277 39 | Tdist_stdvar_ln: 59.763 40 | 41 | # Configuration for point sampling 42 | Denoising: False 43 | PROCESS_EVENT_NUM: 3000 #20000 44 | PROCESS_EVENT_NUM_AA: 3000 #20000 45 | x_patches: 8 46 | y_patches: 6 47 | select_points_from_AA: True 48 | eta_for_select_points: 0.1 49 | 50 | # Configuration for visualization 51 | Regularization: True 52 | RegularizationRadius: 5 53 | RegularizationMinNeighbours: 8 54 | RegularizationMinCloseNeighbours: 8 55 | bVisualizeGlobalPC: True 56 | visualizeGPC_interval: 1 #2[second] 57 | NumGPC_added_per_refresh: 3000 58 | visualize_range: 5.0 59 | 60 | # Configuration for mapping system 61 | TS_HISTORY_LENGTH: 100 62 | USE_IMU: True 63 | INIT_SGM_DP_NUM_THRESHOLD: 500 64 | mapping_rate_hz: 20 65 | large_scale: False -------------------------------------------------------------------------------- /esvo2_core/cfg/mapping/mapping_vector_AA.yaml: -------------------------------------------------------------------------------- 1 | # Configuration for depth estimation 2 | invDepth_min_range: 0.1 3 | invDepth_max_range: 2 4 | residual_vis_threshold: 20 #20 #30 #20 #20 5 | residual_vis_threshold_ln: 20 6 | stdVar_vis_threshold: 0.1 #0.1 #0.2 7 | stdVar_vis_threshold_ln: 1 #0.1 #0.2 8 | age_max_range: 10 9 | age_vis_threshold: 1 10 | # patch size of static BM 11 | patch_size_X: 15 12 | patch_size_Y: 7 13 | # patch size of temporal BM 14 | patch_size_X_2: 5 15 | patch_size_Y_2: 31 16 | # EventBM parameters 17 | BM_half_slice_thickness: 0.001 18 | BM_min_disparity: 0 19 | BM_max_disparity: 300 20 | BM_step: 1 21 | BM_ZNCC_Threshold: 0.2 22 | BM_bUpDownConfiguration: False 23 | distance_from_last_frame: 0.04 24 | SmoothTimeSurface: False #False 25 | 26 | # Configuration for fusion 27 | fusion_radius: 0 28 | FUSION_STRATEGY: CONST_POINTS #"CONST_POINTS" # "CONST_FRAMES" 29 | maxNumFusionFrames: 40 #40 30 | maxNumFusionFrames_ln: 40 31 | maxNumFusionPoints: 12000 #8000 #3000 32 | LSnorm: Tdist #Tdist #Tdist #Tdist # l2 33 | Tdist_nu: 2.182 #2.1897 34 | Tdist_scale: 17.277 #16.6397 35 | Tdist_stdvar: 59.763 #56.5347 36 | LSnorm_ln: Tdist #Tdist #Tdist #Tdist # l2 37 | Tdist_nu_ln: 2.182 38 | Tdist_scale_ln: 17.277 39 | Tdist_stdvar_ln: 59.763 40 | 41 | # Configuration for point sampling 42 | Denoising: False 43 | PROCESS_EVENT_NUM: 3000 #20000 44 | PROCESS_EVENT_NUM_AA: 3000 #20000 45 | x_patches: 8 46 | y_patches: 6 47 | select_points_from_AA: True 48 | eta_for_select_points: 0.0 49 | 50 | # Configuration for visualization 51 | Regularization: True 52 | RegularizationRadius: 20 53 | RegularizationMinNeighbours: 32 54 | RegularizationMinCloseNeighbours: 32 55 | bVisualizeGlobalPC: True 56 | visualizeGPC_interval: 0.5 #2[second] 57 | NumGPC_added_per_refresh: 20 58 | visualize_range: 30 59 | 60 | # Configuration for mapping system 61 | TS_HISTORY_LENGTH: 100 62 | USE_IMU: True 63 | INIT_SGM_DP_NUM_THRESHOLD: 500 64 | mapping_rate_hz: 10 65 | large_scale: False -------------------------------------------------------------------------------- /esvo2_core/cfg/tracking/tracking_dsec_AA.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.02 2 | invDepth_max_range: 0.25 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, Huber 13 | huber_threshold: 50 #50 14 | MIN_NUM_EVENTS: 1000 15 | RegProblemType: 1 # 0 numerical, 1 analytical 16 | SAVE_TRAJECTORY: True 17 | # PATH_TO_SAVE_TRAJECTORY: "/home/bell/data/ESVO2_result/" 18 | PATH_TO_SAVE_TRAJECTORY: "/home/njk/output/ESVO2/" 19 | VISUALIZE_TRAJECTORY: True 20 | USE_IMU: True -------------------------------------------------------------------------------- /esvo2_core/cfg/tracking/tracking_dvx_AA_mapping.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.05 2 | invDepth_max_range: 0.25 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, Huber 13 | huber_threshold: 50 #50 14 | MIN_NUM_EVENTS: 1000 15 | RegProblemType: 1 # 0 numerical, 1 analytical 16 | SAVE_TRAJECTORY: True 17 | # PATH_TO_SAVE_TRAJECTORY: "/home/bell/data/ESVO2_result/" 18 | PATH_TO_SAVE_TRAJECTORY: "/home/njk/output/ESVO2/" 19 | VISUALIZE_TRAJECTORY: True 20 | USE_IMU: True -------------------------------------------------------------------------------- /esvo2_core/cfg/tracking/tracking_dvx_AA_tracking.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.02 2 | invDepth_max_range: 0.33 3 | TS_HISTORY_LENGTH: 100 4 | REF_HISTORY_LENGTH: 10 5 | tracking_rate_hz: 40 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, Huber 13 | huber_threshold: 50 #50 14 | MIN_NUM_EVENTS: 1000 15 | RegProblemType: 1 # 0 numerical, 1 analytical 16 | SAVE_TRAJECTORY: True 17 | # PATH_TO_SAVE_TRAJECTORY: "/home/bell/data/ESVO2_result/" 18 | PATH_TO_SAVE_TRAJECTORY: "/home/njk/output/ESVO2/" 19 | VISUALIZE_TRAJECTORY: True 20 | USE_IMU: True -------------------------------------------------------------------------------- /esvo2_core/cfg/tracking/tracking_online_AA.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.2 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: 300 11 | MAX_ITERATION: 10 12 | LSnorm: Huber #Huber #L2, Huber 13 | huber_threshold: 50 #50 14 | MIN_NUM_EVENTS: 1000 15 | RegProblemType: 1 # 0 numerical, 1 analytical 16 | SAVE_TRAJECTORY: True 17 | # PATH_TO_SAVE_TRAJECTORY: "/home/bell/data/ESVO2_result/" 18 | PATH_TO_SAVE_TRAJECTORY: "/home/njk/output/ESVO2/" 19 | VISUALIZE_TRAJECTORY: True 20 | USE_IMU: False 21 | -------------------------------------------------------------------------------- /esvo2_core/cfg/tracking/tracking_rpg_AA.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: 300 11 | MAX_ITERATION: 20 12 | LSnorm: Huber #Huber #L2, Huber 13 | huber_threshold: 50 #50 14 | MIN_NUM_EVENTS: 1000 15 | RegProblemType: 1 # 0 numerical, 1 analytical 16 | SAVE_TRAJECTORY: True 17 | # PATH_TO_SAVE_TRAJECTORY: "/home/bell/data/ESVO2_result/" 18 | PATH_TO_SAVE_TRAJECTORY: "/home/njk/output/ESVO2/" 19 | VISUALIZE_TRAJECTORY: True 20 | USE_IMU: False 21 | 22 | -------------------------------------------------------------------------------- /esvo2_core/cfg/tracking/tracking_tum_AA.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 1 2 | invDepth_max_range: 3 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: 20 12 | LSnorm: Huber #Huber #L2, Huber 13 | huber_threshold: 50 #50 14 | MIN_NUM_EVENTS: 1000 15 | RegProblemType: 1 # 0 numerical, 1 analytical 16 | SAVE_TRAJECTORY: True 17 | # PATH_TO_SAVE_TRAJECTORY: "/home/bell/data/ESVO2_result/" 18 | PATH_TO_SAVE_TRAJECTORY: "/home/njk/output/ESVO2/" 19 | VISUALIZE_TRAJECTORY: True 20 | USE_IMU: True -------------------------------------------------------------------------------- /esvo2_core/cfg/tracking/tracking_upenn_AA.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.16 2 | invDepth_max_range: 1 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: 7 9 | MAX_REGISTRATION_POINTS: 4000 10 | BATCH_SIZE: 300 11 | MAX_ITERATION: 10 12 | LSnorm: Huber #Huber #L2, Huber 13 | huber_threshold: 50 #50 14 | MIN_NUM_EVENTS: 1000 15 | RegProblemType: 1 # 0 numerical, 1 analytical 16 | SAVE_TRAJECTORY: True 17 | # PATH_TO_SAVE_TRAJECTORY: "/home/bell/data/ESVO2_result/" 18 | PATH_TO_SAVE_TRAJECTORY: "/home/njk/output/ESVO2/" 19 | VISUALIZE_TRAJECTORY: True 20 | USE_IMU: True 21 | -------------------------------------------------------------------------------- /esvo2_core/cfg/tracking/tracking_vector_AA.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.1 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: 300 11 | MAX_ITERATION: 10 12 | LSnorm: Huber #Huber #L2, Huber 13 | huber_threshold: 50 #50 14 | MIN_NUM_EVENTS: 1000 15 | RegProblemType: 1 # 0 numerical, 1 analytical 16 | SAVE_TRAJECTORY: True 17 | # PATH_TO_SAVE_TRAJECTORY: "/home/bell/data/ESVO2_result/" 18 | PATH_TO_SAVE_TRAJECTORY: "/home/njk/output/ESVO2/" 19 | VISUALIZE_TRAJECTORY: True 20 | USE_IMU: True -------------------------------------------------------------------------------- /esvo2_core/esvo2_mvstereo.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1 10 | - /TF1/Frames1 11 | - /TF1/Tree1 12 | - /Pose1 13 | - /PointCloud21 14 | Splitter Ratio: 0.5 15 | Tree Height: 1762 16 | - Class: rviz/Selection 17 | Name: Selection 18 | - Class: rviz/Tool Properties 19 | Expanded: 20 | - /2D Pose Estimate1 21 | - /2D Nav Goal1 22 | - /Publish Point1 23 | Name: Tool Properties 24 | Splitter Ratio: 0.5886790156364441 25 | - Class: rviz/Views 26 | Expanded: 27 | - /Current View1 28 | Name: Views 29 | Splitter Ratio: 0.5 30 | - Class: rviz/Time 31 | Experimental: false 32 | Name: Time 33 | SyncMode: 0 34 | SyncSource: PointCloud2 35 | Preferences: 36 | PromptSaveOnExit: true 37 | Toolbars: 38 | toolButtonStyle: 2 39 | Visualization Manager: 40 | Class: "" 41 | Displays: 42 | - Alpha: 0.5 43 | Cell Size: 1 44 | Class: rviz/Grid 45 | Color: 160; 160; 164 46 | Enabled: true 47 | Line Style: 48 | Line Width: 0.029999999329447746 49 | Value: Lines 50 | Name: Grid 51 | Normal Cell Count: 0 52 | Offset: 53 | X: 0 54 | Y: 0 55 | Z: 0 56 | Plane: XY 57 | Plane Cell Count: 10 58 | Reference Frame: 59 | Value: true 60 | - Class: rviz/TF 61 | Enabled: true 62 | Frame Timeout: 999 63 | Frames: 64 | All Enabled: true 65 | Marker Scale: 1 66 | Name: TF 67 | Show Arrows: true 68 | Show Axes: true 69 | Show Names: true 70 | Tree: 71 | {} 72 | Update Interval: 0 73 | Value: true 74 | - Alpha: 1 75 | Axes Length: 0.15000000596046448 76 | Axes Radius: 0.019999999552965164 77 | Class: rviz/Pose 78 | Color: 255; 25; 0 79 | Enabled: true 80 | Head Length: 0.30000001192092896 81 | Head Radius: 0.10000000149011612 82 | Name: Pose 83 | Shaft Length: 1 84 | Shaft Radius: 0.05000000074505806 85 | Shape: Axes 86 | Topic: /optitrack/davis_stereo 87 | Unreliable: false 88 | Value: true 89 | - Class: rviz/Axes 90 | Enabled: false 91 | Length: 1 92 | Name: Axes 93 | Radius: 0.10000000149011612 94 | Reference Frame: 95 | Value: false 96 | - Alpha: 1 97 | Autocompute Intensity Bounds: true 98 | Autocompute Value Bounds: 99 | Max Value: 10 100 | Min Value: -10 101 | Value: true 102 | Axis: Z 103 | Channel Name: z 104 | Class: rviz/PointCloud2 105 | Color: 255; 255; 255 106 | Color Transformer: Intensity 107 | Decay Time: 0 108 | Enabled: true 109 | Invert Rainbow: true 110 | Max Color: 255; 255; 255 111 | Max Intensity: 1.0458288192749023 112 | Min Color: 0; 0; 0 113 | Min Intensity: -29.558752059936523 114 | Name: PointCloud2 115 | Position Transformer: XYZ 116 | Queue Size: 10 117 | Selectable: true 118 | Size (Pixels): 3 119 | Size (m): 0.05000000074505806 120 | Style: Points 121 | Topic: /esvo2_mvstereo/pointcloud_world 122 | Unreliable: false 123 | Use Fixed Frame: true 124 | Use rainbow: true 125 | Value: true 126 | Enabled: true 127 | Global Options: 128 | Background Color: 255; 255; 255 129 | Default Light: true 130 | Fixed Frame: map 131 | Frame Rate: 30 132 | Name: root 133 | Tools: 134 | - Class: rviz/Interact 135 | Hide Inactive Objects: true 136 | - Class: rviz/MoveCamera 137 | - Class: rviz/Select 138 | - Class: rviz/FocusCamera 139 | - Class: rviz/Measure 140 | - Class: rviz/SetInitialPose 141 | Theta std deviation: 0.2617993950843811 142 | Topic: /initialpose 143 | X std deviation: 0.5 144 | Y std deviation: 0.5 145 | - Class: rviz/SetGoal 146 | Topic: /move_base_simple/goal 147 | - Class: rviz/PublishPoint 148 | Single click: true 149 | Topic: /clicked_point 150 | Value: true 151 | Views: 152 | Current: 153 | Class: rviz/Orbit 154 | Distance: 4.184921741485596 155 | Enable Stereo Rendering: 156 | Stereo Eye Separation: 0.05999999865889549 157 | Stereo Focal Distance: 1 158 | Swap Stereo Eyes: false 159 | Value: false 160 | Focal Point: 161 | X: 1.4528931379318237 162 | Y: 0.03556236997246742 163 | Z: 0.8511919975280762 164 | Focal Shape Fixed Size: true 165 | Focal Shape Size: 0.05000000074505806 166 | Invert Z Axis: false 167 | Name: Current View 168 | Near Clip Distance: 0.009999999776482582 169 | Pitch: 0.2197979986667633 170 | Target Frame: 171 | Value: Orbit (rviz) 172 | Yaw: 2.5921530723571777 173 | Saved: ~ 174 | Window Geometry: 175 | Displays: 176 | collapsed: false 177 | Height: 2088 178 | Hide Left Dock: false 179 | Hide Right Dock: false 180 | QMainWindow State: 000000ff00000000fd00000004000000000000031900000778fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004300000778000000d400fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000778fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004300000778000000b500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000ebd00000044fc0100000002fb0000000800540069006d0065010000000000000ebd000003b700fffffffb0000000800540069006d0065010000000000000450000000000000000000000a890000077800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 181 | Selection: 182 | collapsed: false 183 | Time: 184 | collapsed: false 185 | Tool Properties: 186 | collapsed: false 187 | Views: 188 | collapsed: false 189 | Width: 3773 190 | X: 67 191 | Y: 37 192 | -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/container/CameraSystem.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO2_CORE_CONTAINER_CAMERASYSTEM_H 2 | #define ESVO2_CORE_CONTAINER_CAMERASYSTEM_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace esvo2_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 | std::vector &vT_b_c); 30 | 31 | void preComputeRectifiedCoordinate(); 32 | 33 | Eigen::Matrix getRectifiedUndistortedCoordinate(int xcoor, int ycoor); 34 | 35 | void cam2World(const Eigen::Vector2d &x, double invDepth, Eigen::Vector3d &p); 36 | 37 | void world2Cam(const Eigen::Vector3d &p, Eigen::Vector2d &x); 38 | 39 | public: 40 | size_t width_, height_; 41 | std::string cameraName_; 42 | std::string distortion_model_; 43 | Eigen::Matrix D_; 44 | Eigen::Matrix3d K_, K_inv_; 45 | Eigen::Matrix3d RectMat_; 46 | Eigen::Matrix P_; 47 | Eigen::Matrix T_b_c_; 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 | } 70 | } 71 | 72 | #endif //ESVO2_CORE_CONTAINER_CAMERASYSTEM_H -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/container/DepthMap.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO2_CORE_DEPTHMAP_H 2 | #define ESVO2_CORE_DEPTHMAP_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace esvo2_core 9 | { 10 | using namespace tools; 11 | namespace container 12 | { 13 | using DepthMap = SmartGrid; 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 //ESVO2_CORE_DEPTHMAP_H 51 | -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/container/DepthPoint.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO2_CORE_CONTAINER_DEPTHPOINT_H 2 | #define ESVO2_CORE_CONTAINER_DEPTHPOINT_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace esvo2_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 &residual(); 42 | const double &residual() const; 43 | 44 | size_t &age(); 45 | const size_t &age() const; 46 | 47 | void boundVariance(); 48 | 49 | void update(double invDepth, double variance);// Gaussian distribution 50 | void update_studentT(double invDepth, double scale2, double variance, double nu); // T distribution 51 | 52 | void update_p_cam(const Eigen::Vector3d &p); 53 | const Eigen::Vector3d &p_cam() const; 54 | 55 | void updatePose(Eigen::Matrix &T_world_cam);// used in the fusion of each newly estimate. 56 | // Therefore, it is not necessary to call updatePose for those created in the fusion. Because those share 57 | // the pose of the fused depthFrame. 58 | 59 | const Eigen::Matrix &T_world_cam() const; 60 | 61 | bool valid() const; 62 | bool valid(double var_threshold, 63 | double age_threshold, 64 | double invDepth_max, 65 | double invDepth_min) const; 66 | 67 | //copy an element without the location 68 | void copy(const DepthPoint ©); 69 | 70 | private: 71 | //coordinates in the image 72 | size_t row_; 73 | size_t col_; 74 | Eigen::Vector2d x_; 75 | 76 | //inverse depth parameters 77 | double invDepth_; 78 | double scaleSquared_;// squared scale 79 | double nu_; 80 | double variance_; 81 | double residual_; 82 | 83 | // count the number of fusion has been applied on a depth point 84 | size_t age_; 85 | 86 | //3D point (updated in reference frame before tracking) 87 | Eigen::Vector3d p_cam_; 88 | Eigen::Matrix T_world_cam_; 89 | }; 90 | } 91 | } 92 | 93 | #endif //ESVO2_CORE_CONTAINER_DEPTHPOINT_H -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/container/EventMatchPair.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO2_CORE_CORE_EVENTMATCHPAIR_H 2 | #define ESVO2_CORE_CORE_EVENTMATCHPAIR_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace esvo2_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 | // for debug 40 | Eigen::Vector2d x_last_; 41 | double error_ = 10000; 42 | double lr_cost; 43 | double lr_depth; 44 | double ln_cost; 45 | double ln_depth; 46 | vector costs_; 47 | }; 48 | } 49 | } 50 | 51 | #endif //ESVO2_CORE_CORE_EVENTMATCHPAIR_H 52 | -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/container/ResidualItem.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO2_CORE_CONTAINER_RESIDUALITEM_H 2 | #define ESVO2_CORE_CONTAINER_RESIDUALITEM_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace esvo2_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 esvo2_core 32 | 33 | #endif //ESVO2_CORE_CONTAINER_RESIDUALITEM_H 34 | -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/core/BackendOptimization.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO2_CORE_CORE_BACKEND_H 2 | #define ESVO2_CORE_CORE_BACKEND_H 3 | 4 | // #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | namespace esvo2_core 20 | { 21 | using namespace container; 22 | using namespace tools; 23 | using namespace factor; 24 | namespace core 25 | { 26 | 27 | const int WINDOW_SIZE = 4; 28 | struct DepthPointFrame 29 | { 30 | ros::Time timestamp_; 31 | std::vector DepthPoints_; 32 | 33 | DepthPointFrame(ros::Time t, std::vector DepthPoints) 34 | { 35 | timestamp_ = t; 36 | DepthPoints_ = DepthPoints; 37 | } 38 | 39 | int size() 40 | { 41 | return DepthPoints_.size(); 42 | } 43 | }; 44 | 45 | class BackendOptimization 46 | { 47 | public: 48 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 49 | 50 | BackendOptimization(const CameraSystem::Ptr &camSysPtr); 51 | 52 | void setProblem(std::deque *dqvDepthPoints, 53 | TimeSurfaceHistory *pTS_history, 54 | ros::Publisher *pV_ba_bg_pub, 55 | bool bUSE_IMU); 56 | void sloveProblem(); 57 | 58 | void double2Vector(double para_Pose[][7], double para_SpeedBias[][9]); 59 | void Tcam2Timu(esvo2_core::container::TimeSurfaceHistory::iterator ts_obs, 60 | esvo2_core::container::TimeSurfaceHistory::iterator last_obs, 61 | double para_Pose[][7], 62 | int i); 63 | void vector2Double(esvo2_core::container::TimeSurfaceHistory::iterator ts_obs, 64 | esvo2_core::container::TimeSurfaceHistory::iterator last_obs, 65 | double para_Pose[][7], 66 | double para_SpeedBias[][9], 67 | int i); 68 | void publishVBaBg(double time_v); 69 | bool CalibrationExRotation(Eigen::Matrix3d &calib_ric_result); 70 | bool LinearAlignment(Eigen::Vector3d &g, Eigen::VectorXd &x); 71 | void solveGyroscopeBias(Eigen::Vector3d &Bgs); 72 | void RefineGravity(Eigen::Vector3d &g, Eigen::VectorXd &x); 73 | void slideWindow(); 74 | bool isOrthogonal(const Eigen::Matrix3d &matrix); 75 | Eigen::Matrix3d fixRotationMatrix(const Eigen::Matrix3d &R); 76 | // bool getPoseAt(const ros::Time &t, esvo2_core::Transformation &Tr, const std::string& source_frame); 77 | 78 | private: 79 | CameraSystem::Ptr camSysPtr_; 80 | std::deque *pDepthPoints_; 81 | TimeSurfaceHistory *pTS_history_; 82 | ros::Publisher *pV_ba_bg_pub_; 83 | bool initVsFlag, bUSE_IMU_; 84 | 85 | Eigen::Vector3d TIC_ = Eigen::Vector3d::Zero(); 86 | Eigen::Matrix3d RIC_; 87 | Eigen::Matrix4d T_i_c_; 88 | 89 | public: 90 | int frame_count; 91 | Eigen::Vector3d g_optimal; 92 | Eigen::Vector3d Ps[(WINDOW_SIZE + 1)]; 93 | Eigen::Vector3d Ps_before[(WINDOW_SIZE + 1)]; 94 | Eigen::Vector3d Vs[(WINDOW_SIZE + 1)]; 95 | Eigen::Matrix3d Rs[(WINDOW_SIZE + 1)]; 96 | Eigen::Vector3d Bas[(WINDOW_SIZE + 1)]; 97 | Eigen::Vector3d Bgs[(WINDOW_SIZE + 1)]; 98 | 99 | IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)]; 100 | Eigen::Vector3d acc_0, gyr_0; 101 | std::shared_ptr tf_; 102 | std::vector time_of_pts_; 103 | Eigen::Matrix3d K_, K_of_pts_, K_pts_inv_; 104 | Eigen::Matrix4d T_rect_raw_; 105 | const string world_frame_id_ = "world"; 106 | const std::string dvs_frame_id_ = "dvs"; 107 | Eigen::Matrix4d T_wopt_window0_; 108 | }; 109 | } 110 | } 111 | 112 | #endif // ESVO2_CORE_CORE_DEPTHPROBLEM_H 113 | -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/core/DepthFusion.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO2_CORE_CORE_DEPTHFUSION_H 2 | #define ESVO2_CORE_CORE_DEPTHFUSION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace esvo2_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 //ESVO2_CORE_CORE_DEPTHFUSION_H -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/core/DepthProblem.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO2_CORE_CORE_DEPTHPROBLEM_H 2 | #define ESVO2_CORE_CORE_DEPTHPROBLEM_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace esvo2_core 10 | { 11 | using namespace container; 12 | using namespace tools; 13 | namespace core 14 | { 15 | struct DepthProblemConfig 16 | { 17 | using Ptr = std::shared_ptr; 18 | DepthProblemConfig( 19 | size_t patchSize_X, 20 | size_t patchSize_Y, 21 | const std::string &LSnorm, 22 | double td_nu, 23 | double td_scale, 24 | size_t MAX_ITERATION, 25 | size_t RegularizationRadius, 26 | size_t RegularizationMinNeighbours, 27 | size_t RegularizationMinCloseNeighbours): 28 | patchSize_X_(patchSize_X), 29 | patchSize_Y_(patchSize_Y), 30 | LSnorm_(LSnorm), 31 | td_nu_(td_nu), 32 | td_scale_(td_scale), 33 | td_scaleSquared_(pow(td_scale,2)), 34 | td_stdvar_(sqrt(td_nu / (td_nu - 2) * td_scaleSquared_)), 35 | MAX_ITERATION_(MAX_ITERATION), 36 | RegularizationRadius_(RegularizationRadius), 37 | RegularizationMinNeighbours_(RegularizationMinNeighbours), 38 | RegularizationMinCloseNeighbours_(RegularizationMinCloseNeighbours) 39 | {} 40 | 41 | size_t patchSize_X_, patchSize_Y_; 42 | std::string LSnorm_; 43 | double td_nu_; 44 | double td_scale_; 45 | double td_scaleSquared_;// td_scale_^2 46 | double td_stdvar_;// sigma 47 | size_t MAX_ITERATION_; 48 | size_t RegularizationRadius_; 49 | size_t RegularizationMinNeighbours_; 50 | size_t RegularizationMinCloseNeighbours_; 51 | }; 52 | 53 | struct DepthProblem : public factor::OptimizationFunctor 54 | { 55 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 56 | DepthProblem( 57 | const DepthProblemConfig::Ptr & dpConfig_ptr, 58 | const CameraSystem::Ptr & camSysPtr ); 59 | 60 | void setProblem(Eigen::Vector2d & coor); 61 | 62 | void setProblem( 63 | Eigen::Vector2d & coor, 64 | Eigen::Matrix & T_world_virtual, 65 | constStampedTimeSurfaceObs* pStampedTsObs, 66 | bool problem_lr); 67 | 68 | 69 | // function that is inherited from factor::OptimizationFunctor 70 | int operator()( const Eigen::VectorXd &x, Eigen::VectorXd & fvec ) const; 71 | 72 | int df(const Eigen::VectorXd &x, Eigen::MatrixXd &fjac) const; 73 | 74 | // int inputs() const {return 1;} 75 | // int values() const {return 1;} 76 | // utils 77 | bool warping( 78 | const Eigen::Vector2d &x, 79 | double d, 80 | const Eigen::Matrix &T_left_virtual, 81 | Eigen::Vector2d &x1_s, 82 | Eigen::Vector2d &x2_s) const; 83 | 84 | bool patchInterpolation( 85 | const Eigen::MatrixXd &img, 86 | const Eigen::Vector2d &location, 87 | Eigen::MatrixXd &patch, 88 | bool debug = false) const; 89 | 90 | bool patchInterpolation( 91 | const Eigen::MatrixXd &img, 92 | const Eigen::Vector2d &location, 93 | Eigen::MatrixXd &patch, 94 | int wx, 95 | int wy, 96 | bool debug = false) const; 97 | 98 | 99 | // variables 100 | CameraSystem::Ptr camSysPtr_; 101 | DepthProblemConfig::Ptr dpConfigPtr_; 102 | Eigen::Vector2d coordinate_; 103 | Eigen::Matrix T_world_virtual_; 104 | std::vector, 105 | Eigen::aligned_allocator > > vT_left_virtual_; 106 | constStampedTimeSurfaceObs* pStampedTsObs_; 107 | bool problem_lr_; 108 | Eigen::Matrix4d T_last_now_; 109 | }; 110 | } 111 | } 112 | 113 | 114 | #endif //ESVO2_CORE_CORE_DEPTHPROBLEM_H 115 | -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/core/DepthProblemSolver.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO2_CORE_CORE_DEPTHPROBLEMSOLVER2_H 2 | #define ESVO2_CORE_CORE_DEPTHPROBLEMSOLVER2_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace esvo2_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 | constStampedTimeSurfaceObs* 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 | bool slove_lr = true); 41 | 42 | virtual ~DepthProblemSolver(); 43 | 44 | void solve( 45 | std::vector* pvEMP, 46 | constStampedTimeSurfaceObs* pStampedTsObs, 47 | std::vector &vdp ); 48 | 49 | void solve_multiple_problems(Job &job); 50 | 51 | bool solve_single_problem_numerical( 52 | double d_init, 53 | std::shared_ptr< Eigen::NumericalDiff > & dProblemPtr, 54 | double* result); 55 | 56 | bool solve_single_problem_analytical( 57 | double d_init, 58 | std::shared_ptr< DepthProblem > & dProblemPtr, 59 | double* result); 60 | 61 | bool init_single_point( 62 | Job & job); 63 | 64 | void pointCulling( 65 | std::vector &vdp, 66 | double std_variance_threshold, 67 | double cost_threshold, 68 | double invDepth_min_range, 69 | double invDepth_max_range); 70 | 71 | DepthProblemType getProblemType(); 72 | 73 | private: 74 | CameraSystem::Ptr & camSysPtr_; 75 | std::shared_ptr dpConfigPtr_; 76 | size_t NUM_THREAD_; 77 | DepthProblemType dpType_; 78 | bool slove_lr_; 79 | }; 80 | } 81 | } 82 | #endif //ESVO2_CORE_CORE_DEPTHPROBLEMSOLVER2_H -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/core/DepthRegularization.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO2_CORE_CORE_DEPTHREGULARIZATION_H 2 | #define ESVO2_CORE_CORE_DEPTHREGULARIZATION_H 3 | 4 | #include 5 | #include 6 | #include 7 | namespace esvo2_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 | }// esvo2_core 30 | 31 | #endif //ESVO2_CORE_CORE_DEPTHREGULARIZATION_H -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/core/EventBM.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO2_CORE_CORE_EVENTBM_H 2 | #define ESVO2_CORE_CORE_EVENTBM_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace esvo2_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 | 27 | struct Job2 28 | { 29 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 30 | size_t i_thread_; 31 | std::vector* pvEventPairPtr_; 32 | std::vector >* pvpDisparitySearchBound_; 33 | std::shared_ptr > pvEventMatchPair_; 34 | }; 35 | 36 | public: 37 | EventBM( 38 | CameraSystem::Ptr camSysPtr, 39 | size_t numThread = 1, 40 | bool bSmoothTS = false, 41 | size_t patch_size_X = 25, 42 | size_t patch_size_Y = 25, 43 | size_t min_disparity = 1, 44 | size_t max_disparity = 40, 45 | size_t step = 1, 46 | double ZNCC_Threshold = 0.1, 47 | bool bUpDownConfiguration = false);// bUpDownConfiguration determines the epipolar searching direction (UpDown or LeftRight). 48 | virtual ~EventBM(); 49 | 50 | void resetParameters( 51 | size_t patch_size_X, 52 | size_t patch_size_Y, 53 | size_t min_disparity, 54 | size_t max_disparity, 55 | size_t step, 56 | double ZNCC_Threshold, 57 | bool bDonwUpConfiguration, 58 | size_t patch_size_X_2 = 5, 59 | size_t patch_size_Y_2 = 31); 60 | 61 | void createMatchProblem( 62 | constStampedTimeSurfaceObs * pStampedTsObs, 63 | StampTransformationMap * pSt_map, 64 | std::vector* pvEventsPtr); 65 | void createMatchProblemTwoFrames( 66 | constStampedTimeSurfaceObs * pStampedTsObs, 67 | StampTransformationMap * pSt_map, 68 | std::vector* pvEventsPtr, 69 | std::vector * vEMP); 70 | 71 | bool match_an_event( 72 | dvs_msgs::Event *pEvent, 73 | std::pair& pDisparityBound, 74 | EventMatchPair &emPair); 75 | bool match_an_event2( 76 | const dvs_msgs::Event *pEvent, 77 | std::pair& pDisparityBound, 78 | EventMatchPair &emPair); 79 | bool match_an_event3( 80 | dvs_msgs::Event *pEvent, 81 | std::pair& pDisparityBound, 82 | EventMatchPair &emPair, 83 | Eigen::MatrixXd& TS_left, 84 | Eigen::MatrixXd& TS_right); 85 | bool match_an_eventTwoFrames( 86 | EventMatchPair EMP_lr, 87 | std::pair& pDisparityBound, 88 | EventMatchPair &emPair); 89 | 90 | void match_all_SingleThread(std::vector &vEMP); 91 | void match_all_SingleThreadTwoFrames(std::vector &vEMP, std::vector &vEMP_fail); 92 | void match_all_HyperThread(std::vector &vEMP); 93 | void match_all_HyperThreadTwoFrames(std::vector &vEMP, std::vector &vEMP_fail); 94 | 95 | static double zncc_cost(Eigen::MatrixXd &patch_left, Eigen::MatrixXd &patch_right, bool normalized = false); 96 | double zncc_cost_fast(Eigen::VectorXd &colSum, Eigen::VectorXd &varSum, Eigen::MatrixXd &patch_left, 97 | Eigen::MatrixXd &patch_right, int &disp_to_rm, int &step_to_rm, double &mean_l, double &Tl_2, double &Tr, double &Tr_quare); 98 | double zncc_cost2(Eigen::MatrixXd &patch_left, Eigen::MatrixXd &patch_right, double &var_l, double &mean_l); 99 | float getMSSIM( const cv::Mat& i1, const cv::Mat& i2); 100 | private: 101 | void match(EventBM::Job &job); 102 | void match2(EventBM::Job &job); 103 | void match2_TwoFrames(EventBM::Job2& job); 104 | bool epipolarSearching(double& min_cost, Eigen::Vector2i& bestMatch, size_t& bestDisp, Eigen::MatrixXd& patch_dst, 105 | size_t searching_start_pos, size_t searching_end_pos, size_t searching_step, 106 | Eigen::Vector2i& x1, Eigen::MatrixXd& patch_src, bool bDownUpConfiguration = false); 107 | bool epipolarSearchingTwoFrames(double& min_cost, Eigen::Vector2i& bestMatch, int & bestDisp, Eigen::MatrixXd& patch_dst, 108 | int searching_start_pos, int searching_end_pos, size_t searching_step, 109 | Eigen::Vector2i& x1, Eigen::MatrixXd& patch_src, double dx, double dy, std::vector& costs, 110 | std::vector& fine_search, bool bDownUpConfiguration = false); 111 | 112 | 113 | bool epipolarSearchingCoarse( 114 | double& min_cost, Eigen::Vector2i& bestMatch, size_t& bestDisp, Eigen::MatrixXd& patch_dst, 115 | size_t searching_start_pos, size_t searching_end_pos, size_t searching_step, 116 | Eigen::Vector2i& x1, Eigen::MatrixXd& patch_src, bool bUpDownConfiguration, vector& searching_or_not, 117 | Eigen::VectorXd &colSum, Eigen::VectorXd &varSum, double &mean_l, double &Tl_2, double &Tr, double &Tr_quare); 118 | 119 | bool epipolarSearchingFine(double& min_cost, Eigen::Vector2i& bestMatch, size_t& bestDisp, Eigen::MatrixXd& patch_dst, 120 | vector& searching_radius, 121 | Eigen::Vector2i& x1, Eigen::MatrixXd& patch_src, bool bUpDownConfiguration); 122 | 123 | bool isNeedSSIM(std::vector& costs); 124 | bool isValidPatch(Eigen::Vector2i& x, Eigen::Vector2i& left_top, int size_y, int size_x); 125 | double triangulatePoint(Eigen::Vector2d &point0, Eigen::Vector2d &point1, double depth); 126 | private: 127 | CameraSystem::Ptr camSysPtr_; 128 | constStampedTimeSurfaceObs* pStampedTsObs_; 129 | StampTransformationMap * pSt_map_; 130 | std::vector vEventsPtr_; 131 | std::vector > vpDisparitySearchBound_; 132 | Sobel sb_; 133 | 134 | size_t NUM_THREAD_; 135 | bool bSmoothTS_; 136 | size_t patch_size_X_; 137 | size_t patch_size_Y_; 138 | size_t min_disparity_; 139 | size_t max_disparity_; 140 | size_t step_; 141 | double ZNCC_Threshold_; 142 | double ZNCC_MAX_; 143 | bool bUpDownConfiguration_; 144 | 145 | size_t coarseSearchingFailNum_, fineSearchingFailNum_, infoNoiseRatioLowNum_, outsideNum_, dxdyBigNum_; 146 | int coarseSearchingNum_ = 0; 147 | 148 | //to rm 149 | cv::Mat DispMap_; 150 | std::vector vEMP_lr_; 151 | Eigen::Matrix4d T_last_now_; 152 | Eigen::Matrix3d K_inv_; 153 | Eigen::Matrix3d K_rtf_, K_rtf_inv_, R_ori_rtf_; 154 | size_t patch_size_X_2_, patch_size_Y_2_; 155 | int num_of_need_ssim_, num_of_success_ssim_, avg_num_of_ssim_; 156 | double fine_searching_time_, croase_searching_time_;// croase_searching_time_1, croase_searching_time_2, croase_searching_time_3, croase_searching_time_4, pre_time_; 157 | int zncc_num_; 158 | bool use_fast_zncc_; 159 | std::mutex time_mutex_; 160 | }; 161 | }// core 162 | }// esvo2_core 163 | 164 | #endif //ESVO2_CORE_CORE_EVENTBM_H 165 | -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/core/RegProblemLM.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO2_CORE_CORE_REGPROBLEMLM_H 2 | #define ESVO2_CORE_CORE_REGPROBLEMLM_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | namespace esvo2_core 14 | { 15 | using namespace container; 16 | using namespace tools; 17 | namespace core 18 | { 19 | struct RegProblemConfig 20 | { 21 | using Ptr = std::shared_ptr; 22 | RegProblemConfig( 23 | size_t patchSize_X, 24 | size_t patchSize_Y, 25 | size_t kernelSize, 26 | const std::string &LSnorm, 27 | double huber_threshold, 28 | double invDepth_min_range = 0.2, 29 | double invDepth_max_range = 2.0, 30 | const size_t MIN_NUM_EVENTS = 1000, 31 | const size_t MAX_REGISTRATION_POINTS = 500, 32 | const size_t BATCH_SIZE = 200, 33 | const size_t MAX_ITERATION = 10): 34 | patchSize_X_(patchSize_X), 35 | patchSize_Y_(patchSize_Y), 36 | kernelSize_(kernelSize), 37 | LSnorm_(LSnorm), 38 | huber_threshold_(huber_threshold), 39 | invDepth_min_range_(invDepth_min_range), 40 | invDepth_max_range_(invDepth_max_range), 41 | MIN_NUM_EVENTS_(MIN_NUM_EVENTS), 42 | MAX_REGISTRATION_POINTS_(MAX_REGISTRATION_POINTS), 43 | BATCH_SIZE_(BATCH_SIZE), 44 | MAX_ITERATION_(MAX_ITERATION) 45 | {} 46 | 47 | size_t patchSize_X_, patchSize_Y_; 48 | size_t kernelSize_; 49 | std::string LSnorm_; 50 | double huber_threshold_; 51 | double invDepth_min_range_; 52 | double invDepth_max_range_; 53 | size_t MIN_NUM_EVENTS_; 54 | size_t MAX_REGISTRATION_POINTS_; 55 | size_t BATCH_SIZE_; 56 | size_t MAX_ITERATION_; 57 | }; 58 | 59 | struct RefFrame 60 | { 61 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 62 | ros::Time t_; 63 | std::vector vPointXYZPtr_; 64 | Transformation tr_; 65 | }; 66 | 67 | struct CurFrame 68 | { 69 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 70 | ros::Time t_; 71 | TimeSurfaceObservation* pTsObs_; 72 | Transformation tr_; 73 | Transformation tr_old_; 74 | size_t numEventsSinceLastObs_; 75 | }; 76 | 77 | struct RegProblemLM : public factor::OptimizationFunctor 78 | { 79 | struct Job 80 | { 81 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 82 | ResidualItems* pvRI_; 83 | const TimeSurfaceObservation* pTsObs_; 84 | const Eigen::Matrix4d* T_left_ref_; 85 | size_t i_thread_; 86 | }; 87 | 88 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 89 | RegProblemLM( 90 | const CameraSystem::Ptr& camSysPtr, 91 | const RegProblemConfig::Ptr& rpConfig_ptr, 92 | size_t numThread = 1); 93 | void setProblem(RefFrame* ref, CurFrame* cur, bool bComputeGrad = false); 94 | void setStochasticSampling(size_t offset, size_t N); 95 | 96 | void getWarpingTransformation( 97 | Eigen::Matrix4d& warpingTransf, 98 | const Eigen::Matrix& x) const; 99 | void addMotionUpdate( const Eigen::Matrix& dx ); 100 | void setPose(); 101 | Eigen::Matrix4d getPose(); 102 | 103 | // optimization 104 | int operator()( const Eigen::Matrix& x, Eigen::VectorXd& fvec ) const; 105 | void thread( Job& job ) const; 106 | int df( const Eigen::Matrix& x, Eigen::MatrixXd& fjac ) const; 107 | void computeJ_G(const Eigen::Matrix&x, Eigen::Matrix& J_G); 108 | 109 | // utils 110 | bool isValidPatch( 111 | Eigen::Vector2d& patchCentreCoord, 112 | Eigen::MatrixXi& mask, 113 | size_t wx, 114 | size_t wy) const; 115 | 116 | bool reprojection( 117 | const Eigen::Vector3d& p, 118 | const Eigen::Matrix4d& warpingTransf, 119 | Eigen::Vector2d &x1_s) const; 120 | 121 | bool patchInterpolation( 122 | const Eigen::MatrixXd &img, 123 | const Eigen::Vector2d &location, 124 | Eigen::MatrixXd &patch, 125 | bool debug = false) const; 126 | // 127 | CameraSystem::Ptr camSysPtr_; 128 | RegProblemConfig::Ptr rpConfigPtr_; 129 | size_t patchSize_; 130 | 131 | size_t NUM_THREAD_; 132 | size_t numPoints_; 133 | size_t numBatches_; 134 | 135 | ResidualItems ResItems_, ResItemsStochSampled_; 136 | std::vector VisualizationIdx_; 137 | TimeSurfaceObservation* pTsObs_; 138 | RefFrame* ref_; 139 | CurFrame* cur_; 140 | 141 | Eigen::Matrix T_world_left_;// to record the current pose 142 | Eigen::Matrix T_world_ref_;// to record the ref pose (local ref map) 143 | Eigen::Matrix3d R_;//R_ref_cur_; 144 | Eigen::Vector3d t_;//t_ref_cur 145 | 146 | // Jacobian Constant 147 | Eigen::Matrix J_G_0_; 148 | // debug 149 | bool bPrint_; 150 | };// struct RegProblemLM 151 | }// namespace core 152 | }// namespace esvo2_core 153 | 154 | #endif //ESVO2_CORE_CORE_REGPROBLEMLM_H -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/core/RegProblemSolverLM.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO2_CORE_CORE_REGPROBLEMSOLVERLM_H 2 | #define ESVO2_CORE_CORE_REGPROBLEMSOLVERLM_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | namespace esvo2_core 17 | { 18 | namespace core 19 | { 20 | enum RegProblemType 21 | { 22 | REG_NUMERICAL, 23 | REG_ANALYTICAL 24 | }; 25 | 26 | struct LM_statics 27 | { 28 | size_t nPoints_; 29 | size_t nfev_; 30 | size_t nIter_; 31 | }; 32 | 33 | class RegProblemSolverLM 34 | { 35 | public: 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | RegProblemSolverLM( 38 | CameraSystem::Ptr& camSysPtr, 39 | std::shared_ptr& rpConfigPtr, 40 | RegProblemType rpType = REG_NUMERICAL, 41 | size_t numThread = 1); 42 | virtual ~RegProblemSolverLM(); 43 | 44 | bool resetRegProblem(RefFrame* ref, CurFrame* cur); 45 | bool resetRegProblem(shared_ptr &rpConfigPtr, CameraSystem::Ptr& camSysPtr, RefFrame* ref, CurFrame* cur); 46 | bool solve_numerical();// relatively slower 47 | bool solve_analytical();// faster 48 | 49 | // For test and visualization 50 | void setRegPublisher(image_transport::Publisher* reprojMap_pub); 51 | LM_statics lmStatics_;// record LevenburgMarquardt log. 52 | 53 | // variables 54 | private: 55 | CameraSystem::Ptr& camSysPtr_; 56 | std::shared_ptr rpConfigPtr_; 57 | size_t NUM_THREAD_; 58 | RegProblemType rpType_; 59 | 60 | std::shared_ptr regProblemPtr_; 61 | std::shared_ptr > numDiff_regProblemPtr_; 62 | 63 | // For test 64 | double z_min_, z_max_; 65 | image_transport::Publisher *reprojMap_pub_; 66 | Visualization visualizor_; 67 | bool bPrint_, bVisualize_; 68 | }; 69 | }//namespace core 70 | }//namespace esvo2_core 71 | #endif //ESVO2_CORE_CORE_REGPROBLEMSOLVER2_H 72 | -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/esvo2_Tracking.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO2_CORE_TRACKING_H 2 | #define ESVO2_CORE_TRACKING_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | namespace esvo2_core 41 | { 42 | using namespace core; 43 | using namespace factor; 44 | enum TrackingStatus 45 | { 46 | IDLE, 47 | WORKING 48 | }; 49 | 50 | class esvo2_Tracking 51 | { 52 | public: 53 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 54 | esvo2_Tracking(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private); 55 | virtual ~esvo2_Tracking(); 56 | 57 | // functions regarding tracking 58 | void TrackingLoop(); 59 | bool refDataTransferring(); 60 | bool curDataTransferring(); // These two data transferring functions are decoupled because the data are not updated at the same frequency. 61 | bool curImuTransferring(); 62 | // topic callback functions 63 | void refMapCallback(const sensor_msgs::PointCloud2::ConstPtr &msg); 64 | void refImuCallback(const sensor_msgs::ImuPtr &msg); 65 | void VBaBgCallback(const events_repacking_tool::V_ba_bg &msg); 66 | void groundTruthCallback(const geometry_msgs::PoseStampedConstPtr &msg); 67 | void timeSurface_NegaTS_Callback( 68 | const sensor_msgs::ImageConstPtr &time_surface_left, 69 | const sensor_msgs::ImageConstPtr &time_surface_negative, 70 | const sensor_msgs::ImageConstPtr &time_surface_dx, 71 | const sensor_msgs::ImageConstPtr &time_surface_dy); 72 | void eventsCallback(const dvs_msgs::EventArray::ConstPtr &msg); 73 | 74 | // results 75 | void publishPose(const ros::Time &t, Transformation &tr); 76 | void publishPath(const ros::Time &t, Transformation &tr); 77 | void saveTrajectory(std::string &resultDir); 78 | 79 | // utils 80 | void reset(); 81 | void clearEventQueue(); 82 | void stampedPoseCallback(const geometry_msgs::PoseStampedConstPtr &msg); 83 | bool getPoseAt( 84 | const ros::Time &t, 85 | esvo2_core::Transformation &Tr, // T_world_something 86 | const std::string &source_frame); 87 | void renameOldTraj(); 88 | Eigen::Matrix3d fixRotationMatrix(const Eigen::Matrix3d &R); 89 | 90 | private: 91 | ros::NodeHandle nh_, pnh_; 92 | image_transport::ImageTransport it_; 93 | 94 | // subscribers and publishers 95 | // ros::Subscriber events_left_sub_; 96 | ros::Subscriber map_sub_, map_sub_for_tracking_visualization_; 97 | ros::Subscriber V_ba_bg_sub_; 98 | ros::Subscriber imu_sub_; 99 | ros::Subscriber gt_sub_; 100 | 101 | message_filters::Subscriber TS_left_sub_, TS_right_sub_; 102 | message_filters::Subscriber TS_negative_sub_, TS_dx_sub_, TS_dy_sub_; 103 | ros::Subscriber stampedPose_sub_; 104 | image_transport::Publisher reprojMap_pub_left_; 105 | 106 | // publishers 107 | ros::Publisher pose_pub_, path_pub_; 108 | 109 | // results 110 | nav_msgs::Path path_; 111 | std::list, Eigen::aligned_allocator>> lPose_; 112 | std::list lTimestamp_; 113 | 114 | // Time Surface sync policy 115 | typedef message_filters::sync_policies::ApproximateTime ApproximateSyncPolicy; 116 | message_filters::Synchronizer TS_sync_; 117 | typedef message_filters::sync_policies::ApproximateTime ApproximateSyncPolicy_negaTS; 118 | message_filters::Synchronizer TS_negaTS_sync_; 119 | 120 | // offline data 121 | std::string dvs_frame_id_; 122 | std::string world_frame_id_; 123 | std::string calibInfoDir_; 124 | CameraSystem::Ptr camSysPtr_; 125 | 126 | // inter-thread management 127 | std::mutex data_mutex_; 128 | std::mutex imu_mutex_; 129 | 130 | // imu data 131 | IntegrationBase imu_data_; 132 | Eigen::Vector3d g_optimal{0, 9.81, 0}; 133 | Eigen::Matrix3d R_b_c_; 134 | Eigen::Vector3d ba_ = Eigen::Vector3d(0.0, 0.0, 0.0); 135 | Eigen::Vector3d bg_ = Eigen::Vector3d(0.0, 0.0, 0.0); 136 | Eigen::Quaterniond Imu_q = Eigen::Quaterniond::Identity(); 137 | Eigen::Vector3d Imu_t = Eigen::Vector3d::Zero(); 138 | 139 | // online data 140 | EventQueue events_left_; 141 | TimeSurfaceHistory TS_history_; 142 | size_t TS_id_; 143 | std::shared_ptr tf_; 144 | std::map::Ptr> refPCMap_; 145 | RefFrame ref_; 146 | CurFrame cur_; 147 | 148 | /**** offline parameters ***/ 149 | size_t tracking_rate_hz_; 150 | size_t TS_HISTORY_LENGTH_; 151 | size_t REF_HISTORY_LENGTH_; 152 | bool bSaveTrajectory_; 153 | bool bVisualizeTrajectory_; 154 | bool bUseImu_; 155 | std::string resultPath_; 156 | 157 | Eigen::Matrix T_world_ref_; 158 | Eigen::Matrix T_world_cur_; 159 | 160 | Eigen::Vector3d t_world_cur_; 161 | Eigen::Vector3d last_t_world_cur_; 162 | Eigen::Vector3d last_t_; 163 | 164 | /*** system objects ***/ 165 | RegProblemType rpType_; 166 | TrackingStatus ets_; 167 | std::string ESVO2_System_Status_; 168 | RegProblemConfig::Ptr rpConfigPtr_; 169 | RegProblemSolverLM rpSolver_; 170 | bool initVsFlag; 171 | 172 | /*** for test ***/ 173 | 174 | std::vector qprevious_ts_; 175 | }; 176 | } 177 | 178 | #endif // ESVO2_CORE_TRACKING_H 179 | -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/factor/OptimizationFunctor.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO2_CORE_OPTIMIZATION_OPTIMIZATIONFUNCTOR_H 2 | #define ESVO2_CORE_OPTIMIZATION_OPTIMIZATIONFUNCTOR_H 3 | 4 | #include 5 | #include 6 | 7 | namespace esvo2_core 8 | { 9 | namespace factor 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 //ESVO2_CORE_OPTIMIZATION_OPTIMIZATIONFUNCTOR_H 68 | -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/factor/pose_local_parameterization.h: -------------------------------------------------------------------------------- 1 | // #pragma once 2 | #ifndef ESVO2_CORE_FACTOR_POSELOCAL_H 3 | #define ESVO2_CORE_FACTOR_POSELOCAL_H 4 | 5 | #include 6 | #include 7 | #include 8 | namespace esvo2_core 9 | { 10 | namespace factor 11 | { 12 | class PoseLocalParameterization : public ceres::LocalParameterization 13 | { 14 | public: 15 | ~PoseLocalParameterization() {}; 16 | bool Plus(const double *x, const double *delta, double *x_plus_delta) const 17 | { 18 | Eigen::Map _p(x); 19 | Eigen::Map _q(x + 3); 20 | 21 | Eigen::Map dp(delta); 22 | 23 | Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map(delta + 3)); 24 | 25 | Eigen::Map p(x_plus_delta); 26 | Eigen::Map q(x_plus_delta + 3); 27 | 28 | p = _p + dp; 29 | q = (_q * dq).normalized(); 30 | 31 | return true; 32 | } ; 33 | bool ComputeJacobian(const double *x, double *jacobian) const 34 | { 35 | Eigen::Map> j(jacobian); 36 | j.topRows<6>().setIdentity(); 37 | j.bottomRows<1>().setZero(); 38 | 39 | return true; 40 | }; 41 | int GlobalSize() const { return 7; }; 42 | int LocalSize() const { return 6; }; 43 | }; 44 | } 45 | } 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/factor/utility.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | namespace esvo2_core 8 | { 9 | namespace factor 10 | { 11 | class Utility 12 | { 13 | public: 14 | template 15 | static Eigen::Quaternion deltaQ(const Eigen::MatrixBase &theta) 16 | { 17 | typedef typename Derived::Scalar Scalar_t; 18 | 19 | Eigen::Quaternion dq; 20 | Eigen::Matrix half_theta = theta; 21 | half_theta /= static_cast(2.0); 22 | dq.w() = static_cast(1.0); 23 | dq.x() = half_theta.x(); 24 | dq.y() = half_theta.y(); 25 | dq.z() = half_theta.z(); 26 | return dq; 27 | } 28 | 29 | template 30 | static Eigen::Matrix skewSymmetric(const Eigen::MatrixBase &q) 31 | { 32 | Eigen::Matrix ans; 33 | ans << typename Derived::Scalar(0), -q(2), q(1), 34 | q(2), typename Derived::Scalar(0), -q(0), 35 | -q(1), q(0), typename Derived::Scalar(0); 36 | return ans; 37 | } 38 | 39 | template 40 | static Eigen::Quaternion positify(const Eigen::QuaternionBase &q) 41 | { 42 | // printf("a: %f %f %f %f", q.w(), q.x(), q.y(), q.z()); 43 | // Eigen::Quaternion p(-q.w(), -q.x(), -q.y(), -q.z()); 44 | // printf("b: %f %f %f %f", p.w(), p.x(), p.y(), p.z()); 45 | // return q.template w() >= (typename Derived::Scalar)(0.0) ? q : Eigen::Quaternion(-q.w(), -q.x(), -q.y(), -q.z()); 46 | return q; 47 | } 48 | 49 | template 50 | static Eigen::Matrix Qleft(const Eigen::QuaternionBase &q) 51 | { 52 | Eigen::Quaternion qq = positify(q); 53 | Eigen::Matrix ans; 54 | ans(0, 0) = qq.w(), ans.template block<1, 3>(0, 1) = -qq.vec().transpose(); 55 | ans.template block<3, 1>(1, 0) = qq.vec(), ans.template block<3, 3>(1, 1) = qq.w() * Eigen::Matrix::Identity() + skewSymmetric(qq.vec()); 56 | return ans; 57 | } 58 | 59 | template 60 | static Eigen::Matrix Qright(const Eigen::QuaternionBase &p) 61 | { 62 | Eigen::Quaternion pp = positify(p); 63 | Eigen::Matrix ans; 64 | ans(0, 0) = pp.w(), ans.template block<1, 3>(0, 1) = -pp.vec().transpose(); 65 | ans.template block<3, 1>(1, 0) = pp.vec(), ans.template block<3, 3>(1, 1) = pp.w() * Eigen::Matrix::Identity() - skewSymmetric(pp.vec()); 66 | return ans; 67 | } 68 | 69 | static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R) 70 | { 71 | Eigen::Vector3d n = R.col(0); 72 | Eigen::Vector3d o = R.col(1); 73 | Eigen::Vector3d a = R.col(2); 74 | 75 | Eigen::Vector3d ypr(3); 76 | double y = atan2(n(1), n(0)); 77 | double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); 78 | double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); 79 | ypr(0) = y; 80 | ypr(1) = p; 81 | ypr(2) = r; 82 | 83 | return ypr / M_PI * 180.0; 84 | } 85 | 86 | template 87 | static Eigen::Matrix ypr2R(const Eigen::MatrixBase &ypr) 88 | { 89 | typedef typename Derived::Scalar Scalar_t; 90 | 91 | Scalar_t y = ypr(0) / 180.0 * M_PI; 92 | Scalar_t p = ypr(1) / 180.0 * M_PI; 93 | Scalar_t r = ypr(2) / 180.0 * M_PI; 94 | 95 | Eigen::Matrix Rz; 96 | Rz << cos(y), -sin(y), 0, 97 | sin(y), cos(y), 0, 98 | 0, 0, 1; 99 | 100 | Eigen::Matrix Ry; 101 | Ry << cos(p), 0., sin(p), 102 | 0., 1., 0., 103 | -sin(p), 0., cos(p); 104 | 105 | Eigen::Matrix Rx; 106 | Rx << 1., 0., 0., 107 | 0., cos(r), -sin(r), 108 | 0., sin(r), cos(r); 109 | 110 | return Rz * Ry * Rx; 111 | } 112 | 113 | static Eigen::Matrix3d g2R(const Eigen::Vector3d &g) 114 | { 115 | Eigen::Matrix3d R0; 116 | Eigen::Vector3d ng1 = g.normalized(); 117 | Eigen::Vector3d ng2{0, 0, 1.0}; 118 | R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix(); 119 | double yaw = Utility::R2ypr(R0).x(); 120 | R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0; 121 | // R0 = Utility::ypr2R(Eigen::Vector3d{-90, 0, 0}) * R0; 122 | return R0; 123 | } 124 | 125 | template 126 | struct uint_ 127 | { 128 | }; 129 | 130 | template 131 | void unroller(const Lambda &f, const IterT &iter, uint_) 132 | { 133 | unroller(f, iter, uint_()); 134 | f(iter + N); 135 | } 136 | 137 | template 138 | void unroller(const Lambda &f, const IterT &iter, uint_<0>) 139 | { 140 | f(iter); 141 | } 142 | 143 | template 144 | static T normalizeAngle(const T &angle_degrees) 145 | { 146 | T two_pi(2.0 * 180); 147 | if (angle_degrees > 0) 148 | return angle_degrees - 149 | two_pi * std::floor((angle_degrees + T(180)) / two_pi); 150 | else 151 | return angle_degrees + 152 | two_pi * std::floor((-angle_degrees + T(180)) / two_pi); 153 | }; 154 | 155 | 156 | }; 157 | } // namespace factor 158 | } // namespace esvo2_core -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/tools/TicToc.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO2_CORE_TOOLS_TICTOC_H 2 | #define ESVO2_CORE_TOOLS_TICTOC_H 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace esvo2_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 //ESVO2_CORE_TOOLS_TICTOC_H 40 | -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/tools/Visualization.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO2_CORE_TOOLS_VISUALIZATION_H 2 | #define ESVO2_CORE_TOOLS_VISUALIZATION_H 3 | 4 | #include 5 | 6 | namespace esvo2_core 7 | { 8 | using namespace container; 9 | namespace tools 10 | { 11 | enum VisMapType 12 | { 13 | InvDepthMap, 14 | StdVarMap, 15 | CostMap, 16 | AgeMap, 17 | InvDepthMap_rel, 18 | InvDepthMap_rel2 19 | }; 20 | class Visualization 21 | { 22 | public: 23 | Visualization(); 24 | 25 | virtual ~Visualization(); 26 | 27 | void plot_map( 28 | DepthMap::Ptr &depthMapPtr, 29 | VisMapType vmType, 30 | cv::Mat &img, 31 | double max_range, 32 | double min_range, 33 | double visualization_threshold1, 34 | double visualization_threshold2 = 0.0); 35 | 36 | void plot_eventMap( 37 | std::vector& vEventPtr, 38 | cv::Mat & eventMap, 39 | size_t row, size_t col); 40 | 41 | void plot_events( 42 | std::vector, 43 | Eigen::aligned_allocator > > & vEvents, 44 | cv::Mat & event_img, 45 | size_t row, size_t col); 46 | 47 | void DrawPoint( 48 | double val, 49 | double max_range, 50 | double min_range, 51 | const Eigen::Vector2d &location, 52 | cv::Mat &img, 53 | int radius = 0); 54 | 55 | void DrawPoint2( 56 | double val, 57 | double max_range, 58 | double min_range, 59 | const Eigen::Vector2d &location, 60 | cv::Mat &img ); 61 | 62 | public: 63 | //the rgb values for a jet colormap with 256 values 64 | static const float r[]; 65 | static const float g[]; 66 | static const float b[]; 67 | }; 68 | } 69 | } 70 | 71 | #endif //ESVO2_CORE_TOOLS_VISUALIZATION_H 72 | -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/tools/cayley.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO2_CORE_TOOLS_CAYLEY_H 2 | #define ESVO2_CORE_TOOLS_CAYLEY_H 3 | 4 | #include 5 | namespace esvo2_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 esvo2_core 13 | 14 | 15 | #endif //ESVO2_CORE_TOOLS_CAYLEY_H 16 | -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/tools/params_helper.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO2_CORE_PARAMS_HELPER_H 2 | #define ESVO2_CORE_PARAMS_HELPER_H 3 | 4 | #pragma once 5 | #include 6 | #include 7 | 8 | namespace esvo2_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 | } // esvo2_core 61 | 62 | #endif //ESVO2_CORE_PARAMS_HELPER_H 63 | -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/tools/sobel.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO2_CORE_TOOLS_SOBEL_H 2 | #define ESVO2_CORE_TOOLS_SOBEL_H 3 | 4 | #include 5 | #include 6 | 7 | using namespace std; 8 | namespace esvo2_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 //ESVO2_CORE_TOOLS_SOBEL_H -------------------------------------------------------------------------------- /esvo2_core/include/esvo2_core/tools/utils.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO2_CORE_TOOLS_UTILS_H 2 | #define ESVO2_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 esvo2_core 31 | { 32 | namespace tools 33 | { 34 | // TUNE this according to your platform's computational capability. 35 | #define NUM_THREAD_TRACKING 1 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 | double 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 | sigma = 1.0 / sigma; 92 | patch_dst = (patch_src.array() - mean) * sigma; 93 | } 94 | 95 | // recursively create a directory 96 | static inline void _mkdir(const char *dir) 97 | { 98 | char tmp[256]; 99 | char *p = NULL; 100 | size_t len; 101 | 102 | snprintf(tmp, sizeof(tmp),"%s",dir); 103 | len = strlen(tmp); 104 | if(tmp[len - 1] == '/') 105 | tmp[len - 1] = 0; 106 | for(p = tmp + 1; *p; p++) 107 | if(*p == '/') { 108 | *p = 0; 109 | mkdir(tmp, S_IRWXU); 110 | *p = '/'; 111 | } 112 | mkdir(tmp, S_IRWXU); 113 | } 114 | 115 | }// tools 116 | }// esvo2_core 117 | #endif //ESVO2_CORE_TOOLS_UTILS_H 118 | -------------------------------------------------------------------------------- /esvo2_core/include/sophus/sophus.hpp: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2013 Hauke Strasdat 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #ifndef SOPHUS_HPP 24 | #define SOPHUS_HPP 25 | 26 | #include 27 | 28 | // fix log1p not being found on Android in Eigen 29 | #if defined( ANDROID ) 30 | #include 31 | namespace std 32 | { 33 | using ::log1p; 34 | } 35 | #endif 36 | 37 | #include 38 | #include 39 | 40 | namespace Sophus { 41 | using namespace Eigen; 42 | 43 | template 44 | struct SophusConstants { 45 | EIGEN_ALWAYS_INLINE static 46 | const Scalar epsilon() { 47 | return static_cast ( 1e-10 ); 48 | } 49 | 50 | EIGEN_ALWAYS_INLINE static 51 | const Scalar pi() { 52 | return static_cast ( M_PI ); 53 | } 54 | }; 55 | 56 | template<> 57 | struct SophusConstants { 58 | EIGEN_ALWAYS_INLINE static 59 | float epsilon() { 60 | return static_cast ( 1e-5 ); 61 | } 62 | 63 | EIGEN_ALWAYS_INLINE static 64 | float pi() { 65 | return static_cast ( M_PI ); 66 | } 67 | }; 68 | 69 | class SophusException : public std::runtime_error { 70 | public: 71 | SophusException(const std::string &str) 72 | : runtime_error("Sophus exception: " + str) { 73 | } 74 | }; 75 | 76 | } 77 | 78 | #endif 79 | -------------------------------------------------------------------------------- /esvo2_core/launch/system/system_dsec.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | $(arg calibInfoDirStr) 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | $(arg calibInfoDirStr) 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | "dvs" 48 | "map" 49 | $(arg calibInfoDirStr) 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | "dvs" 67 | "map" 68 | $(arg calibInfoDirStr) 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /esvo2_core/launch/system/system_dvx_mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | $(arg calibInfoDirStr) 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | $(arg calibInfoDirStr) 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | "dvs" 51 | "map" 52 | $(arg calibInfoDirStr) 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | "dvs" 70 | "map" 71 | $(arg calibInfoDirStr) 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /esvo2_core/launch/system/system_dvx_tracking.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | $(arg calibInfoDirStr) 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | $(arg calibInfoDirStr) 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | "dvs" 53 | "map" 54 | $(arg calibInfoDirStr) 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | "dvs" 72 | "map" 73 | $(arg calibInfoDirStr) 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /esvo2_core/launch/system/system_rpg.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | $(arg calibInfoDirStr) 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | $(arg calibInfoDirStr) 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | "dvs" 48 | "map" 49 | $(arg calibInfoDirStr) 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | "dvs" 67 | "map" 68 | $(arg calibInfoDirStr) 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /esvo2_core/launch/system/system_tum_DoF.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | $(arg calibInfoDirStr) 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | $(arg calibInfoDirStr) 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | "dvs" 46 | "map" 47 | $(arg calibInfoDirStr) 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | "dvs" 64 | "map" 65 | $(arg calibInfoDirStr) 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /esvo2_core/launch/system/system_tum_desk.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | $(arg calibInfoDirStr) 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | $(arg calibInfoDirStr) 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | "dvs" 46 | "map" 47 | $(arg calibInfoDirStr) 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | "dvs" 64 | "map" 65 | $(arg calibInfoDirStr) 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /esvo2_core/launch/system/system_upenn.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | $(arg calibInfoDirStr) 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | $(arg calibInfoDirStr) 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | "dvs" 47 | "map" 48 | $(arg calibInfoDirStr) 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | "dvs" 65 | "map" 66 | $(arg calibInfoDirStr) 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /esvo2_core/launch/system/system_vector.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | $(arg calibInfoDirStr) 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | $(arg calibInfoDirStr) 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | "dvs" 47 | "map" 48 | $(arg calibInfoDirStr) 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | "dvs" 66 | "map" 67 | $(arg calibInfoDirStr) 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | -------------------------------------------------------------------------------- /esvo2_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | esvo2_core 4 | 0.0.0 5 | The esvo2_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 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /esvo2_core/src/container/DepthPoint.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace esvo2_core 4 | { 5 | namespace container 6 | { 7 | DepthPoint::DepthPoint() 8 | { 9 | row_ = 0; 10 | col_ = 0; 11 | x_ = Eigen::Vector2d(col_ + 0.5, row_ + 0.5); 12 | 13 | invDepth_ = -1.0; 14 | variance_ = 0.0; 15 | residual_ = 0.0; 16 | 17 | age_ = 0; 18 | // outlierProbability_ = 0.0; 19 | } 20 | 21 | DepthPoint::DepthPoint( 22 | size_t row, size_t col) 23 | { 24 | row_ = row; 25 | col_ = col; 26 | x_ = Eigen::Vector2d(col_ + 0.5, row_ + 0.5); 27 | 28 | invDepth_ = -1.0; 29 | variance_ = 0.0; 30 | residual_ = 0.0; 31 | 32 | age_ = 0; 33 | // outlierProbability_ = 0.0; 34 | } 35 | 36 | DepthPoint::~DepthPoint() 37 | { 38 | 39 | } 40 | 41 | size_t 42 | DepthPoint::row() const 43 | { 44 | return row_; 45 | } 46 | 47 | size_t 48 | DepthPoint::col() const 49 | { 50 | return col_; 51 | } 52 | 53 | const Eigen::Vector2d & 54 | DepthPoint::x() const 55 | { 56 | return x_; 57 | } 58 | 59 | void 60 | DepthPoint::update_x(const Eigen::Vector2d &x) 61 | { 62 | x_ = x; 63 | } 64 | 65 | double & 66 | DepthPoint::invDepth() 67 | { 68 | return invDepth_; 69 | } 70 | 71 | const double & 72 | DepthPoint::invDepth() const 73 | { 74 | return invDepth_; 75 | } 76 | 77 | double & 78 | DepthPoint::scaleSquared() 79 | { 80 | return scaleSquared_; 81 | } 82 | 83 | const double & 84 | DepthPoint::scaleSquared() const 85 | { 86 | return scaleSquared_; 87 | } 88 | 89 | double & 90 | DepthPoint::nu() 91 | { 92 | return nu_; 93 | } 94 | 95 | const double & 96 | DepthPoint::nu() const 97 | { 98 | return nu_; 99 | } 100 | 101 | double & 102 | DepthPoint::variance() 103 | { 104 | return variance_; 105 | } 106 | 107 | const double & 108 | DepthPoint::variance() const 109 | { 110 | return variance_; 111 | } 112 | 113 | double & 114 | DepthPoint::residual() 115 | { 116 | return residual_; 117 | } 118 | 119 | const double & 120 | DepthPoint::residual() const 121 | { 122 | return residual_; 123 | } 124 | 125 | size_t & 126 | DepthPoint::age() 127 | { 128 | return age_; 129 | } 130 | 131 | const size_t & 132 | DepthPoint::age() const 133 | { 134 | return age_; 135 | } 136 | 137 | void 138 | DepthPoint::boundVariance() 139 | { 140 | double eps = 1e-6; 141 | if (variance_ < eps) 142 | variance_ = eps; 143 | } 144 | 145 | void 146 | DepthPoint::update( 147 | double invDepth, double variance) 148 | { 149 | if (invDepth_ > -1e-6) 150 | { 151 | //do an actual update 152 | double temp = invDepth_; 153 | invDepth_ = (variance_ * invDepth + variance * temp) / (variance_ + variance); 154 | temp = variance_; 155 | variance_ = (temp * variance) / (temp + variance); 156 | } 157 | else 158 | { 159 | //this is a new point, so simply take the first measurement 160 | invDepth_ = invDepth; 161 | variance_ = variance; 162 | } 163 | boundVariance(); 164 | } 165 | 166 | void 167 | DepthPoint::update_studentT(double invDepth, double scale2, double variance, double nu) 168 | { 169 | if(invDepth_ > -1e-6) 170 | { 171 | double nu_update = std::min(nu, nu_); 172 | double invDepth_update = (scale2*invDepth_ + scaleSquared_ * invDepth) / (scaleSquared_ + scale2); 173 | double scale2_update = (nu_update + pow(invDepth_ - invDepth,2) / (scaleSquared_ + scale2)) / (nu_update + 1) * (scaleSquared_ * scale2) / (scaleSquared_ + scale2); 174 | 175 | invDepth_ = invDepth_update; 176 | scaleSquared_ = scale2_update; 177 | nu_ = nu_update + 1; 178 | variance_ = nu_ / (nu_ - 2) * scaleSquared_; 179 | age_++; 180 | } 181 | else 182 | { 183 | invDepth_ = invDepth; 184 | scaleSquared_ = scale2; 185 | variance_ = variance; 186 | nu_ = nu; 187 | } 188 | } 189 | 190 | void 191 | DepthPoint::update_p_cam(const Eigen::Vector3d &p) 192 | { 193 | p_cam_ = p; 194 | } 195 | 196 | const Eigen::Vector3d & 197 | DepthPoint::p_cam() const 198 | { 199 | return p_cam_; 200 | } 201 | 202 | void 203 | DepthPoint::updatePose(Eigen::Matrix &T_world_cam) 204 | { 205 | T_world_cam_ = T_world_cam; 206 | } 207 | 208 | const Eigen::Matrix & 209 | DepthPoint::T_world_cam() const 210 | { 211 | return T_world_cam_; 212 | } 213 | 214 | bool 215 | DepthPoint::valid() const 216 | { 217 | return invDepth_ > -1e-6; 218 | } 219 | 220 | bool 221 | DepthPoint::valid(double var_threshold, 222 | double age_threshold, 223 | double invDepth_max, 224 | double invDepth_min) const 225 | { 226 | return invDepth_ > -1e-6 && 227 | age_ >= age_threshold && 228 | variance_ <= var_threshold && 229 | invDepth_ <= invDepth_max && 230 | invDepth_ >= invDepth_min; 231 | } 232 | 233 | void 234 | DepthPoint::copy(const DepthPoint ©) 235 | { 236 | invDepth_ = copy.invDepth_; 237 | variance_ = copy.variance_; 238 | scaleSquared_ = copy.scaleSquared_; 239 | nu_ = copy.nu_; 240 | x_ = copy.x_; 241 | p_cam_ = copy.p_cam_; 242 | T_world_cam_ = copy.T_world_cam_; 243 | residual_ = copy.residual_; 244 | age_ = copy.age_; 245 | } 246 | 247 | } 248 | 249 | } -------------------------------------------------------------------------------- /esvo2_core/src/container/EventPoint.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace esvo2_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 | } -------------------------------------------------------------------------------- /esvo2_core/src/container/ResidualItem.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | esvo2_core::container::ResidualItem::ResidualItem() = default; 4 | 5 | esvo2_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 esvo2_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 | } -------------------------------------------------------------------------------- /esvo2_core/src/core/DepthRegularization.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace esvo2_core 4 | { 5 | namespace core 6 | { 7 | DepthRegularization::DepthRegularization( 8 | std::shared_ptr & dpConfigPtr) 9 | { 10 | dpConfigPtr_ = dpConfigPtr; 11 | //parameters 12 | _regularizationRadius = dpConfigPtr_->RegularizationRadius_;//20;//5;//10 13 | _regularizationMinNeighbours = dpConfigPtr_->RegularizationMinNeighbours_;//32;//8;//16 14 | _regularizationMinCloseNeighbours = dpConfigPtr_->RegularizationMinCloseNeighbours_;//32;//8;//16 15 | } 16 | 17 | DepthRegularization::~DepthRegularization() {} 18 | 19 | void DepthRegularization::apply( DepthMap::Ptr& depthMapPtr ) 20 | { 21 | DepthMap &dm = *depthMapPtr.get(); 22 | 23 | DepthMap dmTmp(dm.rows(), dm.cols()); 24 | 25 | DepthMap::iterator it = dm.begin(); 26 | while (it != dm.end()) 27 | { 28 | dmTmp.set(it->row(), it->col(), *it); 29 | DepthPoint &newDp = dmTmp.get(it->row(), it->col()); 30 | 31 | if (it->valid()) 32 | { 33 | //get the valid neighbourhood pixels 34 | std::vector neighbours; 35 | dm.getNeighbourhood(it->row(), it->col(), _regularizationRadius, neighbours); 36 | 37 | bool isSet = false; 38 | if (neighbours.size() > _regularizationMinNeighbours) 39 | { 40 | //find close neighbours (will include this point) 41 | std::vector closeNeighbours; 42 | for (size_t i = 0; i < neighbours.size(); i++) 43 | { 44 | if (neighbours[i]->valid()) 45 | { 46 | double diff = fabs(it->invDepth() - neighbours[i]->invDepth()); 47 | if (diff < 2.0 * sqrt(it->variance()) || 48 | diff < 2.0 * sqrt(neighbours[i]->variance())) 49 | closeNeighbours.push_back(neighbours[i]); 50 | } 51 | } 52 | // regularizationMinCloseNeighbours is larger than the fusion's applied region (namely 4 pixels in my implementation) 53 | if (closeNeighbours.size() > _regularizationMinCloseNeighbours) 54 | { 55 | double statisticalMean = 0.0; 56 | if(strcmp(dpConfigPtr_->LSnorm_.c_str(), "l2") == 0) 57 | { 58 | //compute statistical average 59 | double totalInvVariances = 0.0; 60 | for (size_t i = 0; i < closeNeighbours.size(); i++) 61 | totalInvVariances += 1.0 / closeNeighbours[i]->variance(); 62 | for (size_t i = 0; i < closeNeighbours.size(); i++) 63 | statisticalMean += closeNeighbours[i]->invDepth() * 64 | (1.0 / closeNeighbours[i]->variance()) / totalInvVariances; 65 | } 66 | else if(strcmp(dpConfigPtr_->LSnorm_.c_str(), "Tdist") == 0) 67 | { 68 | double nu_post = closeNeighbours[0]->nu(); 69 | double invDepth_post = closeNeighbours[0]->invDepth(); 70 | double scale2_post = closeNeighbours[0]->scaleSquared(); 71 | 72 | for (size_t i = 1; i < closeNeighbours.size(); i++) 73 | { 74 | double nu_prior = nu_post; 75 | double invDepth_prior = invDepth_post; 76 | double scale2_prior = scale2_post; 77 | 78 | double nu_obs = closeNeighbours[i]->nu(); 79 | double invDepth_obs = closeNeighbours[i]->invDepth(); 80 | double scale2_obs = closeNeighbours[i]->scaleSquared(); 81 | 82 | nu_post = std::min(nu_prior, nu_obs); 83 | invDepth_post = (scale2_obs * invDepth_prior + scale2_prior * invDepth_obs) / (scale2_obs + scale2_prior); 84 | scale2_post = (nu_post + pow(invDepth_prior - invDepth_obs,2) / (scale2_prior + scale2_obs)) / 85 | (nu_post + 1) * (scale2_prior * scale2_obs) / (scale2_prior + scale2_obs); 86 | } 87 | statisticalMean = invDepth_post; 88 | } 89 | else 90 | { 91 | LOG(INFO) << "(Regularization) Wrong dpConfiguration is provided."; 92 | exit(-1); 93 | } 94 | 95 | //set the statistical average (everything else is simply copied) 96 | newDp.invDepth() = statisticalMean; 97 | isSet = true; 98 | } 99 | } 100 | 101 | if (!isSet) 102 | newDp.invDepth() = -1.0; 103 | } 104 | 105 | it++; 106 | } 107 | 108 | //transfer the result 109 | dm = dmTmp; 110 | } 111 | 112 | }// core 113 | }// esvo2_core -------------------------------------------------------------------------------- /esvo2_core/src/esvo2_MappingNode.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char** argv) 4 | { 5 | ros::init(argc, argv, "esvo2_Mapping"); 6 | ros::NodeHandle nh; 7 | ros::NodeHandle nh_private("~"); 8 | 9 | esvo2_core::esvo2_Mapping mapper(nh, nh_private); 10 | ros::spin(); 11 | return 0; 12 | } 13 | 14 | -------------------------------------------------------------------------------- /esvo2_core/src/esvo2_TrackingNode.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char** argv) 4 | { 5 | ros::init(argc, argv, "esvo2_Tracking"); 6 | ros::NodeHandle nh; 7 | ros::NodeHandle nh_private("~"); 8 | 9 | esvo2_core::esvo2_Tracking tracker(nh, nh_private); 10 | ros::spin(); 11 | return 0; 12 | } 13 | 14 | -------------------------------------------------------------------------------- /esvo2_core/src/factor/pose_local_parameterization.cpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include "esvo2_core/factor/pose_local_parameterization.h" 4 | 5 | // namespace esvo2_core 6 | // { 7 | // namespace factor 8 | // { 9 | bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const 10 | { 11 | Eigen::Map _p(x); 12 | Eigen::Map _q(x + 3); 13 | 14 | Eigen::Map dp(delta); 15 | 16 | Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map(delta + 3)); 17 | 18 | Eigen::Map p(x_plus_delta); 19 | Eigen::Map q(x_plus_delta + 3); 20 | 21 | p = _p + dp; 22 | q = (_q * dq).normalized(); 23 | 24 | return true; 25 | } 26 | bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const 27 | { 28 | Eigen::Map> j(jacobian); 29 | j.topRows<6>().setIdentity(); 30 | j.bottomRows<1>().setZero(); 31 | 32 | return true; 33 | } 34 | // } 35 | // } -------------------------------------------------------------------------------- /esvo2_core/src/tools/cayley.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | Eigen::Matrix3d 4 | esvo2_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 | esvo2_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 | -------------------------------------------------------------------------------- /esvo2_core/src/tools/sobel.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace esvo2_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 | }//esvo2_core -------------------------------------------------------------------------------- /events_repacking_tool/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(events_repacking_tool) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | find_package(Eigen3 REQUIRED) 7 | set(CMAKE_BUILD_TYPE DEBUG) 8 | 9 | ## Find catkin macros and libraries 10 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 11 | ## is used, also find other catkin packages 12 | find_package(catkin_simple REQUIRED COMPONENTS 13 | rosbag 14 | roscpp 15 | rospy 16 | std_msgs 17 | message_generation 18 | ) 19 | 20 | add_message_files( 21 | FILES 22 | V_ba_bg.msg 23 | # Message2.msg 24 | ) 25 | catkin_simple() 26 | 27 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O0") 28 | 29 | # make the executable 30 | 31 | # install(TARGETS image_synchronizer 32 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 33 | 34 | cs_add_executable(EventMessageEditor src/EventMessageEditor.cpp) 35 | target_link_libraries(EventMessageEditor ${catkin_LIBARIES}) 36 | 37 | # generate_messages( 38 | # DEPENDENCIES 39 | # # geometry_msgs 40 | # ) 41 | -------------------------------------------------------------------------------- /events_repacking_tool/README.md: -------------------------------------------------------------------------------- 1 | # events_repacking_tool 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_tool repacking.launch` 24 | 25 | This command will return a bag file (e.g., output.bag.events) which only contains the re-packed stereo event and imu 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 | -------------------------------------------------------------------------------- /events_repacking_tool/launch/repacking.launch: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /events_repacking_tool/msg/V_ba_bg.msg: -------------------------------------------------------------------------------- 1 | float64[] ba 2 | float64[] bg 3 | float64[] Vs 4 | float64[] head 5 | float64[] g -------------------------------------------------------------------------------- /events_repacking_tool/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | events_repacking_tool 4 | 0.0.0 5 | The events_repacking_tool package 6 | 7 | 8 | zhouyi 9 | 10 | 11 | 12 | 13 | GPLv3 14 | message_generation 15 | message_runtime 16 | catkin 17 | rosbag 18 | roscpp 19 | rospy 20 | std_msgs 21 | dvs_msgs 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /events_repacking_tool/src/EventMessageEditor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "sensor_msgs/Imu.h" 8 | #include 9 | struct EventMessageEditor 10 | { 11 | EventMessageEditor( 12 | double frequency, 13 | std::string & messageTopic) 14 | :bFirstMessage_(true) 15 | { 16 | eArray_ = dvs_msgs::EventArray(); 17 | duration_threshold_ = 1 / frequency; 18 | message_topic_ = messageTopic; 19 | } 20 | 21 | void resetBuffer(ros::Time startTimeStamp) 22 | { 23 | start_time_ = startTimeStamp; 24 | end_time_ = ros::Time(start_time_.toSec() + duration_threshold_); 25 | eArray_.events.clear(); 26 | eArray_.header.stamp = end_time_; 27 | } 28 | 29 | void resetArraySize(size_t width, size_t height) 30 | { 31 | eArray_.width = width; 32 | eArray_.height = height; 33 | } 34 | 35 | void insertEvent( 36 | dvs_msgs::Event & e, 37 | rosbag::Bag* bag) 38 | { 39 | if(bFirstMessage_) 40 | { 41 | resetBuffer(e.ts); 42 | bFirstMessage_ = false; 43 | } 44 | 45 | if(e.ts.toSec() >= end_time_.toSec()) 46 | { 47 | bag->write(message_topic_.c_str(), eArray_.header.stamp, eArray_); 48 | resetBuffer(end_time_); 49 | } 50 | eArray_.events.push_back(e); 51 | } 52 | 53 | // variables 54 | dvs_msgs::EventArray eArray_; 55 | double duration_threshold_; 56 | ros::Time start_time_, end_time_; 57 | bool bFirstMessage_; 58 | std::string message_topic_; 59 | }; 60 | 61 | 62 | int main(int argc, char* argv[]) 63 | { 64 | ros::init(argc, argv, "event_message_editor"); 65 | auto start = std::chrono::system_clock::now(); 66 | std::string src_bag_path(argv[1]); 67 | std::string src_bag_path2(argv[2]); 68 | std::string dst_bag_path(argv[3]); 69 | rosbag::Bag bag_src, bag_src2, 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 | sensor_msgs::Imu imu_buff; 74 | 75 | if(!bag_src.isOpen()) 76 | { 77 | ROS_INFO("No rosbag is found in the give path."); 78 | exit(-1); 79 | } 80 | else 81 | { 82 | ROS_INFO("***********Input Bag File Name ***********"); 83 | ROS_INFO(argv[1]); 84 | ROS_INFO(argv[2]); 85 | ROS_INFO("******************************************"); 86 | } 87 | 88 | if(!bag_dst.isOpen()) 89 | { 90 | ROS_INFO("The dst bag is not opened."); 91 | exit(-1); 92 | } 93 | else 94 | { 95 | ROS_INFO("***********Output Bag File Name ***********"); 96 | ROS_INFO(argv[3]); 97 | ROS_INFO("***************************"); 98 | } 99 | ros::Time start_time; 100 | ros::Time end_time; 101 | 102 | const double frequency = 1000; 103 | int nums = 0; 104 | // process events 105 | std::vector topics; 106 | // the topic name in the input bag file 107 | topics.push_back(std::string("/davis/left/events")); 108 | topics.push_back(std::string("/davis/right/events")); 109 | std::vector topics_rename; 110 | // the topic name in the output bag file 111 | topics_rename.push_back(std::string("/davis/left/events")); 112 | topics_rename.push_back(std::string("/davis/right/events")); 113 | for(size_t i = 0;i < topics.size(); i++) 114 | { 115 | rosbag::View view(bag_src, rosbag::TopicQuery(topics[i])); 116 | EventMessageEditor eArrayEditor(frequency, topics_rename[i]); 117 | start_time = ros::Time(view.getBeginTime().toSec()); 118 | end_time = ros::Time(view.getEndTime().toSec()+0.01); 119 | 120 | // topic loop 121 | for(rosbag::MessageInstance const m: view) 122 | { 123 | dvs_msgs::EventArray::ConstPtr msg = m.instantiate(); 124 | eArrayEditor.resetArraySize(msg->width, msg->height); 125 | // message loop 126 | for(dvs_msgs::Event e : msg->events) 127 | { 128 | nums++; 129 | eArrayEditor.insertEvent(const_cast(e), &bag_dst); 130 | } 131 | } 132 | } 133 | 134 | bag_src.close(); 135 | auto end = std::chrono::system_clock::now(); 136 | std::chrono::duration elapsed_seconds = end - start; 137 | bag_src2.open(src_bag_path2.c_str(), rosbag::bagmode::Read); 138 | // the topic name in the input bag file 139 | rosbag::View view(bag_src2, rosbag::TopicQuery("/imu/data")); 140 | for(rosbag::MessageInstance const m: view) 141 | { 142 | sensor_msgs::Imu::ConstPtr msg = m.instantiate(); 143 | if(msg->header.stamp.toSec() < start_time.toSec() || msg->header.stamp.toSec() > end_time.toSec()) 144 | continue; 145 | // the topic name in the output bag file 146 | bag_dst.write("/imu/data", msg->header.stamp, *msg); 147 | } 148 | 149 | 150 | bag_dst.close(); 151 | return 0; 152 | } 153 | 154 | 155 | -------------------------------------------------------------------------------- /image_representation/.gitignore: -------------------------------------------------------------------------------- 1 | /.idea/ 2 | /cmake-build-debug/ 3 | -------------------------------------------------------------------------------- /image_representation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(image_representation) 3 | 4 | # To be consistent with the configuration in esvo2_core 5 | set(CMAKE_CXX_STANDARD 14) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | # set(CMAKE_BUILD_TYPE DEBUG) 8 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -O3") 9 | # set(CMAKE_CXX_FLAGS "-O0") 10 | 11 | # set(catkin_simple_DIR ~/ros/rpg_eklt/build/catkin_simple/catkin_generated/installspace) 12 | find_package(catkin_simple REQUIRED) 13 | 14 | # set(IPP_DIR "/opt/intel/oneapi/ipp/2021.11/lib/cmake/ipp/") 15 | # find_package(IPP REQUIRED) 16 | 17 | catkin_simple(ALL_DEPS_REQUIRED) 18 | 19 | # SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") 20 | 21 | find_package(OpenCV REQUIRED) 22 | 23 | include_directories(include ${catkin_INCLUDE_DIRS}) 24 | 25 | # make the executable 26 | cs_add_executable(image_representation 27 | src/ImageRepresentation.cpp 28 | src/ImageRepresentation_node.cpp 29 | ) 30 | 31 | # link the executable to the necesarry libs 32 | target_link_libraries(image_representation 33 | ${catkin_LIBRARIES} 34 | ${OpenCV_LIBRARIES} 35 | # ${IPP_LIBRARIES} 36 | yaml-cpp 37 | ) 38 | 39 | cs_install() -------------------------------------------------------------------------------- /image_representation/cfg/image_representation_fast.yaml: -------------------------------------------------------------------------------- 1 | synchronize_on_external_time: True 2 | use_stereo_cam: True 3 | representation_mode: 2 4 | # Linear_TS //0 5 | # AA2(Local decay accumulation) //1 6 | decay_ms: 20 7 | median_blur_kernel_size: 1 8 | blur_size: 7 #15 for 1Mpx 9 | use_sim_time: True 10 | is_left: True 11 | x_patches: 8 12 | y_patches: 6 13 | generation_rate_hz: 100 -------------------------------------------------------------------------------- /image_representation/cfg/image_representation_fast_40hz.yaml: -------------------------------------------------------------------------------- 1 | synchronize_on_external_time: True 2 | use_stereo_cam: True 3 | representation_mode: 2 4 | # Linear_TS //0 5 | # AA2(Local decay accumulation) //1 6 | decay_ms: 20 7 | median_blur_kernel_size: 1 8 | blur_size: 7 #15 for 1Mpx 9 | use_sim_time: True 10 | is_left: True 11 | x_patches: 8 12 | y_patches: 6 13 | generation_rate_hz: 40 -------------------------------------------------------------------------------- /image_representation/cfg/image_representation_fast_r.yaml: -------------------------------------------------------------------------------- 1 | synchronize_on_external_time: True 2 | use_stereo_cam: True 3 | representation_mode: 2 4 | # Linear_TS //0 5 | # AA2(Local decay accumulation) //1 6 | decay_ms: 20 7 | median_blur_kernel_size: 1 8 | blur_size: 7 #15 for 1Mpx 9 | use_sim_time: True 10 | is_left: False 11 | x_patches: 8 12 | y_patches: 6 13 | generation_rate_hz: 100 -------------------------------------------------------------------------------- /image_representation/cfg/image_representation_fast_r_40hz.yaml: -------------------------------------------------------------------------------- 1 | synchronize_on_external_time: True 2 | use_stereo_cam: True 3 | representation_mode: 2 4 | # Linear_TS //0 5 | # AA2(Local decay accumulation) //1 6 | decay_ms: 20 7 | median_blur_kernel_size: 1 8 | blur_size: 7 #15 for 1Mpx 9 | use_sim_time: True 10 | is_left: False 11 | x_patches: 8 12 | y_patches: 6 13 | generation_rate_hz: 40 -------------------------------------------------------------------------------- /image_representation/include/image_representation/ImageRepresentation.h: -------------------------------------------------------------------------------- 1 | #ifndef image_representation_H_ 2 | #define image_representation_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | namespace image_representation 30 | { 31 | using EventQueue = std::deque; 32 | 33 | struct ROSTimeCmp 34 | { 35 | bool operator()(const ros::Time &a, const ros::Time &b) const 36 | { 37 | return a.toNSec() < b.toNSec(); 38 | } 39 | }; 40 | using GlobalEventQueue = std::map; 41 | 42 | inline static EventQueue::iterator EventBuffer_lower_bound( 43 | EventQueue &eb, ros::Time &t) 44 | { 45 | return std::lower_bound(eb.begin(), eb.end(), t, 46 | [](const dvs_msgs::Event &e, const ros::Time &t) 47 | { return e.ts.toSec() < t.toSec(); }); 48 | } 49 | 50 | inline static EventQueue::iterator EventBuffer_upper_bound( 51 | EventQueue &eb, ros::Time &t) 52 | { 53 | return std::upper_bound(eb.begin(), eb.end(), t, 54 | [](const ros::Time &t, const dvs_msgs::Event &e) 55 | { return t.toSec() < e.ts.toSec(); }); 56 | } 57 | 58 | inline static std::vector::iterator EventVector_lower_bound( 59 | std::vector &ev, double &t) 60 | { 61 | return std::lower_bound(ev.begin(), ev.end(), t, 62 | [](const dvs_msgs::Event &e, const double &t) 63 | { return e.ts.toSec() < t; }); 64 | } 65 | 66 | class ImageRepresentation 67 | { 68 | public: 69 | ImageRepresentation(ros::NodeHandle &nh, ros::NodeHandle nh_private); 70 | virtual ~ImageRepresentation(); 71 | 72 | static bool compare_time(const dvs_msgs::Event &e, const double reference_time) 73 | { 74 | return reference_time < e.ts.toSec(); 75 | } 76 | 77 | private: 78 | ros::NodeHandle nh_; 79 | // core 80 | void init(int width, int height); 81 | // Support: TS, AA, negative_TS, negative_TS_dx, negative_TS_dy 82 | void createImageRepresentationAtTime(const ros::Time &external_sync_time); 83 | void GenerationLoop(); 84 | 85 | // callbacks 86 | void eventsCallback(const dvs_msgs::EventArray::ConstPtr &msg); 87 | 88 | // utils 89 | void clearEventQueue(); 90 | bool loadCalibInfo(const std::string &cameraSystemDir, bool &is_left); 91 | void clearEvents(int distance, std::vector::iterator ptr_e); 92 | 93 | void AA_thread(std::vector::iterator &ptr_e, int distance, double external_t); 94 | void sobel(double external_t); 95 | bool fileExists(const std::string &filename); 96 | // tests 97 | 98 | // calibration parameters 99 | cv::Mat camera_matrix_, dist_coeffs_; 100 | cv::Mat rectification_matrix_, projection_matrix_; 101 | std::string distortion_model_; 102 | cv::Mat undistort_map1_, undistort_map2_; 103 | Eigen::Matrix2Xd precomputed_rectified_points_; 104 | 105 | // sub & pub 106 | ros::Subscriber event_sub_; 107 | ros::Subscriber camera_info_sub_; 108 | 109 | image_transport::Publisher dx_image_pub_, dy_image_pub_; 110 | image_transport::Publisher image_representation_pub_TS_; 111 | image_transport::Publisher image_representation_pub_negative_TS_; 112 | image_transport::Publisher image_representation_pub_AA_frequency_; 113 | image_transport::Publisher image_representation_pub_AA_mat_; 114 | 115 | bool left_; 116 | cv::Mat negative_TS_img; 117 | cv_bridge::CvImage cv_dx_image, cv_dy_image; 118 | std::thread thread_sobel; 119 | 120 | // online parameters 121 | bool bCamInfoAvailable_; 122 | bool bUse_Sim_Time_; 123 | cv::Size sensor_size_; 124 | ros::Time sync_time_; 125 | bool bSensorInitialized_; 126 | 127 | // offline parameters TODO 128 | double decay_ms_; 129 | bool ignore_polarity_; 130 | int median_blur_kernel_size_; 131 | int blur_size_; 132 | int max_event_queue_length_; 133 | int events_maintained_size_; 134 | 135 | // containers 136 | EventQueue events_; 137 | 138 | std::vector vEvents_; 139 | 140 | cv::Mat representation_TS_; 141 | cv::Mat representation_AA_; 142 | 143 | Eigen::MatrixXd TS_temp_map; 144 | 145 | // for rectify 146 | cv::Mat undistmap1_, undistmap2_; 147 | bool is_left_, bcreat_; 148 | 149 | // thread mutex 150 | std::mutex data_mutex_; 151 | 152 | enum RepresentationMode 153 | { 154 | Linear_TS, // 0 155 | AA2, // 1 156 | Fast // 2 157 | } representation_mode_; 158 | 159 | // parameters 160 | bool bUseStereoCam_; 161 | double decay_sec_; // TS param 162 | int generation_rate_hz_; 163 | int x_patches_, y_patches_; 164 | // std::vector::iterator ptr_e_; 165 | 166 | // calib info 167 | std::string calibInfoDir_; 168 | std::vector trapezoid_; 169 | }; 170 | } // namespace image_representation 171 | #endif // image_representation_H_ -------------------------------------------------------------------------------- /image_representation/include/image_representation/TicToc.h: -------------------------------------------------------------------------------- 1 | #ifndef esvo2_tictoc_H_ 2 | #define esvo2_tictoc_H_ 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace image_representation 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 // esvo2_tictoc_H_ 38 | -------------------------------------------------------------------------------- /image_representation/launch/image_representation.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | $(arg calibInfoDirStr) 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | $(arg calibInfoDirStr) 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /image_representation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | image_representation 4 | 0.0.0 5 | The image_representation 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 | -------------------------------------------------------------------------------- /image_representation/src/ImageRepresentation_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char* argv[]) 4 | { 5 | ros::init(argc, argv, "image_representation"); 6 | 7 | ros::NodeHandle nh; 8 | ros::NodeHandle nh_private("~"); 9 | 10 | bool left = true; 11 | image_representation::ImageRepresentation ts(nh, nh_private); 12 | 13 | ros::spin(); 14 | 15 | return 0; 16 | } 17 | -------------------------------------------------------------------------------- /results/gt/dsec/04b.txt: -------------------------------------------------------------------------------- 1 | 36607.498297000 0.000000000 0.000000000 0.000000000 -0.000000000 0.000000000 0.000000000 1.000000000 2 | 36607.700016000 0.004503936 -0.001206623 0.975690490 0.000372756 0.000128530 -0.000259091 0.999999889 3 | 36607.901736000 0.011311180 -0.012836439 2.011657318 0.000601149 0.000554007 -0.000215070 0.999999643 4 | 36608.103456000 0.015038367 -0.016669351 3.123489690 0.001207435 0.000853818 -0.000219943 0.999998882 5 | 36608.305176000 0.020670961 -0.020225910 4.285953492 0.002127514 0.001038221 -0.000483299 0.999997081 6 | 36608.506896000 0.029216575 -0.028253696 5.457188662 0.000253873 0.001263774 -0.000616960 0.999998979 7 | 36608.708616000 0.025506143 -0.027815959 6.660556074 -0.000578881 0.001242890 -0.000442108 0.999998962 8 | 36608.910335000 0.036697472 -0.024339100 7.918314798 0.001670859 0.001173704 -0.001151470 0.999997252 9 | 36609.112056000 0.039759458 -0.038956076 9.165138040 0.002426992 0.001184768 -0.000954300 0.999995898 10 | 36609.313776000 0.047160196 -0.043368468 10.437668873 0.002343166 0.001202540 -0.002309308 0.999993865 11 | 36609.515496000 0.054285035 -0.051511053 11.752208592 0.002186768 0.001535933 -0.004546242 0.999986095 12 | 36609.717216000 0.062550320 -0.064202536 13.070985956 0.001248068 0.001223769 -0.004527041 0.999988225 13 | 36609.918936000 0.067162035 -0.062679614 14.374389144 0.000588105 0.000527426 -0.004298551 0.999990449 14 | 36610.120656000 0.074381463 -0.058503332 15.679011653 0.001822011 -0.002054275 -0.003665881 0.999989511 15 | 36610.322375000 0.051970454 -0.061467836 16.967847699 0.003129944 -0.004971719 -0.001986384 0.999980770 16 | 36610.524095000 0.028886070 -0.055217864 18.228270658 0.002385709 -0.008541012 -0.001593971 0.999959409 17 | 36610.725815000 -0.007979550 -0.055120313 19.476815695 0.002902212 -0.011766595 -0.001144455 0.999925905 18 | 36610.927535000 -0.049094132 -0.055601765 20.707517964 0.002546376 -0.014317392 -0.000305697 0.999894211 19 | 36611.129255000 -0.083942040 -0.054411736 21.920497340 0.002589122 -0.016476735 0.000769681 0.999860600 20 | 36611.330975000 -0.143840611 -0.054240317 23.106712445 0.002696141 -0.018192307 0.001275392 0.999830058 21 | 36611.532695000 -0.204559216 -0.056385318 24.273253335 0.002863270 -0.019949495 0.001825193 0.999795221 22 | 36611.734414000 -0.247696289 -0.050523192 25.418662090 0.002305673 -0.021141432 0.002209351 0.999771397 23 | 36611.936134000 -0.299244027 -0.054609307 26.530156698 0.001827600 -0.022309799 0.002561818 0.999746150 24 | 36612.137854000 -0.347038053 -0.043944847 27.631556031 0.001518993 -0.023001250 0.002297416 0.999731644 25 | 36612.339574000 -0.403979490 -0.050932620 28.737559081 0.000886077 -0.023502699 0.003092825 0.999718596 26 | 36612.541294000 -0.443083049 -0.042472702 29.822428210 0.000287087 -0.023871845 0.003056718 0.999710311 27 | 36612.743014000 -0.507330286 -0.036094341 30.910676940 0.000458191 -0.023853989 0.004217507 0.999706449 28 | 36612.944734000 -0.519934619 -0.030648495 32.005658077 0.000062858 -0.023396746 0.003550738 0.999719949 29 | 36613.146453000 -0.580139624 -0.021206829 33.043106287 -0.000671236 -0.021228802 0.003546679 0.999768128 30 | 36613.348173000 -0.611778856 -0.005564318 34.090962406 -0.000755563 -0.018925368 0.003636593 0.999813998 31 | 36613.549893000 -0.625242118 0.008017008 35.110790904 -0.001009989 -0.014861465 0.003453658 0.999883088 32 | 36613.751613000 -0.630920121 0.017721661 36.115349813 -0.001189088 -0.009604136 0.003319648 0.999947661 33 | 36613.953333000 -0.620928404 0.029471152 37.088668745 -0.001371534 -0.001748765 0.003313788 0.999992040 34 | 36614.155053000 -0.609072163 0.051260567 38.050165763 -0.001157926 0.007845603 0.003499124 0.999962430 35 | 36614.356772000 -0.559194133 0.060677100 38.969922574 -0.001109053 0.018812845 0.003541158 0.999816135 36 | 36614.558492000 -0.477160231 0.064109306 39.881838616 -0.001476463 0.031283750 0.003385646 0.999503719 37 | 36614.760212000 -0.386288604 0.059262960 40.766435273 -0.002228768 0.044935108 0.003290205 0.998981997 38 | 36614.961932000 -0.273475355 0.061160558 41.623802290 -0.003093544 0.059258995 0.003562439 0.998231502 39 | 36615.163652000 -0.123954338 0.063110763 42.465025049 -0.004067862 0.074075706 0.003459411 0.997238317 40 | 36615.365372000 0.047736395 0.061207182 43.293106051 -0.004555909 0.088395393 0.002907411 0.996070771 41 | 36615.567091000 0.245187926 0.084114342 44.080977400 -0.004364939 0.103186258 0.002297302 0.994649801 42 | 36615.768811000 0.465349527 0.092025941 44.851589937 -0.003562463 0.118482433 0.001039532 0.992949255 43 | 36615.970532000 0.687859956 0.115213548 45.623046564 -0.003573874 0.136201744 0.000874072 0.990674286 44 | 36616.172252000 0.968917275 0.110910003 46.369473068 -0.003969247 0.155422570 0.000592214 0.987839892 45 | 36616.373972000 1.266059138 0.112830209 47.121729764 -0.004587301 0.175166060 0.000574040 0.984527870 46 | 36616.575692000 1.594934310 0.133556944 47.890307025 -0.004202721 0.196411757 0.001702835 0.980510855 47 | 36616.777411000 1.981324374 0.140661817 48.649439564 -0.003783714 0.218870163 0.002009697 0.975744453 48 | 36616.979131000 2.386164535 0.156695195 49.387951778 -0.003319197 0.242035402 0.002089494 0.970259540 49 | 36617.180851000 2.825767988 0.159347727 50.073195888 -0.003008597 0.266438407 0.002224339 0.963844412 50 | 36617.382571000 3.290738672 0.178923588 50.740940572 -0.001203518 0.290034855 0.001795389 0.957013551 51 | 36617.584291000 3.805593799 0.173741154 51.355983145 -0.000466410 0.314633770 0.000021010 0.949213076 52 | 36617.786011000 4.333788381 0.178648263 51.960460046 0.001049460 0.339428510 -0.000001246 0.940631098 53 | 36617.987730000 4.863776062 0.181896329 52.548248206 0.002525378 0.364284122 0.004688654 0.931272008 54 | 36618.189450000 5.466013895 0.202343461 53.069004490 0.000966145 0.387639524 0.003557306 0.921802802 55 | 36618.391170000 6.076131558 0.184544643 53.572123132 -0.000183713 0.412997161 0.004317105 0.910722401 56 | 36618.592890000 6.722594052 0.169688807 54.067698905 0.002042329 0.437248989 0.006679624 0.899313253 57 | 36618.794610000 7.393808686 0.205064887 54.508754611 0.009290884 0.460517074 0.006428199 0.887578419 58 | 36618.996330000 8.112521396 0.190116738 54.910757456 0.010691033 0.483686594 0.002618557 0.875172064 59 | 36619.198049000 8.810132197 0.186342751 55.267396876 0.010155689 0.506490677 0.002187798 0.862183751 60 | 36619.399769000 9.534782791 0.167760772 55.606228447 0.009925365 0.529459885 0.004944023 0.848261630 61 | 36619.601489000 10.260379183 0.182758059 55.895477901 0.008583839 0.550733530 0.006638248 0.834610518 62 | 36619.803209000 11.032686402 0.181863556 56.133060249 0.006084119 0.572178457 0.005297350 0.820088817 63 | 36620.004929000 11.797709199 0.173261031 56.355791987 0.006529477 0.591938489 0.005514792 0.805936233 64 | 36620.206649000 12.597625750 0.162655577 56.542948837 0.007597184 0.610803717 0.006904741 0.791714249 65 | 36620.408369000 13.425205631 0.169126225 56.720216849 0.009502062 0.628919971 0.006274769 0.777386998 66 | 36620.610088000 14.273132717 0.161395140 56.828851959 0.007635914 0.647241492 0.006367167 0.762217501 67 | 36620.811808000 15.154345748 0.166900497 56.894338351 0.004939127 0.665039288 0.004903216 0.746776647 68 | -------------------------------------------------------------------------------- /results/gt/dsec/04e.txt: -------------------------------------------------------------------------------- 1 | 36880.021832000 0.000000000 0.000000000 0.000000000 0.000000000 0.000000000 0.000000000 1.000000000 2 | 36880.223552000 -0.012447998 -0.001696407 0.760255634 0.000997717 -0.000619568 0.000966546 0.999998843 3 | 36880.425272000 -0.017169399 0.000228568 1.582144537 0.002227569 -0.001127302 0.001476250 0.999995794 4 | 36880.626992000 -0.014702867 -0.012304100 2.456492960 0.003111107 -0.002587035 0.001276397 0.999990999 5 | 36880.828711000 -0.018491246 -0.028610351 3.403552907 0.003288835 -0.005661102 0.000432549 0.999978474 6 | 36881.030431000 -0.041296651 -0.033671408 4.385785301 0.003811063 -0.010301706 0.000333437 0.999939619 7 | 36881.232151000 -0.083268762 -0.046559717 5.437334192 0.003738159 -0.016524182 0.000261306 0.999856446 8 | 36881.433871000 -0.131458184 -0.053780909 6.574163117 0.003603199 -0.026127539 -0.000048005 0.999652125 9 | 36881.635591000 -0.238345080 -0.061889496 7.741539201 0.003443531 -0.038827092 0.001198563 0.999239304 10 | 36881.837311000 -0.382613629 -0.069470480 8.935660089 0.004539420 -0.052917112 0.002243278 0.998586080 11 | 36882.039030000 -0.563313977 -0.074216447 10.169355847 0.005049490 -0.067110400 0.003963864 0.997724908 12 | 36882.240750000 -0.779512012 -0.087652493 11.398830868 0.005676338 -0.081629979 0.004668776 0.996635623 13 | 36882.442470000 -1.029270180 -0.101449782 12.650094040 0.007164277 -0.096783498 0.005066746 0.995266830 14 | 36882.644190000 -1.313077534 -0.126939044 13.944871315 0.006030239 -0.112084627 0.004243748 0.993671411 15 | 36882.845910000 -1.655075940 -0.158817885 15.238561172 0.000849673 -0.127844655 0.004067068 0.991785618 16 | 36883.047629000 -2.053806333 -0.156964679 16.594301156 -0.001890333 -0.145466210 0.002484293 0.989358353 17 | 36883.249349000 -2.533034209 -0.141465682 18.016181714 -0.000193538 -0.163161529 0.002992377 0.986595046 18 | 36883.451069000 -3.074686683 -0.143211420 19.446586226 0.001794593 -0.182167359 0.003521008 0.983259853 19 | 36883.652789000 -3.664675934 -0.148980308 20.829219376 0.002672292 -0.201348493 0.003742860 0.979509085 20 | 36883.854509000 -4.317478108 -0.157226776 22.218446800 0.002377499 -0.221677964 0.003333512 0.975111177 21 | 36884.056229000 -5.059354993 -0.167718945 23.612916557 0.001362437 -0.242176610 0.003402459 0.970225227 22 | 36884.257948000 -5.874255287 -0.184334622 25.026817140 0.002265641 -0.262626193 0.002427896 0.964891945 23 | 36884.459668000 -6.790702960 -0.193832960 26.469525995 0.003372208 -0.283363865 0.001287653 0.959006058 24 | 36884.661388000 -7.787506124 -0.206216944 27.890413569 0.000859310 -0.304070174 -0.000060635 0.952649692 25 | 36884.863108000 -8.878169151 -0.212403935 29.325500140 -0.000191399 -0.323418743 -0.000382839 0.946256556 26 | 36885.064829000 -10.065873580 -0.215452723 30.746135739 -0.001630914 -0.341912389 0.000134740 0.939730662 27 | 36885.266549000 -11.335837327 -0.207811414 32.167385890 -0.003893668 -0.358098138 0.001482711 0.933675066 28 | 36885.468268000 -12.673593513 -0.198763763 33.590889963 -0.001785499 -0.371365224 0.003431438 0.928480490 29 | 36885.669988000 -14.058055164 -0.185325572 35.013447543 -0.000360490 -0.380960734 0.003679800 0.924584074 30 | 36885.871708000 -15.487791266 -0.185398457 36.434471806 -0.000816515 -0.387747962 0.002996523 0.921760316 31 | 36886.073428000 -16.953511281 -0.187803754 37.849187317 -0.002288561 -0.392198964 0.003182039 0.919873865 32 | 36886.275148000 -18.478964128 -0.185213619 39.284743973 -0.004643002 -0.395650650 0.004670349 0.918378580 33 | 36886.476868000 -20.018387493 -0.173937964 40.710673090 -0.007230126 -0.400324324 0.004983732 0.916332400 34 | 36886.678587000 -21.624072452 -0.149929643 42.156737662 -0.009122341 -0.404647792 0.007143607 0.914400535 35 | 36886.880307000 -23.237943594 -0.129966802 43.599058313 -0.009872137 -0.409875034 0.008410135 0.912050551 36 | 36887.082027000 -24.893110422 -0.100890737 45.035239979 -0.009061942 -0.415071090 0.010155425 0.909689500 37 | 36887.283747000 -26.575542225 -0.100519243 46.460728846 -0.007564295 -0.419793880 0.012443159 0.907503095 38 | 36887.485467000 -28.274979100 -0.073653197 47.873639782 -0.005666411 -0.424219281 0.013671763 0.905439931 39 | 36887.687187000 -29.990682195 -0.073099071 49.281068062 -0.006615275 -0.427567971 0.014983019 0.903836149 40 | 36887.888906000 -31.732674911 -0.065462558 50.693599398 -0.007279235 -0.429701384 0.015899826 0.902803407 41 | 36888.090626000 -33.481856382 -0.052731895 52.106812572 -0.006983051 -0.430345237 0.015684993 0.902501881 42 | 36888.292346000 -35.230482918 -0.049262269 53.527180090 -0.007383731 -0.430496372 0.015488685 0.902431464 43 | 36888.494066000 -37.002763432 -0.048683440 54.967590508 -0.008027307 -0.430500154 0.014434133 0.902439768 44 | 36888.695786000 -38.788950634 -0.045450034 56.416117310 -0.007996055 -0.430717196 0.013539067 0.902349960 45 | 36888.897506000 -40.583921340 -0.033087374 57.878842937 -0.007784232 -0.430813633 0.012539192 0.902321307 46 | 36889.099225000 -42.382057995 -0.036309158 59.348686195 -0.007232536 -0.431052512 0.012199384 0.902214794 47 | 36889.300945000 -44.211242804 -0.024129615 60.834569102 -0.006596695 -0.431227537 0.011440474 0.902146349 48 | 36889.502665000 -46.062591275 -0.003312443 62.341249488 -0.006120611 -0.431158049 0.009696977 0.902204231 49 | 36889.704385000 -47.943973964 -0.003728353 63.869414273 -0.005292150 -0.431591737 0.008160184 0.902017345 50 | 36889.906105000 -49.826822064 0.005150318 65.404489697 -0.006559019 -0.431877936 0.005541156 0.901891551 51 | 36890.107824000 -51.744417485 0.037267096 66.938204978 -0.004876093 -0.432041787 0.003232133 0.901834962 52 | 36890.309544000 -53.688463004 0.051924421 68.494687388 -0.005325114 -0.432427306 0.002093819 0.901651337 53 | 36890.511264000 -55.636184564 0.079559847 70.047081593 -0.005744097 -0.432834301 0.002865905 0.901452351 54 | 36890.712984000 -57.613209013 0.112728166 71.629420075 -0.006020189 -0.432875011 0.003413845 0.901427423 55 | 36890.914704000 -59.577167607 0.139189688 73.207282496 -0.006674230 -0.432424794 0.003843356 0.901638629 56 | 36891.116424000 -61.550360335 0.172024646 74.788545092 -0.006966148 -0.431947753 0.005357046 0.901856734 57 | 36891.318143000 -63.513091858 0.200934012 76.368042317 -0.007576165 -0.431736509 0.006631614 0.901945166 58 | 36891.519863000 -65.493153895 0.229766850 77.969623263 -0.007683049 -0.431442626 0.006864766 0.902082951 59 | 36891.721583000 -67.464103707 0.264008343 79.561020747 -0.007222104 -0.430772268 0.006441176 0.902409029 60 | 36891.923303000 -69.440022662 0.295644042 81.171909126 -0.007416156 -0.430103493 0.006555978 0.902726788 61 | 36892.125023000 -71.392036545 0.321875312 82.749173004 -0.007765024 -0.429319585 0.006843081 0.903093369 62 | 36892.326743000 -73.353277014 0.361452455 84.351947343 -0.007865053 -0.428844136 0.007331530 0.903315710 63 | 36892.528462000 -75.313750339 0.389406781 85.968834210 -0.008059315 -0.428380857 0.008683270 0.903521265 64 | 36892.730182000 -77.267125571 0.425059480 87.586259103 -0.007959603 -0.427846057 0.007630544 0.903784356 65 | 36892.931902000 -79.215795951 0.449595308 89.182491215 -0.007045049 -0.427621582 0.007678754 0.903898134 66 | 36893.133622000 -81.147966042 0.471273860 90.770757644 -0.007211099 -0.427267466 0.006469769 0.904074345 67 | 36893.335342000 -83.042522250 0.503617422 92.337144507 -0.007430837 -0.427091349 0.004641291 0.904167998 68 | 36893.537062000 -84.963822134 0.563538463 93.920321934 -0.007194295 -0.426905784 0.004277514 0.904258689 69 | 36893.738781000 -86.856630080 0.590495703 95.483968667 -0.007354275 -0.426911473 0.004033693 0.904255550 70 | 36893.940501000 -88.740727392 0.621568533 97.042703072 -0.007282765 -0.426895905 0.003520921 0.904266535 71 | 36894.142222000 -90.625283532 0.669345722 98.607228561 -0.006818176 -0.426693530 0.003669651 0.904365065 72 | 36894.343942000 -92.497072910 0.698492897 100.154552878 -0.007921509 -0.426684873 0.002904974 0.904360672 73 | 36894.545662000 -94.341228387 0.743855855 101.675102954 -0.008122064 -0.426716507 0.003165238 0.904343294 74 | --------------------------------------------------------------------------------