├── README.md ├── dependencies.yaml ├── esvo_core ├── CMakeLists.txt ├── calib │ ├── dsec │ │ └── zurich_city_04_a │ │ │ ├── left.yaml │ │ │ └── right.yaml │ ├── hkust │ │ ├── left.yaml │ │ └── right.yaml │ ├── rpg │ │ ├── left.yaml │ │ └── right.yaml │ └── upenn │ │ ├── left.yaml │ │ └── right.yaml ├── cfg │ ├── DVS_MappingStereo.cfg │ ├── mapping │ │ ├── mapping_dsec.yaml │ │ ├── mapping_hkust.yaml │ │ ├── mapping_rpg.yaml │ │ └── mapping_upenn.yaml │ ├── mvstereo │ │ ├── mvstereo_rpg.yaml │ │ └── mvstereo_upenn.yaml │ ├── time_surface │ │ └── ts_parameters.yaml │ └── tracking │ │ ├── tracking_dsec.yaml │ │ ├── tracking_hkust.yaml │ │ ├── tracking_rpg.yaml │ │ └── tracking_upenn.yaml ├── esvo_mvstereo.perspective ├── esvo_mvstereo.rviz ├── esvo_system.perspective ├── esvo_system.rviz ├── esvo_system_DSEC.perspective ├── esvo_system_DSEC.rviz ├── include │ └── esvo_core │ │ ├── container │ │ ├── CameraSystem.h │ │ ├── DepthMap.h │ │ ├── DepthPoint.h │ │ ├── EventMatchPair.h │ │ ├── ResidualItem.h │ │ ├── SmartGrid.h │ │ └── TimeSurfaceObservation.h │ │ ├── core │ │ ├── DepthFusion.h │ │ ├── DepthProblem.h │ │ ├── DepthProblemSolver.h │ │ ├── DepthRegularization.h │ │ ├── EventBM.h │ │ ├── EventMatcher.h │ │ ├── RegProblemLM.h │ │ └── RegProblemSolverLM.h │ │ ├── esvo_MVStereo.h │ │ ├── esvo_Mapping.h │ │ ├── esvo_Tracking.h │ │ ├── optimization │ │ └── OptimizationFunctor.h │ │ └── tools │ │ ├── TicToc.h │ │ ├── Visualization.h │ │ ├── cayley.h │ │ ├── params_helper.h │ │ ├── sobel.h │ │ └── utils.h ├── launch │ ├── mvstereo │ │ ├── mvstereo_rpg.launch │ │ └── mvstereo_upenn.launch │ └── system │ │ ├── DSEC │ │ └── system_zurich_city_04_a.launch │ │ ├── system_hkust.launch │ │ ├── system_rpg.launch │ │ └── system_upenn.launch ├── package.xml └── src │ ├── container │ ├── CameraSystem.cpp │ ├── DepthPoint.cpp │ ├── EventPoint.cpp │ └── ResidualItem.cpp │ ├── core │ ├── DepthFusion.cpp │ ├── DepthProblem.cpp │ ├── DepthProblemSolver.cpp │ ├── DepthRegularization.cpp │ ├── EventBM.cpp │ ├── EventMatcher.cpp │ ├── RegProblemLM.cpp │ └── RegProblemSolverLM.cpp │ ├── esvo_MVStereo.cpp │ ├── esvo_MVStereoNode.cpp │ ├── esvo_Mapping.cpp │ ├── esvo_MappingNode.cpp │ ├── esvo_Tracking.cpp │ ├── esvo_TrackingNode.cpp │ └── tools │ ├── Visualization.cpp │ ├── cayley.cpp │ └── sobel.cpp ├── esvo_time_surface ├── CMakeLists.txt ├── cfg │ └── parameters.yaml ├── esvo_time_surface.perspective ├── include │ └── esvo_time_surface │ │ ├── TicToc.h │ │ └── TimeSurface.h ├── launch │ ├── rosbag_launcher │ │ ├── dsec │ │ │ └── zurich_city_04_a │ │ │ │ ├── calib_info.launch │ │ │ │ └── zurich_city_04_a.launch │ │ ├── hkust │ │ │ ├── hkust_calib_info.launch │ │ │ └── hkust_lab.launch │ │ ├── rpg │ │ │ ├── rpg_bin.launch │ │ │ ├── rpg_boxes.launch │ │ │ ├── rpg_calib_info.launch │ │ │ ├── rpg_desk.launch │ │ │ └── rpg_monitor.launch │ │ └── upenn │ │ │ ├── upenn_indoor_flying1.launch │ │ │ └── upenn_indoor_flying3.launch │ └── stereo_time_surface.launch ├── package.xml └── src │ ├── TimeSurface.cpp │ └── TimeSurface_node.cpp ├── events_repacking_helper ├── CMakeLists.txt ├── README.md ├── launch │ └── repacking.launch ├── package.xml └── src │ └── EventMessageEditor.cpp └── stereo_rig_design ├── Stereo_Rig_Base.pdf └── Stereo_Rig_Base.step /dependencies.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | catkin_simple: 3 | type: git 4 | url: https://github.com/catkin/catkin_simple.git 5 | version: master 6 | rpg_dvs_ros: 7 | type: git 8 | url: https://github.com/uzh-rpg/rpg_dvs_ros.git 9 | version: master 10 | gflags_catkin: 11 | type: git 12 | url: https://github.com/ethz-asl/gflags_catkin.git 13 | version: master 14 | glog_catkin: 15 | type: git 16 | url: https://github.com/ethz-asl/glog_catkin.git 17 | version: master 18 | minkindr: 19 | type: git 20 | url: https://github.com/ethz-asl/minkindr.git 21 | version: master 22 | eigen_catkin: 23 | type: git 24 | url: https://github.com/ethz-asl/eigen_catkin.git 25 | version: master 26 | eigen_checks: 27 | type: git 28 | url: https://github.com/ethz-asl/eigen_checks.git 29 | version: master 30 | minkindr_ros: 31 | type: git 32 | url: https://github.com/ethz-asl/minkindr_ros.git 33 | version: master 34 | 35 | -------------------------------------------------------------------------------- /esvo_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(esvo_core) 3 | # explicitly set std=c++14 to remove errors from pcl library 4 | set(CMAKE_CXX_STANDARD 14) 5 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 6 | # SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") 7 | set(CMAKE_CXX_FLAGS "-O3") 8 | 9 | find_package(catkin_simple REQUIRED) 10 | catkin_simple(ALL_DEPS_REQUIRED) 11 | 12 | find_package(OpenCV REQUIRED) 13 | 14 | ########### 15 | ## Build ## 16 | ########### 17 | 18 | ## Specify additional locations of header files 19 | ## Your package locations should be listed before other locations 20 | include_directories(include) 21 | 22 | set(HEADERS 23 | include/esvo_core/core/DepthFusion.h 24 | include/esvo_core/core/DepthRegularization.h 25 | include/esvo_core/core/DepthProblem.h 26 | include/esvo_core/core/DepthProblemSolver.h 27 | include/esvo_core/core/EventBM.h 28 | include/esvo_core/core/RegProblemLM.h 29 | include/esvo_core/core/RegProblemSolverLM.h 30 | include/esvo_core/core/EventMatcher.h 31 | include/esvo_core/optimization/OptimizationFunctor.h 32 | include/esvo_core/container/CameraSystem.h 33 | include/esvo_core/container/DepthPoint.h 34 | include/esvo_core/container/EventMatchPair.h 35 | include/esvo_core/container/SmartGrid.h 36 | include/esvo_core/container/DepthMap.h 37 | include/esvo_core/container/ResidualItem.h 38 | include/esvo_core/container/TimeSurfaceObservation.h 39 | include/esvo_core/tools/Visualization.h 40 | include/esvo_core/tools/utils.h 41 | include/esvo_core/tools/TicToc.h 42 | include/esvo_core/tools/sobel.h 43 | include/esvo_core/tools/cayley.h 44 | include/esvo_core/tools/params_helper.h) 45 | 46 | set(SOURCES 47 | src/core/DepthFusion.cpp 48 | src/core/DepthRegularization.cpp 49 | src/core/DepthProblem.cpp 50 | src/core/DepthProblemSolver.cpp 51 | src/core/EventBM.cpp 52 | src/core/RegProblemLM.cpp 53 | src/core/RegProblemSolverLM.cpp 54 | src/core/EventMatcher.cpp 55 | src/container/CameraSystem.cpp 56 | src/container/DepthPoint.cpp 57 | src/container/ResidualItem.cpp 58 | src/tools/Visualization.cpp 59 | src/tools/sobel.cpp 60 | src/tools/cayley.cpp) 61 | 62 | cs_add_library(${PROJECT_NAME}_LIB ${SOURCES} ${HEADERS}) 63 | 64 | # Node esvo_Mapping 65 | cs_add_executable(esvo_Mapping src/esvo_MappingNode.cpp 66 | src/esvo_Mapping.cpp include/esvo_core/esvo_Mapping.h) 67 | target_link_libraries(esvo_Mapping ${PROJECT_NAME}_LIB 68 | ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} yaml-cpp) 69 | 70 | # Node esvo_Tracking 71 | cs_add_executable(esvo_Tracking src/esvo_TrackingNode.cpp 72 | src/esvo_Tracking.cpp include/esvo_core/esvo_Tracking.h) 73 | target_link_libraries(esvo_Tracking ${PROJECT_NAME}_LIB 74 | ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} yaml-cpp) 75 | 76 | # Node esvo_MVS (newly added in branch "mvs") 77 | cs_add_executable(esvo_MVStereo 78 | src/esvo_MVStereoNode.cpp 79 | src/esvo_MVStereo.cpp 80 | include/esvo_core/esvo_MVStereo.h) 81 | target_link_libraries(esvo_MVStereo ${PROJECT_NAME}_LIB 82 | ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} yaml-cpp) 83 | -------------------------------------------------------------------------------- /esvo_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: [534.094, 0, 335.446, 0, 25 | 0, 534.094, 223.233, 0, 26 | 0, 0, 1, 0] 27 | T_right_left: 28 | rows: 3 29 | cols: 4 30 | data: [0.999759, -0.0113618, 0.0187657, -0.599011, 31 | 0.0114448, 0.999925, -0.0043174, -0.00490041, 32 | -0.0187153, -0.0187153, 0.999815, 0.0016045] 33 | -------------------------------------------------------------------------------- /esvo_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: [534.094, 0, 335.446, -319.94, 25 | 0, 534.094, 223.233, 0, 26 | 0, 0, 1, 0] 27 | T_right_left: 28 | rows: 3 29 | cols: 4 30 | data: [0.999759, -0.0113618, 0.0187657, -0.599011, 31 | 0.0114448, 0.999925, -0.0043174, -0.00490041, 32 | -0.0187153, -0.0187153, 0.999815, 0.0016045] 33 | -------------------------------------------------------------------------------- /esvo_core/calib/hkust/left.yaml: -------------------------------------------------------------------------------- 1 | image_width: 346 2 | image_height: 260 3 | camera_name: hkust_DAVIS346_left 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [263.796, 0, 176.994, 0, 263.738, 124.373, 0, 0, 1] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 4 12 | data: [-0.386589, 0.157241, 0.000322143, 6.13759e-06] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.999809, 0.0161928, 0.0109163, 17 | -0.0162088, 0.999868, 0.0013701, 18 | -0.0108927, -0.00154678, 0.999939] 19 | projection_matrix: 20 | rows: 3 21 | cols: 4 22 | data: [189.705, 0, 165.382, 0, 0, 189.705, 121.295, 0, 0, 0, 1, 0] 23 | T_right_left: 24 | rows: 3 25 | cols: 4 26 | data: [9.99990798e-01, -6.32492385e-04, -4.24307214e-03, -7.30597639e-02, 27 | 6.44736387e-04, 9.99995631e-01, 2.88489843e-03, -1.23275257e-03, 28 | 4.24122892e-03, -2.88760755e-03, 9.99986837e-01, -1.10420407e-03] -------------------------------------------------------------------------------- /esvo_core/calib/hkust/right.yaml: -------------------------------------------------------------------------------- 1 | image_width: 346 2 | image_height: 260 3 | camera_name: hkust_DAVIS346_right 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [263.485, 0, 162.942, 0, 263.276, 118.029, -0.0151344, 0.00133093, 0.999885] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 4 12 | data: [-0.383425, 0.152823, -0.000257745, 0.000268432] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.9993960957463914, 0.0034732142808621717, -0.03457427641222047, 17 | -0.0035085878889783376, 0.9999933816804096, -0.0009625000798637905, 18 | 0.03457070461958685, 0.0010832257094615543, 0.9994016675011942] 19 | projection_matrix: 20 | rows: 3 21 | cols: 4 22 | data: [189.705, 0, 165.382, -13.8634, 0, 189.705, 121.295, 0, 0, 0, 1, 0] 23 | T_right_left: 24 | rows: 3 25 | cols: 4 26 | data: [9.99990798e-01, -6.32492385e-04, -4.24307214e-03, -7.30597639e-02, 27 | 6.44736387e-04, 9.99995631e-01, 2.88489843e-03, -1.23275257e-03, 28 | 4.24122892e-03, -2.88760755e-03, 9.99986837e-01, -1.10420407e-03] -------------------------------------------------------------------------------- /esvo_core/calib/rpg/left.yaml: -------------------------------------------------------------------------------- 1 | image_width: 240 2 | image_height: 180 3 | camera_name: rpg_DAVIS240C_left 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [196.639, 0, 105.064, 0, 196.733, 72.4717, 0.0, 0.0, 1.0] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 4 12 | data: [-0.336733, 0.111789, -0.00140053, -0.000459594] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.999791, -0.018779, -0.00802416, 0.0187767, 0.999824, -0.000360707, 0.00802952, 0.000209964, 0.999968] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [156.925, 0, 108.167, 0, 0, 156.925, 78.4205, 0, 0, 0, 1, 0] 21 | T_right_left: 22 | rows: 3 23 | cols: 4 24 | data: [0.9991089760393723, -0.04098010198963204, 0.010093821797214667, -0.1479883582369969, 25 | 0.04098846609277917, 0.9991594254283246, -0.000623077121092687, -0.003289908601915284, 26 | -0.010059803423311134, 0.0010362522169301642, 0.9999488619606629, 0.0026798262366239016] -------------------------------------------------------------------------------- /esvo_core/calib/rpg/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] -------------------------------------------------------------------------------- /esvo_core/calib/upenn/left.yaml: -------------------------------------------------------------------------------- 1 | image_width: 346 2 | image_height: 260 3 | camera_name: upenn_DAVIS346_left 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [226.38018519795807, 0.0, 173.6470807871759, 0.0, 226.15002947047415, 133.73271487507847, 0, 0, 1] 8 | distortion_model: equidistant 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 4 12 | data: [-0.048031442223833355, 0.011330957517194437, -0.055378166304281135, 0.021500973881459395] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.999877311526236, 0.015019439766575743, -0.004447282784398257, 17 | -0.014996983873604017, 0.9998748347535599, 0.005040367172759556, 18 | 0.004522429630305261, -0.004973052949604937, 0.9999774079320989] 19 | projection_matrix: 20 | rows: 3 21 | cols: 4 22 | data: [199.6530123165822, 0.0, 177.43276376280926, 0.0, 23 | 0.0, 199.6530123165822, 126.81215684365904, 0.0, 24 | 0.0, 0.0, 1.0, 0.0] 25 | T_right_left: 26 | rows: 3 27 | cols: 4 28 | data: [0.9999285439274112, 0.011088072985503046, -0.004467849222081981, -0.09988137641750752, 29 | -0.011042817783611191, 0.9998887260774646, 0.01002953830336461, -0.0003927067773089277, 30 | 0.004578560319692358, -0.009979483987103495, 0.9999397215256256, 1.8880107752680777e-06] -------------------------------------------------------------------------------- /esvo_core/calib/upenn/right.yaml: -------------------------------------------------------------------------------- 1 | image_width: 346 2 | image_height: 260 3 | camera_name: upenn_DAVIS346_right 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [226.0181418548734, 0, 174.5433576736815, 0, 225.7869434267677, 124.21627572590607, 0, 0, 1] 8 | distortion_model: equidistant 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 4 12 | data: [-0.04846669832871334, 0.010092844338123635, -0.04293073765014637, 0.005194706897326005] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.9999922706537476, 0.003931701344419404, -1.890238450965101e-05, 17 | -0.003931746704476347, 0.9999797362744968, -0.005006836150689904, 18 | -7.83382948021244e-07, 0.0050068717705076754, 0.9999874655386736] 19 | projection_matrix: 20 | rows: 3 21 | cols: 4 22 | data: [199.6530123165822, 0.0, 177.43276376280926, -19.941771812941038, 23 | 0.0, 199.6530123165822, 126.81215684365904, 0.0, 24 | 0.0, 0.0, 1.0, 0.0] 25 | T_right_left: 26 | rows: 3 27 | cols: 4 28 | data: [0.9999285439274112, 0.011088072985503046, -0.004467849222081981, -0.09988137641750752, 29 | -0.011042817783611191, 0.9998887260774646, 0.01002953830336461, -0.0003927067773089277, 30 | 0.004578560319692358, -0.009979483987103495, 0.9999397215256256, 1.8880107752680777e-06] -------------------------------------------------------------------------------- /esvo_core/cfg/DVS_MappingStereo.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "esvo_core" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | # for EventMatcher 9 | gen.add("EM_TIME_THRESHOLD", double_t, 0, "EM_TIME_THRESHOLD", 0.0001, 0.000001, 0.001) 10 | gen.add("EM_EPIPOLAR_THRESHOLD", double_t, 0, "EM_EPIPOLAR_THRESHOLD", 0.5, 0.1, 5) 11 | gen.add("EM_TS_NCC_THRESHOLD", double_t, 0, "EM_TS_NCC_THRESHOLD", 0.1, 0.0, 1.0) 12 | gen.add("EM_NUM_EVENT_MATCHING", int_t, 0, "EM_NUM_EVENT_MATCHING", 30000, 0, 100000) 13 | gen.add("EM_PATCH_INTENSITY_THRESHOLD", int_t, 0, "EM_PATCH_INTENSITY_THRESHOLD", 125, 0, 255) 14 | gen.add("EM_PATCH_VALID_RATIO", double_t, 0, "EM_PATCH_VALID_RATIO", 0.1, 0, 1.0) 15 | 16 | # for Block Matcher 17 | gen.add("BM_MAX_NUM_EVENTS_PER_MATCHING", int_t, 0, "BM_MAX_NUM_EVENTS_PER_MATCHING", 400, 1, 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 esvo_Mapping 24 | gen.add("invDepth_min_range", double_t, 0, "InvDepth_MIN_RANGE", 0.16, 0.0, 10.0) 25 | gen.add("invDepth_max_range", double_t, 0, "InvDepth_MAX_RANGE", 2.0, 0.0, 10.0) 26 | 27 | gen.add("residual_vis_threshold", double_t, 0, "Residual_VIS_THRESHOLD", 12, 0.0, 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 esvo_System 44 | gen.add("ResetButton", bool_t, 0, "reset system (used as a button...)", False) 45 | exit(gen.generate(PACKAGE, "esvo_core", "DVS_MappingStereo")) -------------------------------------------------------------------------------- /esvo_core/cfg/mapping/mapping_dsec.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.001 2 | invDepth_max_range: 0.25 3 | residual_vis_threshold: 30 #20 #30 #20 #20 4 | stdVar_vis_threshold: 1 #0.1 #0.2 5 | age_max_range: 10 6 | age_vis_threshold: 1 7 | fusion_radius: 1 8 | FUSION_STRATEGY: CONST_FRAMES #"CONST_POINTS" # "CONST_FRAMES" 9 | maxNumFusionFrames: 5 #40 10 | maxNumFusionPoints: 20000 #8000 #3000 11 | Denoising: False 12 | SmoothTimeSurface: True #False 13 | Regularization: True 14 | bVisualizeGlobalPC: True 15 | visualizeGPC_interval: 0.5 #2[second] 16 | NumGPC_added_per_refresh: 10000 17 | visualize_range: 30 18 | PROCESS_EVENT_NUM: 10000 #20000 19 | TS_HISTORY_LENGTH: 100 20 | INIT_SGM_DP_NUM_THRESHOLD: 500 21 | mapping_rate_hz: 20 22 | # DepthProblemConfig 23 | patch_size_X: 15 #15 24 | patch_size_Y: 7 #7 25 | LSnorm: Tdist #Tdist #Tdist #Tdist # l2 26 | Tdist_nu: 2.182 #2.1897 27 | Tdist_scale: 17.277 #16.6397 28 | Tdist_stdvar: 59.763 #56.5347 29 | RegularizationRadius: 20 30 | RegularizationMinNeighbours: 32 31 | RegularizationMinCloseNeighbours: 32 32 | # EventBM parameters 33 | BM_half_slice_thickness: 0.001 34 | BM_min_disparity: 0 35 | BM_max_disparity: 150 36 | BM_step: 1 37 | BM_ZNCC_Threshold: 0.1 38 | BM_bUpDownConfiguration: False -------------------------------------------------------------------------------- /esvo_core/cfg/mapping/mapping_hkust.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.25 2 | invDepth_max_range: 2 3 | residual_vis_threshold: 20 4 | stdVar_vis_threshold: 0.15 5 | age_max_range: 10 6 | age_vis_threshold: 1 7 | fusion_radius: 0 8 | FUSION_STRATEGY: "CONST_FRAMES" #"CONST_POINTS"# #"CONST_FRAMES"# 9 | maxNumFusionFrames: 20 10 | maxNumFusionPoints: 4000 11 | Denoising: True 12 | SmoothTimeSurface: False 13 | Regularization: True 14 | bVisualizeGlobalPC: True 15 | visualizeGPC_interval: 1 # (unit: s) 16 | NumGPC_added_per_refresh: 1500 17 | visualize_range: 2.5 18 | PROCESS_EVENT_NUM: 1000 19 | TS_HISTORY_LENGTH: 100 20 | INIT_SGM_DP_NUM_THRESHOLD: 500 21 | mapping_rate_hz: 20 22 | # DepthProblemConfig 23 | patch_size_X: 15 24 | patch_size_Y: 7 25 | Lnorm: Tdist #(Tdist or l2) 26 | Tdist_nu: 2.1897 27 | Tdist_scale: 16.6397 28 | Tdist_stdvar: 56.5347 29 | # EventBM parameters 30 | BM_half_slice_thickness: 0.001 31 | BM_min_disparity: 1 32 | BM_max_disparity: 40 33 | BM_step: 1 34 | BM_ZNCC_Threshold: 0.1 35 | BM_bUpDownConfiguration: False -------------------------------------------------------------------------------- /esvo_core/cfg/mapping/mapping_rpg.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.2 2 | invDepth_max_range: 2 3 | residual_vis_threshold: 20 4 | stdVar_vis_threshold: 0.015 5 | age_max_range: 10 6 | age_vis_threshold: 1 7 | fusion_radius: 0 8 | FUSION_STRATEGY: "CONST_POINTS" #"CONST_FRAMES" or "CONST_POINTS" 9 | maxNumFusionFrames: 40 10 | maxNumFusionPoints: 5000 11 | Denoising: True 12 | SmoothTimeSurface: False 13 | Regularization: True 14 | bVisualizeGlobalPC: True 15 | visualizeGPC_interval: 3 16 | NumGPC_added_per_refresh: 1000 17 | visualize_range: 5.0 18 | PROCESS_EVENT_NUM: 1000 19 | TS_HISTORY_LENGTH: 100 20 | INIT_SGM_DP_NUM_THRESHOLD: 500 21 | mapping_rate_hz: 20 22 | # DepthProblemConfig 23 | patch_size_X: 15 24 | patch_size_Y: 7 25 | Lnorm: Tdist #(Tdist or l2) 26 | Tdist_nu: 2.1897 27 | Tdist_scale: 16.6397 28 | Tdist_stdvar: 56.5347 29 | # EventBM parameters 30 | BM_half_slice_thickness: 0.001 31 | BM_min_disparity: 1 32 | BM_max_disparity: 40 33 | BM_step: 1 34 | BM_ZNCC_Threshold: 0.1 35 | BM_bUpDownConfiguration: False -------------------------------------------------------------------------------- /esvo_core/cfg/mapping/mapping_upenn.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.16 2 | invDepth_max_range: 1.0 3 | residual_vis_threshold: 20 4 | stdVar_vis_threshold: 0.15 5 | age_max_range: 10 6 | age_vis_threshold: 1 7 | fusion_radius: 0 8 | FUSION_STRATEGY: "CONST_POINTS" # "CONST_FRAMES" 9 | maxNumFusionFrames: 40 10 | maxNumFusionPoints: 3000 11 | Denoising: False 12 | SmoothTimeSurface: False 13 | Regularization: False 14 | bVisualizeGlobalPC: True 15 | visualizeGPC_interval: 1 16 | NumGPC_added_per_refresh: 3000 17 | visualize_range: 5.0 18 | PROCESS_EVENT_NUM: 1000 19 | TS_HISTORY_LENGTH: 100 20 | INIT_SGM_DP_NUM_THRESHOLD: 500 21 | mapping_rate_hz: 20 22 | # DepthProblemConfig 23 | patch_size_X: 15 24 | patch_size_Y: 7 25 | LSnorm: Tdist # l2 26 | Tdist_nu: 2.182 27 | Tdist_scale: 17.277 28 | Tdist_stdvar: 59.763 29 | # EventBM parameters 30 | BM_half_slice_thickness: 0.001 31 | BM_min_disparity: 1 32 | BM_max_disparity: 40 33 | BM_step: 1 34 | BM_ZNCC_Threshold: 0.1 35 | BM_bUpDownConfiguration: False -------------------------------------------------------------------------------- /esvo_core/cfg/mvstereo/mvstereo_rpg.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.2 2 | invDepth_max_range: 2 3 | residual_vis_threshold: 20 4 | stdVar_vis_threshold: 0.015 5 | age_max_range: 10 6 | age_vis_threshold: 1 7 | fusion_radius: 0 8 | FUSION_STRATEGY: "CONST_FRAMES" #"CONST_FRAMES" or "CONST_POINTS" 9 | maxNumFusionFrames: 40 10 | maxNumFusionPoints: 5000 11 | Denoising: True 12 | SmoothTimeSurface: False 13 | Regularization: True 14 | PROCESS_EVENT_NUM: 1000 15 | TS_HISTORY_LENGTH: 100 16 | mapping_rate_hz: 20 17 | # DepthProblemConfig 18 | patch_size_X: 15 19 | patch_size_Y: 7 20 | LSnorm: Tdist #(Tdist or l2) 21 | Tdist_nu: 2.1897 22 | Tdist_scale: 16.6397 23 | Tdist_stdvar: 56.5347 24 | # Event Matcher (EM) parameters 25 | EM_Slice_Thickness: 0.001 26 | EM_Time_THRESHOLD: 0.0005 27 | EM_EPIPOLAR_THRESHOLD: 1.0 28 | EM_TS_NCC_THRESHOLD: 0.1 29 | EM_NUM_EVENT_MATCHING: 3000 30 | EM_PATCH_INTENSITY_THRESHOLD: 10 31 | EM_PATCH_VALID_RATIO: 0.75 32 | # EventBM parameters 33 | BM_half_slice_thickness: 0.001 34 | BM_min_disparity: 1 35 | BM_max_disparity: 40 36 | BM_step: 1 37 | BM_ZNCC_Threshold: 0.1 38 | BM_bUpDownConfiguration: False 39 | # MVStereo Mode 40 | MVStereoMode: 3 41 | # PURE_EVENT_MATCHING //0 EM [26] 42 | # PURE_BLOCK_MATCHING //1 BM 43 | # EM_PLUS_ESTIMATION //2 EM [26] + nonlinear opt. 44 | # BM_PLUS_ESTIMATION //3 ESVO's mapper 45 | # SEMI_GLOBAL_MATCHING //4 [45] -------------------------------------------------------------------------------- /esvo_core/cfg/mvstereo/mvstereo_upenn.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.16 2 | invDepth_max_range: 1.0 3 | residual_vis_threshold: 20 4 | stdVar_vis_threshold: 0.15 5 | age_max_range: 10 6 | age_vis_threshold: 1 7 | fusion_radius: 0 8 | FUSION_STRATEGY: "CONST_POINTS" #"CONST_FRAMES" or "CONST_POINTS" 9 | maxNumFusionFrames: 40 10 | maxNumFusionPoints: 3000 11 | Denoising: False 12 | SmoothTimeSurface: False 13 | Regularization: False 14 | PROCESS_EVENT_NUM: 1000 15 | TS_HISTORY_LENGTH: 100 16 | mapping_rate_hz: 20 17 | # DepthProblemConfig 18 | patch_size_X: 15 19 | patch_size_Y: 7 20 | LSnorm: Tdist #(Tdist or l2) 21 | Tdist_nu: 2.182 22 | Tdist_scale: 17.277 23 | Tdist_stdvar: 59.763 24 | # Event Matcher (EM) parameters 25 | EM_Slice_Thickness: 0.001 26 | EM_Time_THRESHOLD: 0.0005 27 | EM_EPIPOLAR_THRESHOLD: 1.0 28 | EM_TS_NCC_THRESHOLD: 0.1 29 | EM_NUM_EVENT_MATCHING: 3000 30 | EM_PATCH_INTENSITY_THRESHOLD: 10 31 | EM_PATCH_VALID_RATIO: 0.75 32 | # EventBM parameters 33 | BM_half_slice_thickness: 0.001 34 | BM_min_disparity: 1 35 | BM_max_disparity: 40 36 | BM_step: 1 37 | BM_ZNCC_Threshold: 0.1 38 | BM_bUpDownConfiguration: False 39 | # MVStereo Mode 40 | MVStereoMode: 3 41 | # PURE_EVENT_MATCHING //0 EM [26] 42 | # PURE_BLOCK_MATCHING //1 BM 43 | # EM_PLUS_ESTIMATION //2 EM [26] + nonlinear opt. 44 | # BM_PLUS_ESTIMATION //3 ESVO's mapper 45 | # SEMI_GLOBAL_MATCHING //4 [45] -------------------------------------------------------------------------------- /esvo_core/cfg/time_surface/ts_parameters.yaml: -------------------------------------------------------------------------------- 1 | use_sim_time: True 2 | ignore_polarity: True 3 | time_surface_mode: 0 # 0: Backward; 1: Forward 4 | decay_ms: 30 5 | median_blur_kernel_size: 1 6 | max_event_queue_len: 20 -------------------------------------------------------------------------------- /esvo_core/cfg/tracking/tracking_dsec.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.02 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: 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: False 17 | PATH_TO_SAVE_TRAJECTORY: "/home/zhouyi/Desktop/ESVO_result/dsec_zurich_city_04_b/" 18 | VISUALIZE_TRAJECTORY: True -------------------------------------------------------------------------------- /esvo_core/cfg/tracking/tracking_hkust.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.25 2 | invDepth_max_range: 2 3 | TS_HISTORY_LENGTH: 100 4 | REF_HISTORY_LENGTH: 10 5 | tracking_rate_hz: 100 6 | patch_size_X: 1 7 | patch_size_Y: 1 8 | kernelSize: 5 9 | MAX_REGISTRATION_POINTS: 2000 10 | BATCH_SIZE: 500 11 | MAX_ITERATION: 10 12 | LSnorm: Huber #l2, Huber 13 | huber_threshold: 50 14 | MIN_NUM_EVENTS: 1000 15 | RegProblemType: 1 #( 0 numerical; 1 analytical) 16 | SAVE_TRAJECTORY: False 17 | PATH_TO_SAVE_TRAJECTORY: "/home/zhouyi/Desktop/ESVO_result/hkust_lab/" # CHANGE THIS PATH 18 | VISUALIZE_TRAJECTORY: True -------------------------------------------------------------------------------- /esvo_core/cfg/tracking/tracking_rpg.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 #l2, Huber 13 | huber_threshold: 50 14 | MIN_NUM_EVENTS: 1000 15 | RegProblemType: 1 #( 0 numerical; 1 analytical) 16 | SAVE_TRAJECTORY: False 17 | PATH_TO_SAVE_TRAJECTORY: "/home/zhouyi/Desktop/ESVO_result/rpg_bin/" # CHANGE THIS PATH 18 | VISUALIZE_TRAJECTORY: True -------------------------------------------------------------------------------- /esvo_core/cfg/tracking/tracking_upenn.yaml: -------------------------------------------------------------------------------- 1 | invDepth_min_range: 0.16 2 | invDepth_max_range: 1.0 3 | TS_HISTORY_LENGTH: 100 4 | REF_HISTORY_LENGTH: 10 5 | tracking_rate_hz: 100 6 | patch_size_X: 1 7 | patch_size_Y: 1 8 | kernelSize: 5 9 | MAX_REGISTRATION_POINTS: 2000 10 | BATCH_SIZE: 300 11 | MAX_ITERATION: 10 12 | LSnorm: Huber #Huber #l2 13 | huber_threshold: 50 14 | MIN_NUM_EVENTS: 1000 15 | RegProblemType: 1 #( 0 numerical; 1 analytical) 16 | SAVE_TRAJECTORY: False 17 | PATH_TO_SAVE_TRAJECTORY: "/home/zhouyi/Desktop/ESVO_result/upenn_indoor_flying1/" # CHANGE THIS PATH 18 | VISUALIZE_TRAJECTORY: True -------------------------------------------------------------------------------- /esvo_core/esvo_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: /esvo_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 | -------------------------------------------------------------------------------- /esvo_core/esvo_system.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 | - /Path1 15 | Splitter Ratio: 0.501366138458252 16 | Tree Height: 755 17 | - Class: rviz/Selection 18 | Name: Selection 19 | - Class: rviz/Tool Properties 20 | Expanded: 21 | - /2D Pose Estimate1 22 | - /2D Nav Goal1 23 | - /Publish Point1 24 | Name: Tool Properties 25 | Splitter Ratio: 0.5886790156364441 26 | - Class: rviz/Views 27 | Expanded: 28 | - /Current View1 29 | Name: Views 30 | Splitter Ratio: 0.5 31 | - Class: rviz/Time 32 | Experimental: false 33 | Name: Time 34 | SyncMode: 0 35 | SyncSource: PointCloud2 36 | Preferences: 37 | PromptSaveOnExit: true 38 | Toolbars: 39 | toolButtonStyle: 2 40 | Visualization Manager: 41 | Class: "" 42 | Displays: 43 | - Alpha: 0.5 44 | Cell Size: 1 45 | Class: rviz/Grid 46 | Color: 160; 160; 164 47 | Enabled: false 48 | Line Style: 49 | Line Width: 0.029999999329447746 50 | Value: Lines 51 | Name: Grid 52 | Normal Cell Count: 0 53 | Offset: 54 | X: 0 55 | Y: 0 56 | Z: 0 57 | Plane: XY 58 | Plane Cell Count: 10 59 | Reference Frame: 60 | Value: false 61 | - Class: rviz/TF 62 | Enabled: true 63 | Frame Timeout: 999 64 | Frames: 65 | All Enabled: false 66 | dvs: 67 | Value: true 68 | map: 69 | Value: true 70 | Marker Scale: 1 71 | Name: TF 72 | Show Arrows: false 73 | Show Axes: true 74 | Show Names: true 75 | Tree: 76 | map: 77 | dvs: 78 | {} 79 | Update Interval: 0 80 | Value: true 81 | - Alpha: 1 82 | Axes Length: 0.15000000596046448 83 | Axes Radius: 0.019999999552965164 84 | Class: rviz/Pose 85 | Color: 255; 25; 0 86 | Enabled: false 87 | Head Length: 0.30000001192092896 88 | Head Radius: 0.10000000149011612 89 | Name: Pose 90 | Shaft Length: 1 91 | Shaft Radius: 0.05000000074505806 92 | Shape: Axes 93 | Topic: /esvo_core/pose_pub 94 | Unreliable: false 95 | Value: false 96 | - Class: rviz/Axes 97 | Enabled: false 98 | Length: 1 99 | Name: Axes 100 | Radius: 0.10000000149011612 101 | Reference Frame: 102 | Value: false 103 | - Alpha: 1 104 | Autocompute Intensity Bounds: true 105 | Autocompute Value Bounds: 106 | Max Value: 7.365790843963623 107 | Min Value: 1.6606168746948242 108 | Value: true 109 | Axis: X 110 | Channel Name: z 111 | Class: rviz/PointCloud2 112 | Color: 255; 255; 255 113 | Color Transformer: Intensity 114 | Decay Time: 0 115 | Enabled: true 116 | Invert Rainbow: false 117 | Max Color: 255; 255; 255 118 | Max Intensity: 4.929167747497559 119 | Min Color: 0; 0; 0 120 | Min Intensity: 0.756216287612915 121 | Name: PointCloud2 122 | Position Transformer: XYZ 123 | Queue Size: 10 124 | Selectable: true 125 | Size (Pixels): 3 126 | Size (m): 0.05000000074505806 127 | Style: Points 128 | Topic: /esvo_mapping/pointcloud_local 129 | Unreliable: false 130 | Use Fixed Frame: true 131 | Use rainbow: true 132 | Value: true 133 | - Alpha: 1 134 | Buffer Length: 1 135 | Class: rviz/Path 136 | Color: 46; 52; 54 137 | Enabled: true 138 | Head Diameter: 0.30000001192092896 139 | Head Length: 0.20000000298023224 140 | Length: 0.30000001192092896 141 | Line Style: Billboards 142 | Line Width: 0.009999999776482582 143 | Name: Path 144 | Offset: 145 | X: 0 146 | Y: 0 147 | Z: 0 148 | Pose Color: 255; 85; 255 149 | Pose Style: None 150 | Radius: 0.029999999329447746 151 | Shaft Diameter: 0.10000000149011612 152 | Shaft Length: 0.10000000149011612 153 | Topic: /esvo_tracking/trajectory 154 | Unreliable: false 155 | Value: true 156 | Enabled: true 157 | Global Options: 158 | Background Color: 186; 189; 182 159 | Default Light: true 160 | Fixed Frame: map 161 | Frame Rate: 30 162 | Name: root 163 | Tools: 164 | - Class: rviz/Interact 165 | Hide Inactive Objects: true 166 | - Class: rviz/MoveCamera 167 | - Class: rviz/Select 168 | - Class: rviz/FocusCamera 169 | - Class: rviz/Measure 170 | - Class: rviz/SetInitialPose 171 | Theta std deviation: 0.2617993950843811 172 | Topic: /initialpose 173 | X std deviation: 0.5 174 | Y std deviation: 0.5 175 | - Class: rviz/SetGoal 176 | Topic: /move_base_simple/goal 177 | - Class: rviz/PublishPoint 178 | Single click: true 179 | Topic: /clicked_point 180 | Value: true 181 | Views: 182 | Current: 183 | Class: rviz/Orbit 184 | Distance: 3.4973807334899902 185 | Enable Stereo Rendering: 186 | Stereo Eye Separation: 0.05999999865889549 187 | Stereo Focal Distance: 1 188 | Swap Stereo Eyes: false 189 | Value: false 190 | Focal Point: 191 | X: 0.059282515197992325 192 | Y: 0.27604925632476807 193 | Z: 0.8497702479362488 194 | Focal Shape Fixed Size: true 195 | Focal Shape Size: 0.05000000074505806 196 | Invert Z Axis: false 197 | Name: Current View 198 | Near Clip Distance: 0.009999999776482582 199 | Pitch: -1.5297963619232178 200 | Target Frame: 201 | Value: Orbit (rviz) 202 | Yaw: 4.707154750823975 203 | Saved: ~ 204 | Window Geometry: 205 | Displays: 206 | collapsed: true 207 | Height: 990 208 | Hide Left Dock: true 209 | Hide Right Dock: true 210 | QMainWindow State: 000000ff00000000fd0000000400000000000002d30000037efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000037e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000255fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000255000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000002ec0000003efc0100000002fb0000000800540069006d00650100000000000002ec000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002ec0000034000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 211 | Selection: 212 | collapsed: false 213 | Time: 214 | collapsed: false 215 | Tool Properties: 216 | collapsed: false 217 | Views: 218 | collapsed: true 219 | Width: 748 220 | X: 1142 221 | Y: 20 222 | -------------------------------------------------------------------------------- /esvo_core/esvo_system_DSEC.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 | - /Axes1 14 | - /PointCloud21 15 | - /PointCloud21/Autocompute Value Bounds1 16 | - /Path1 17 | Splitter Ratio: 0.501366138458252 18 | Tree Height: 357 19 | - Class: rviz/Selection 20 | Name: Selection 21 | - Class: rviz/Tool Properties 22 | Expanded: 23 | - /2D Pose Estimate1 24 | - /2D Nav Goal1 25 | - /Publish Point1 26 | Name: Tool Properties 27 | Splitter Ratio: 0.5886790156364441 28 | - Class: rviz/Views 29 | Expanded: 30 | - /Current View1 31 | - /ThirdPersonFollower1 32 | Name: Views 33 | Splitter Ratio: 0.5 34 | - Class: rviz/Time 35 | Experimental: false 36 | Name: Time 37 | SyncMode: 0 38 | SyncSource: "" 39 | Preferences: 40 | PromptSaveOnExit: true 41 | Toolbars: 42 | toolButtonStyle: 2 43 | Visualization Manager: 44 | Class: "" 45 | Displays: 46 | - Alpha: 0.5 47 | Cell Size: 1 48 | Class: rviz/Grid 49 | Color: 160; 160; 164 50 | Enabled: false 51 | Line Style: 52 | Line Width: 0.029999999329447746 53 | Value: Lines 54 | Name: Grid 55 | Normal Cell Count: 0 56 | Offset: 57 | X: 0 58 | Y: 0 59 | Z: 0 60 | Plane: XY 61 | Plane Cell Count: 10 62 | Reference Frame: 63 | Value: false 64 | - Class: rviz/TF 65 | Enabled: true 66 | Frame Timeout: 999 67 | Frames: 68 | All Enabled: false 69 | Marker Scale: 20 70 | Name: TF 71 | Show Arrows: false 72 | Show Axes: true 73 | Show Names: true 74 | Tree: 75 | {} 76 | Update Interval: 0 77 | Value: true 78 | - Alpha: 1 79 | Axes Length: 0.15000000596046448 80 | Axes Radius: 0.019999999552965164 81 | Class: rviz/Pose 82 | Color: 255; 25; 0 83 | Enabled: false 84 | Head Length: 0.30000001192092896 85 | Head Radius: 0.10000000149011612 86 | Name: Pose 87 | Shaft Length: 1 88 | Shaft Radius: 0.05000000074505806 89 | Shape: Axes 90 | Topic: /esvo_core/pose_pub 91 | Unreliable: false 92 | Value: false 93 | - Class: rviz/Axes 94 | Enabled: false 95 | Length: 1 96 | Name: Axes 97 | Radius: 0.10000000149011612 98 | Reference Frame: 99 | Value: false 100 | - Alpha: 1 101 | Autocompute Intensity Bounds: true 102 | Autocompute Value Bounds: 103 | Max Value: 262.61248779296875 104 | Min Value: -154.3746795654297 105 | Value: true 106 | Axis: Z 107 | Channel Name: x 108 | Class: rviz/PointCloud2 109 | Color: 85; 87; 83 110 | Color Transformer: FlatColor 111 | Decay Time: 0 112 | Enabled: true 113 | Invert Rainbow: false 114 | Max Color: 255; 255; 255 115 | Max Intensity: 727.3013305664062 116 | Min Color: 0; 0; 0 117 | Min Intensity: 32.05800247192383 118 | Name: PointCloud2 119 | Position Transformer: XYZ 120 | Queue Size: 10 121 | Selectable: false 122 | Size (Pixels): 3 123 | Size (m): 0.05000000074505806 124 | Style: Points 125 | Topic: /esvo_mapping/pointcloud_global 126 | Unreliable: false 127 | Use Fixed Frame: true 128 | Use rainbow: true 129 | Value: true 130 | - Alpha: 1 131 | Buffer Length: 1 132 | Class: rviz/Path 133 | Color: 225; 91; 195 134 | Enabled: true 135 | Head Diameter: 0.30000001192092896 136 | Head Length: 0.20000000298023224 137 | Length: 0.30000001192092896 138 | Line Style: Billboards 139 | Line Width: 0.5 140 | Name: Path 141 | Offset: 142 | X: 0 143 | Y: 0 144 | Z: 0 145 | Pose Color: 255; 85; 255 146 | Pose Style: None 147 | Radius: 0.029999999329447746 148 | Shaft Diameter: 0.10000000149011612 149 | Shaft Length: 0.10000000149011612 150 | Topic: /esvo_tracking/trajectory 151 | Unreliable: false 152 | Value: true 153 | Enabled: true 154 | Global Options: 155 | Background Color: 211; 215; 207 156 | Default Light: true 157 | Fixed Frame: map 158 | Frame Rate: 30 159 | Name: root 160 | Tools: 161 | - Class: rviz/Interact 162 | Hide Inactive Objects: true 163 | - Class: rviz/MoveCamera 164 | - Class: rviz/Select 165 | - Class: rviz/FocusCamera 166 | - Class: rviz/Measure 167 | - Class: rviz/SetInitialPose 168 | Theta std deviation: 0.2617993950843811 169 | Topic: /initialpose 170 | X std deviation: 0.5 171 | Y std deviation: 0.5 172 | - Class: rviz/SetGoal 173 | Topic: /move_base_simple/goal 174 | - Class: rviz/PublishPoint 175 | Single click: true 176 | Topic: /clicked_point 177 | Value: true 178 | Views: 179 | Current: 180 | Class: rviz/FrameAligned 181 | Enable Stereo Rendering: 182 | Stereo Eye Separation: 0.05999999865889549 183 | Stereo Focal Distance: 1 184 | Swap Stereo Eyes: false 185 | Value: false 186 | Invert Z Axis: false 187 | Lock Camera: false 188 | Name: Current View 189 | Near Clip Distance: 0.009999999776482582 190 | Pitch: -1.691992163658142 191 | Point towards: arbitrary 192 | Position: 193 | X: 0.4810287356376648 194 | Y: -6.820411205291748 195 | Z: -30.676294326782227 196 | Roll: 3.141587734222412 197 | Target Frame: dvs 198 | Value: FrameAligned (rviz) 199 | Yaw: -1.5449943542480469 200 | Saved: 201 | - Class: rviz/ThirdPersonFollower 202 | Distance: 38.74925994873047 203 | Enable Stereo Rendering: 204 | Stereo Eye Separation: 0.05999999865889549 205 | Stereo Focal Distance: 1 206 | Swap Stereo Eyes: false 207 | Value: false 208 | Focal Point: 209 | X: 14.926668167114258 210 | Y: -5.084822654724121 211 | Z: 1.7814527382142842e-5 212 | Focal Shape Fixed Size: true 213 | Focal Shape Size: 0.05000000074505806 214 | Invert Z Axis: false 215 | Name: ThirdPersonFollower 216 | Near Clip Distance: 0.009999999776482582 217 | Pitch: -1.0247968435287476 218 | Target Frame: 219 | Value: ThirdPersonFollower (rviz) 220 | Yaw: 4.613580226898193 221 | - Class: rviz/FrameAligned 222 | Enable Stereo Rendering: 223 | Stereo Eye Separation: 0.05999999865889549 224 | Stereo Focal Distance: 1 225 | Swap Stereo Eyes: false 226 | Value: false 227 | Invert Z Axis: false 228 | Lock Camera: false 229 | Name: FrameAligned 230 | Near Clip Distance: 0.009999999776482582 231 | Pitch: -1.9769917726516724 232 | Point towards: arbitrary 233 | Position: 234 | X: 0.08751553297042847 235 | Y: -12.823699951171875 236 | Z: -42.410736083984375 237 | Roll: 3.141587734222412 238 | Target Frame: dvs 239 | Value: FrameAligned (rviz) 240 | Yaw: -1.5599943399429321 241 | - Class: rviz/FrameAligned 242 | Enable Stereo Rendering: 243 | Stereo Eye Separation: 0.05999999865889549 244 | Stereo Focal Distance: 1 245 | Swap Stereo Eyes: false 246 | Value: false 247 | Invert Z Axis: false 248 | Lock Camera: false 249 | Name: FrameAligned 250 | Near Clip Distance: 0.009999999776482582 251 | Pitch: -1.691992163658142 252 | Point towards: arbitrary 253 | Position: 254 | X: -0.7189003229141235 255 | Y: -11.10289478302002 256 | Z: -42.24729919433594 257 | Roll: 3.141587734222412 258 | Target Frame: dvs 259 | Value: FrameAligned (rviz) 260 | Yaw: -1.5449943542480469 261 | Window Geometry: 262 | Displays: 263 | collapsed: true 264 | Height: 1017 265 | Hide Left Dock: true 266 | Hide Right Dock: true 267 | QMainWindow State: 000000ff00000000fd0000000400000000000002d300000351fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000004300000351000000d400fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001bb00000349fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004300000349000000b500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004e300000044fc0100000002fb0000000800540069006d00650100000000000004e3000003b700fffffffb0000000800540069006d00650100000000000004500000000000000000000004e30000034900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 268 | Selection: 269 | collapsed: false 270 | Time: 271 | collapsed: false 272 | Tool Properties: 273 | collapsed: false 274 | Views: 275 | collapsed: true 276 | Width: 1251 277 | X: 996 278 | Y: 27 279 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/container/CameraSystem.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CONTAINER_CAMERASYSTEM_H 2 | #define ESVO_CORE_CONTAINER_CAMERASYSTEM_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace esvo_core 11 | { 12 | namespace container 13 | { 14 | class PerspectiveCamera 15 | { 16 | public: 17 | PerspectiveCamera(); 18 | virtual ~PerspectiveCamera(); 19 | using Ptr = std::shared_ptr; 20 | 21 | void setIntrinsicParameters( 22 | size_t width, size_t height, 23 | std::string& cameraName, 24 | std::string& distortion_model, 25 | std::vector& vD, 26 | std::vector& vK, 27 | std::vector& vRectMat, 28 | std::vector& vP); 29 | 30 | void preComputeRectifiedCoordinate(); 31 | 32 | Eigen::Matrix getRectifiedUndistortedCoordinate(int xcoor, int ycoor); 33 | 34 | void cam2World(const Eigen::Vector2d &x, double invDepth, Eigen::Vector3d &p); 35 | 36 | void world2Cam(const Eigen::Vector3d &p, Eigen::Vector2d &x); 37 | 38 | public: 39 | size_t width_, height_; 40 | std::string cameraName_; 41 | std::string distortion_model_; 42 | Eigen::Matrix D_; 43 | Eigen::Matrix3d K_; 44 | Eigen::Matrix3d RectMat_; 45 | Eigen::Matrix P_; 46 | 47 | Eigen::Matrix2Xd precomputed_rectified_points_; 48 | cv::Mat undistort_map1_, undistort_map2_; 49 | Eigen::MatrixXi UndistortRectify_mask_; 50 | }; 51 | 52 | class CameraSystem 53 | { 54 | public: 55 | CameraSystem(const std::string& calibInfoDir, bool bPrintCalibInfo = false); 56 | virtual ~CameraSystem(); 57 | using Ptr = std::shared_ptr; 58 | 59 | void computeBaseline(); 60 | void loadCalibInfo(const std::string & cameraSystemDir, bool bPrintCalibInfo = false); 61 | void printCalibInfo(); 62 | 63 | PerspectiveCamera::Ptr cam_left_ptr_, cam_right_ptr_; // intrinsics 64 | Eigen::Matrix T_right_left_;// extrinsics 65 | double baseline_; 66 | }; 67 | } 68 | } 69 | 70 | #endif //ESVO_CORE_CONTAINER_CAMERASYSTEM_H -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/container/DepthMap.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_DEPTHMAP_H 2 | #define ESVO_CORE_DEPTHMAP_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace esvo_core 9 | { 10 | using namespace tools; 11 | namespace container 12 | { 13 | using DepthMap = SmartGrid; 14 | 15 | struct DepthFrame 16 | { 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | typedef std::shared_ptr Ptr; 19 | 20 | DepthFrame(size_t row, size_t col) 21 | { 22 | dMap_ = std::make_shared(row, col); 23 | id_ = 0; 24 | T_world_frame_.setIdentity(); 25 | } 26 | 27 | void setId(size_t id) 28 | { 29 | id_ = id; 30 | } 31 | 32 | void setTransformation(Transformation &T_world_frame) 33 | { 34 | T_world_frame_ = T_world_frame; 35 | } 36 | 37 | void clear() 38 | { 39 | dMap_->reset(); 40 | id_ = 0; 41 | T_world_frame_.setIdentity(); 42 | } 43 | 44 | DepthMap::Ptr dMap_; 45 | size_t id_; 46 | Transformation T_world_frame_; 47 | }; 48 | } 49 | } 50 | #endif //ESVO_CORE_DEPTHMAP_H 51 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/container/DepthPoint.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CONTAINER_DEPTHPOINT_H 2 | #define ESVO_CORE_CONTAINER_DEPTHPOINT_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace esvo_core 9 | { 10 | namespace container 11 | { 12 | class DepthPoint 13 | { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | typedef std::shared_ptr Ptr; 17 | 18 | DepthPoint(); 19 | 20 | DepthPoint(size_t row, size_t col); 21 | virtual ~DepthPoint(); 22 | 23 | size_t row() const; 24 | size_t col() const; 25 | 26 | void update_x(const Eigen::Vector2d &x); 27 | const Eigen::Vector2d &x() const; 28 | 29 | double &invDepth(); 30 | const double &invDepth() const; 31 | 32 | double &scaleSquared(); 33 | const double &scaleSquared() const; 34 | 35 | double &nu(); 36 | const double &nu() const; 37 | 38 | double &variance(); 39 | const double &variance() const; 40 | 41 | double &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 //ESVO_CORE_CONTAINER_DEPTHPOINT_H -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/container/EventMatchPair.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CORE_EVENTMATCHPAIR_H 2 | #define ESVO_CORE_CORE_EVENTMATCHPAIR_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace esvo_core 11 | { 12 | using namespace container; 13 | using namespace tools; 14 | namespace core 15 | { 16 | struct EventMatchPair 17 | { 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | 20 | EventMatchPair() {} 21 | 22 | // raw event coordinate 23 | Eigen::Vector2d x_left_raw_; 24 | // rectified_event coordinate (left, right) 25 | Eigen::Vector2d x_left_, x_right_; 26 | // timestamp 27 | ros::Time t_; 28 | // pose of virtual view (T_world_virtual) 29 | Transformation trans_; 30 | // inverse depth 31 | double invDepth_; 32 | // match cost 33 | double cost_; 34 | // gradient (left) 35 | double gx_, gy_; 36 | // disparity 37 | double disp_; 38 | }; 39 | } 40 | } 41 | 42 | #endif //ESVO_CORE_CORE_EVENTMATCHPAIR_H 43 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/container/ResidualItem.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CONTAINER_RESIDUALITEM_H 2 | #define ESVO_CORE_CONTAINER_RESIDUALITEM_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace esvo_core 9 | { 10 | namespace container 11 | { 12 | struct ResidualItem 13 | { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | Eigen::Vector3d p_;// 3D coordinate in the reference frame 17 | Eigen::Vector2d p_img_;// 2D coordinate in the image plane 18 | 19 | // double IRLS_weight_; 20 | // double variance_; 21 | Eigen::VectorXd residual_; 22 | // bool bOutlier_; 23 | 24 | ResidualItem(); 25 | ResidualItem(const double x,const double y,const double z); 26 | void initialize(const double x,const double y,const double z); 27 | }; 28 | 29 | using ResidualItems = std::vector >; 30 | }// namespace container 31 | }// namespace esvo_core 32 | 33 | #endif //ESVO_CORE_CONTAINER_RESIDUALITEM_H 34 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/container/TimeSurfaceObservation.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CONTAINER_TIMESURFACEOBSERVATION_H 2 | #define ESVO_CORE_CONTAINER_TIMESURFACEOBSERVATION_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | //#define TIME_SURFACE_OBSERVATION_LOG 22 | namespace esvo_core 23 | { 24 | using namespace tools; 25 | namespace container 26 | { 27 | struct TimeSurfaceObservation 28 | { 29 | TimeSurfaceObservation( 30 | cv_bridge::CvImagePtr &left, 31 | cv_bridge::CvImagePtr &right, 32 | Transformation &tr, 33 | size_t id, 34 | bool bCalcTsGradient = false) 35 | : tr_(tr), 36 | id_(id) 37 | { 38 | cv::cv2eigen(left->image, TS_left_); 39 | cv::cv2eigen(right->image, TS_right_); 40 | 41 | if (bCalcTsGradient) 42 | { 43 | #ifdef TIME_SURFACE_OBSERVATION_LOG 44 | TicToc tt; 45 | tt.tic(); 46 | #endif 47 | cv::Mat cv_dTS_du_left, cv_dTS_dv_left; 48 | cv::Sobel(left->image, cv_dTS_du_left, CV_64F, 1, 0); 49 | cv::Sobel(left->image, cv_dTS_dv_left, CV_64F, 0, 1); 50 | cv::cv2eigen(cv_dTS_du_left, dTS_du_left_); 51 | cv::cv2eigen(cv_dTS_dv_left, dTS_dv_left_); 52 | #ifdef TIME_SURFACE_OBSERVATION_LOG 53 | LOG(INFO) << "@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@Sobel computation (" << id_ << ") takes " << tt.toc() << " ms."; 54 | #endif 55 | } 56 | } 57 | 58 | // override version without initializing the transformation in the constructor. 59 | TimeSurfaceObservation( 60 | cv_bridge::CvImagePtr &left, 61 | cv_bridge::CvImagePtr &right, 62 | size_t id, 63 | bool bCalcTsGradient = false) 64 | : id_(id) 65 | { 66 | cvImagePtr_left_ = left; 67 | cvImagePtr_right_ = right; 68 | cv::cv2eigen(left->image, TS_left_); 69 | cv::cv2eigen(right->image, TS_right_); 70 | 71 | if (bCalcTsGradient) 72 | { 73 | #ifdef TIME_SURFACE_OBSERVATION_LOG 74 | TicToc tt; 75 | tt.tic(); 76 | #endif 77 | cv::Mat cv_dTS_du_left, cv_dTS_dv_left; 78 | cv::Mat cv_dTS_du_right, cv_dTS_dv_right; 79 | cv::Sobel(left->image, cv_dTS_du_left, CV_64F, 1, 0); 80 | cv::Sobel(left->image, cv_dTS_dv_left, CV_64F, 0, 1); 81 | 82 | cv::cv2eigen(cv_dTS_du_left, dTS_du_left_); 83 | cv::cv2eigen(cv_dTS_dv_left, dTS_dv_left_); 84 | 85 | #ifdef TIME_SURFACE_OBSERVATION_LOG 86 | LOG(INFO) << "@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@Sobel computation (" << id_ << ") takes " << tt.toc() << " ms."; 87 | #endif 88 | } 89 | } 90 | 91 | TimeSurfaceObservation() 92 | {}; 93 | 94 | inline bool isEmpty() 95 | { 96 | if(TS_left_.rows() == 0 || TS_left_.cols() == 0 || TS_right_.rows() == 0 || TS_right_.cols() == 0) 97 | return true; 98 | else 99 | return false; 100 | } 101 | 102 | inline void setTransformation(Transformation &tr) 103 | { 104 | tr_ = tr; 105 | } 106 | 107 | inline void GaussianBlurTS(size_t kernelSize) 108 | { 109 | cv::Mat mat_left_, mat_right_; 110 | cv::GaussianBlur(cvImagePtr_left_->image, mat_left_, 111 | cv::Size(kernelSize, kernelSize), 0.0); 112 | cv::GaussianBlur(cvImagePtr_right_->image, mat_right_, 113 | cv::Size(kernelSize, kernelSize), 0.0); 114 | cv::cv2eigen(mat_left_, TS_left_); 115 | cv::cv2eigen(mat_right_, TS_right_); 116 | } 117 | 118 | inline void getTimeSurfaceNegative(size_t kernelSize) 119 | { 120 | Eigen::MatrixXd ceilMat(TS_left_.rows(), TS_left_.cols()); 121 | ceilMat.setConstant(255.0); 122 | if (kernelSize > 0) 123 | { 124 | cv::Mat mat_left_; 125 | cv::GaussianBlur(cvImagePtr_left_->image, mat_left_, 126 | cv::Size(kernelSize, kernelSize), 0.0); 127 | cv::cv2eigen(mat_left_, TS_blurred_left_); 128 | TS_negative_left_ = ceilMat - TS_blurred_left_; 129 | } 130 | else 131 | { 132 | TS_negative_left_ = ceilMat - TS_left_; 133 | } 134 | } 135 | 136 | inline void computeTsNegativeGrad() 137 | { 138 | cv::Mat cv_TS_flipped_left; 139 | cv::eigen2cv(TS_negative_left_, cv_TS_flipped_left); 140 | 141 | cv::Mat cv_dFlippedTS_du_left, cv_dFlippedTS_dv_left; 142 | cv::Sobel(cv_TS_flipped_left, cv_dFlippedTS_du_left, CV_64F, 1, 0); 143 | cv::Sobel(cv_TS_flipped_left, cv_dFlippedTS_dv_left, CV_64F, 0, 1); 144 | 145 | cv::cv2eigen(cv_dFlippedTS_du_left, dTS_negative_du_left_); 146 | cv::cv2eigen(cv_dFlippedTS_dv_left, dTS_negative_dv_left_); 147 | } 148 | 149 | Eigen::MatrixXd TS_left_, TS_right_; 150 | Eigen::MatrixXd TS_blurred_left_; 151 | Eigen::MatrixXd TS_negative_left_; 152 | cv_bridge::CvImagePtr cvImagePtr_left_, cvImagePtr_right_; 153 | Transformation tr_; 154 | Eigen::MatrixXd dTS_du_left_, dTS_dv_left_; 155 | Eigen::MatrixXd dTS_negative_du_left_, dTS_negative_dv_left_; 156 | size_t id_; 157 | }; 158 | 159 | struct ROSTimeCmp 160 | { 161 | bool operator()(const ros::Time &a, const ros::Time &b) const 162 | { 163 | return a.toNSec() < b.toNSec(); 164 | } 165 | }; 166 | 167 | using TimeSurfaceHistory = std::map; 168 | using StampedTimeSurfaceObs = std::pair; 169 | 170 | inline static TimeSurfaceHistory::iterator TSHistory_lower_bound(TimeSurfaceHistory &ts_history, ros::Time &t) 171 | { 172 | return std::lower_bound(ts_history.begin(), ts_history.end(), t, 173 | [](const std::pair &tso, const ros::Time &t) { 174 | return tso.first.toSec() < t.toSec(); 175 | }); 176 | } 177 | 178 | inline static TimeSurfaceHistory::iterator TSHistory_upper_bound(TimeSurfaceHistory &ts_history, ros::Time &t) 179 | { 180 | return std::upper_bound(ts_history.begin(), ts_history.end(), t, 181 | [](const ros::Time &t, const std::pair &tso) { 182 | return t.toSec() < tso.first.toSec(); 183 | }); 184 | } 185 | } 186 | } 187 | 188 | #endif //ESVO_CORE_CONTAINER_TIMESURFACEOBSERVATION_H -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/core/DepthFusion.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CORE_DEPTHFUSION_H 2 | #define ESVO_CORE_CORE_DEPTHFUSION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace esvo_core 12 | { 13 | using namespace container; 14 | namespace core 15 | { 16 | class DepthFusion 17 | { 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | 21 | DepthFusion( 22 | CameraSystem::Ptr &camSysPtr, 23 | std::shared_ptr & dpConfigPtr); 24 | 25 | virtual ~DepthFusion(); 26 | 27 | bool propagate_one_point( 28 | DepthPoint &dp_prior, 29 | DepthPoint &dp_prop, 30 | Eigen::Matrix &T_prop_prior); 31 | 32 | int fusion( 33 | DepthPoint &dp_prop, 34 | DepthMap::Ptr &dm, 35 | int fusion_radius = 0); 36 | 37 | int update( 38 | std::vector &dp_obs, 39 | DepthFrame::Ptr &df, 40 | int fusion_radius = 0); 41 | 42 | bool boundaryCheck( 43 | double xcoor, 44 | double ycoor, 45 | size_t width, 46 | size_t height); 47 | 48 | bool chiSquareTest( 49 | double invD1, double invD2, 50 | double var1, double var2); 51 | 52 | bool studentTCompatibleTest( 53 | double invD1, double invD2, 54 | double var1, double var2); 55 | 56 | // Used by GTL and SGM for comparison, and also used in the initialization. 57 | void naive_propagation( 58 | std::vector &dp_obs, 59 | DepthFrame::Ptr &df); 60 | 61 | bool naive_propagate_one_point( 62 | DepthPoint &dp_prior, 63 | DepthPoint &dp_prop, 64 | Eigen::Matrix &T_prop_prior); 65 | 66 | private: 67 | CameraSystem::Ptr camSysPtr_; 68 | std::shared_ptr dpConfigPtr_; 69 | }; 70 | } 71 | } 72 | 73 | #endif //ESVO_CORE_CORE_DEPTHFUSION_H -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/core/DepthProblem.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CORE_DEPTHPROBLEM_H 2 | #define ESVO_CORE_CORE_DEPTHPROBLEM_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace esvo_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 optimization::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( 61 | Eigen::Vector2d & coor, 62 | Eigen::Matrix & T_world_virtual, 63 | StampedTimeSurfaceObs* pStampedTsObs); 64 | 65 | // function that is inherited from optimization::OptimizationFunctor 66 | int operator()( const Eigen::VectorXd &x, Eigen::VectorXd & fvec ) const; 67 | 68 | // utils 69 | bool warping( 70 | const Eigen::Vector2d &x, 71 | double d, 72 | const Eigen::Matrix &T_left_virtual, 73 | Eigen::Vector2d &x1_s, 74 | Eigen::Vector2d &x2_s) const; 75 | 76 | bool patchInterpolation( 77 | const Eigen::MatrixXd &img, 78 | const Eigen::Vector2d &location, 79 | Eigen::MatrixXd &patch, 80 | bool debug = false) const; 81 | 82 | // variables 83 | CameraSystem::Ptr camSysPtr_; 84 | DepthProblemConfig::Ptr dpConfigPtr_; 85 | Eigen::Vector2d coordinate_; 86 | Eigen::Matrix T_world_virtual_; 87 | std::vector, 88 | Eigen::aligned_allocator > > vT_left_virtual_; 89 | StampedTimeSurfaceObs* pStampedTsObs_; 90 | }; 91 | } 92 | } 93 | 94 | 95 | #endif //ESVO_CORE_CORE_DEPTHPROBLEM_H 96 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/core/DepthProblemSolver.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CORE_DEPTHPROBLEMSOLVER2_H 2 | #define ESVO_CORE_CORE_DEPTHPROBLEMSOLVER2_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace esvo_core 12 | { 13 | namespace core 14 | { 15 | enum DepthProblemType 16 | { 17 | ANALYTICAL, 18 | NUMERICAL 19 | }; 20 | class DepthProblemSolver 21 | { 22 | struct Job 23 | { 24 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 25 | size_t i_thread_; 26 | std::vector *pvEMP_; 27 | StampedTimeSurfaceObs* pStamped_TS_obs_; 28 | std::shared_ptr dProblemPtr_; 29 | std::shared_ptr< Eigen::NumericalDiff > numDiff_dProblemPtr_; 30 | std::shared_ptr< std::vector > vdpPtr_; 31 | }; 32 | 33 | public: 34 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 35 | DepthProblemSolver( 36 | CameraSystem::Ptr & camSysPtr, 37 | std::shared_ptr & dpConfigPtr, 38 | DepthProblemType dpType = NUMERICAL, // we only provide the numerical (jacobian) solver here, because the analytical one will not show remarkable efficiency. 39 | size_t numThread = 1); 40 | 41 | virtual ~DepthProblemSolver(); 42 | 43 | void solve( 44 | std::vector* pvEMP, 45 | StampedTimeSurfaceObs* pStampedTsObs, 46 | std::vector &vdp ); 47 | 48 | void solve_multiple_problems(Job &job); 49 | 50 | bool solve_single_problem_numerical( 51 | double d_init, 52 | std::shared_ptr< Eigen::NumericalDiff > & dProblemPtr, 53 | double* result); 54 | 55 | void pointCulling( 56 | std::vector &vdp, 57 | double std_variance_threshold, 58 | double cost_threshold, 59 | double invDepth_min_range, 60 | double invDepth_max_range); 61 | 62 | DepthProblemType getProblemType(); 63 | 64 | private: 65 | CameraSystem::Ptr & camSysPtr_; 66 | std::shared_ptr dpConfigPtr_; 67 | size_t NUM_THREAD_; 68 | DepthProblemType dpType_; 69 | }; 70 | } 71 | } 72 | #endif //ESVO_CORE_CORE_DEPTHPROBLEMSOLVER2_H -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/core/DepthRegularization.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CORE_DEPTHREGULARIZATION_H 2 | #define ESVO_CORE_CORE_DEPTHREGULARIZATION_H 3 | 4 | #include 5 | #include 6 | #include 7 | namespace esvo_core 8 | { 9 | using namespace container; 10 | namespace core 11 | { 12 | class DepthRegularization 13 | { 14 | public: 15 | typedef std::shared_ptr Ptr; 16 | 17 | DepthRegularization(std::shared_ptr & dpConfigPtr); 18 | virtual ~DepthRegularization(); 19 | 20 | void apply( DepthMap::Ptr & depthMapPtr ); 21 | 22 | private: 23 | std::shared_ptr dpConfigPtr_; 24 | size_t _regularizationRadius; 25 | size_t _regularizationMinNeighbours; 26 | size_t _regularizationMinCloseNeighbours; 27 | }; 28 | }// core 29 | }// esvo_core 30 | 31 | #endif //ESVO_CORE_CORE_DEPTHREGULARIZATION_H -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/core/EventBM.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CORE_EVENTBM_H 2 | #define ESVO_CORE_CORE_EVENTBM_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace esvo_core 11 | { 12 | using namespace tools; 13 | using namespace container; 14 | namespace core 15 | { 16 | class EventBM 17 | { 18 | struct Job 19 | { 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 21 | size_t i_thread_; 22 | std::vector* pvEventPtr_; 23 | std::vector >* pvpDisparitySearchBound_; 24 | std::shared_ptr > pvEventMatchPair_; 25 | }; 26 | public: 27 | EventBM( 28 | CameraSystem::Ptr camSysPtr, 29 | size_t numThread = 1, 30 | bool bSmoothTS = false, 31 | size_t patch_size_X = 25, 32 | size_t patch_size_Y = 25, 33 | size_t min_disparity = 1, 34 | size_t max_disparity = 40, 35 | size_t step = 1, 36 | double ZNCC_Threshold = 0.1, 37 | bool bUpDownConfiguration = false);// bUpDownConfiguration determines the epipolar searching direction (UpDown or LeftRight). 38 | virtual ~EventBM(); 39 | 40 | void resetParameters( 41 | size_t patch_size_X, 42 | size_t patch_size_Y, 43 | size_t min_disparity, 44 | size_t max_disparity, 45 | size_t step, 46 | double ZNCC_Threshold, 47 | bool bDonwUpConfiguration); 48 | 49 | void createMatchProblem( 50 | StampedTimeSurfaceObs * pStampedTsObs, 51 | StampTransformationMap * pSt_map, 52 | std::vector* pvEventsPtr); 53 | 54 | bool match_an_event( 55 | dvs_msgs::Event *pEvent, 56 | std::pair& pDisparityBound, 57 | EventMatchPair &emPair); 58 | 59 | void match_all_SingleThread(std::vector &vEMP); 60 | void match_all_HyperThread(std::vector &vEMP); 61 | 62 | static double zncc_cost(Eigen::MatrixXd &patch_left, Eigen::MatrixXd &patch_right, bool normalized = false); 63 | 64 | private: 65 | void match(EventBM::Job &job); 66 | bool epipolarSearching(double& min_cost, Eigen::Vector2i& bestMatch, size_t& bestDisp, Eigen::MatrixXd& patch_dst, 67 | size_t searching_start_pos, size_t searching_end_pos, size_t searching_step, 68 | Eigen::Vector2i& x1, Eigen::MatrixXd& patch_src, bool bDownUpConfiguration = false); 69 | bool isValidPatch(Eigen::Vector2i& x, Eigen::Vector2i& left_top); 70 | private: 71 | CameraSystem::Ptr camSysPtr_; 72 | StampedTimeSurfaceObs* pStampedTsObs_; 73 | StampTransformationMap * pSt_map_; 74 | std::vector vEventsPtr_; 75 | std::vector > vpDisparitySearchBound_; 76 | Sobel sb_; 77 | 78 | size_t NUM_THREAD_; 79 | bool bSmoothTS_; 80 | size_t patch_size_X_; 81 | size_t patch_size_Y_; 82 | size_t min_disparity_; 83 | size_t max_disparity_; 84 | size_t step_; 85 | double ZNCC_Threshold_; 86 | double ZNCC_MAX_; 87 | bool bUpDownConfiguration_; 88 | 89 | size_t coarseSearchingFailNum_, fineSearchingFailNum_, infoNoiseRatioLowNum_; 90 | }; 91 | }// core 92 | }// esvo_core 93 | 94 | #endif //ESVO_CORE_CORE_EVENTBM_H 95 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/core/EventMatcher.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_EVENTMATCHER_H 2 | #define ESVO_CORE_EVENTMATCHER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace esvo_core 11 | { 12 | using namespace container; 13 | using namespace tools; 14 | namespace core 15 | { 16 | struct EventSlice 17 | { 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | EventSlice(double SLICE_THICKNESS = 2 * 1e-3) 20 | :SLICE_THICKNESS_(SLICE_THICKNESS){} 21 | 22 | size_t numEvents_; 23 | double SLICE_THICKNESS_; 24 | ros::Time t_median_;// used for Transformation iterpolation 25 | tools::Transformation transf_; 26 | std::vector::iterator it_begin_, it_end_; 27 | }; 28 | 29 | class EventMatcher 30 | { 31 | struct Job 32 | { 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 34 | size_t i_thread_; 35 | std::vector::iterator vEventPtr_it_begin_; 36 | std::vector* pvEventSlice_; 37 | std::vector* pvIndexEventSlice_; 38 | std::shared_ptr< std::vector > pvEventMatchPair_; 39 | }; 40 | public: 41 | EventMatcher( 42 | CameraSystem::Ptr camSysPtr, 43 | size_t numThread = 1, 44 | double Time_THRESHOLD = 10e-5,// 100us 45 | double EPIPOLAR_THRESHOLD = 0.5, 46 | double TS_NCC_THRESHOLD = 0.1, 47 | size_t patch_size_X = 25, 48 | size_t patch_size_Y = 5, 49 | size_t patch_intensity_threshold = 125, 50 | double patch_valid_ratio = 0.1); 51 | virtual ~EventMatcher(); 52 | 53 | void resetParameters( 54 | double Time_THRESHOLD, 55 | double EPIPOLAR_THRESHOLD, 56 | double TS_NCC_THRESHOLD, 57 | size_t patch_size_X, 58 | size_t patch_size_Y, 59 | size_t patch_intensity_threshold, 60 | double patch_valid_ratio); 61 | 62 | void createMatchProblem( 63 | StampedTimeSurfaceObs * pTS_obs, 64 | std::vector* vEventSlice_ptr, 65 | std::vector* vEventPtr_cand); 66 | 67 | bool match_an_event( 68 | dvs_msgs::Event* ev_ptr, 69 | Transformation &Trans_world_rv, 70 | EventMatchPair &emPair); 71 | 72 | void match_all_SingleThread(std::vector &vEMP); 73 | void match_all_HyperThread(std::vector &vEMP); 74 | void match(EventMatcher::Job& job); 75 | 76 | double zncc_cost(Eigen::MatrixXd &patch_left, Eigen::MatrixXd &patch_right); 77 | 78 | bool warping2( 79 | const Eigen::Vector2d &x, 80 | double invDepth, 81 | const Eigen::Matrix &T_left_rv, 82 | Eigen::Vector2d &x1_s, 83 | Eigen::Vector2d &x2_s); 84 | 85 | bool patchInterpolation2( 86 | const Eigen::MatrixXd &img, 87 | const Eigen::Vector2d &location, 88 | Eigen::MatrixXd &patch); 89 | private: 90 | container::CameraSystem::Ptr camSysPtr_; 91 | std::vector vEvents_left_itr_, vEvents_right_itr_; 92 | size_t NUM_THREAD_; 93 | double Time_THRESHOLD_, EPIPOLAR_THRESHOLD_, TS_NCC_THRESHOLD_; 94 | size_t patch_size_X_, patch_size_Y_; 95 | size_t patch_intensity_threshold_; 96 | double patch_valid_ratio_; 97 | 98 | StampedTimeSurfaceObs * pTS_obs_; 99 | std::vector* pvEventSlice_; 100 | std::vector* pvCandEventPtr_; 101 | }; 102 | } 103 | } 104 | #endif //ESVO_CORE_EVENTMATCHER_H 105 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/core/RegProblemLM.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CORE_REGPROBLEMLM_H 2 | #define ESVO_CORE_CORE_REGPROBLEMLM_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | namespace esvo_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 optimization::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 | TimeSurfaceObservation* pTsObs_; 137 | RefFrame* ref_; 138 | CurFrame* cur_; 139 | 140 | Eigen::Matrix T_world_left_;// to record the current pose 141 | Eigen::Matrix T_world_ref_;// to record the ref pose (local ref map) 142 | Eigen::Matrix3d R_;//R_ref_cur_; 143 | Eigen::Vector3d t_;//t_ref_cur 144 | 145 | // Jacobian Constant 146 | Eigen::Matrix J_G_0_; 147 | // debug 148 | bool bPrint_; 149 | };// struct RegProblemLM 150 | }// namespace core 151 | }// namespace esvo_core 152 | 153 | #endif //ESVO_CORE_CORE_REGPROBLEMLM_H -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/core/RegProblemSolverLM.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_CORE_REGPROBLEMSOLVERLM_H 2 | #define ESVO_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 esvo_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 solve_numerical();// relatively slower 46 | bool solve_analytical();// faster 47 | 48 | // For test and visualization 49 | void setRegPublisher(image_transport::Publisher* reprojMap_pub); 50 | LM_statics lmStatics_;// record LevenburgMarquardt log. 51 | 52 | // variables 53 | private: 54 | CameraSystem::Ptr& camSysPtr_; 55 | std::shared_ptr rpConfigPtr_; 56 | size_t NUM_THREAD_; 57 | RegProblemType rpType_; 58 | 59 | std::shared_ptr regProblemPtr_; 60 | std::shared_ptr > numDiff_regProblemPtr_; 61 | 62 | // For test 63 | double z_min_, z_max_; 64 | image_transport::Publisher *reprojMap_pub_; 65 | Visualization visualizor_; 66 | bool bPrint_, bVisualize_; 67 | }; 68 | }//namespace core 69 | }//namespace esvo_core 70 | #endif //ESVO_CORE_CORE_REGPROBLEMSOLVER2_H -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/esvo_MVStereo.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_ESVO_MVSTEREO_H 2 | #define ESVO_CORE_ESVO_MVSTEREO_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | namespace esvo_core 41 | { 42 | using namespace core; 43 | enum eMVStereoMode 44 | { 45 | PURE_EVENT_MATCHING, //0 (this one implements GTS [26]) 46 | PURE_BLOCK_MATCHING, //1 (this one implements BM) 47 | EM_PLUS_ESTIMATION, //2 (GTS [26] + nonliear opt.) 48 | BM_PLUS_ESTIMATION, //3 (this one is ESVO's mapping method, namely BM + nonliear opt.) 49 | PURE_SEMI_GLOBAL_MATCHING //4 (this one implements SGM [45]) 50 | }; 51 | class esvo_MVStereo 52 | { 53 | public: 54 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 55 | esvo_MVStereo( 56 | const ros::NodeHandle& nh, 57 | const ros::NodeHandle& nh_private); 58 | virtual ~esvo_MVStereo(); 59 | 60 | // functions regarding mapping 61 | void MappingLoop(std::promise prom_mapping, std::future future_reset); 62 | void MappingAtTime(const ros::Time& t); 63 | bool dataTransferring(); 64 | void vEMP2vDP(std::vector& vEMP,std::vector& vdp); 65 | void eventSlicingForEM(std::vector& eventSlices); 66 | 67 | // callback functions 68 | void stampedPoseCallback(const geometry_msgs::PoseStampedConstPtr &ps_msg); 69 | void eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg, EventQueue& EQ); 70 | void timeSurfaceCallback( 71 | const sensor_msgs::ImageConstPtr& time_surface_left, 72 | const sensor_msgs::ImageConstPtr& time_surface_right); 73 | void onlineParameterChangeCallback(DVS_MappingStereoConfig &config, uint32_t level); 74 | 75 | // utils 76 | bool getPoseAt(const ros::Time& t, Transformation& Tr, const std::string& source_frame); 77 | void clearEventQueue(EventQueue& EQ); 78 | void reset(); 79 | 80 | // results 81 | void publishMappingResults( 82 | DepthMap::Ptr depthMapPtr, 83 | Transformation tr, 84 | ros::Time t); 85 | void publishPointCloud( 86 | DepthMap::Ptr& depthMapPtr, 87 | Transformation & tr, 88 | ros::Time& t); 89 | void publishImage( 90 | const cv::Mat &image, 91 | const ros::Time & t, 92 | image_transport::Publisher & pub, 93 | std::string encoding = "bgr8"); 94 | void publishKFPose(const ros::Time& t, Transformation& tr); 95 | void saveDepthMap( 96 | DepthMap::Ptr& depthMapPtr, 97 | std::string& saveDir, 98 | ros::Time t); 99 | 100 | void createEdgeMask( 101 | std::vector& vEventsPtr, 102 | PerspectiveCamera::Ptr& camPtr, 103 | cv::Mat& edgeMap, 104 | std::vector >& vEdgeletCoordinates, 105 | bool bUndistortEvents = true, 106 | size_t radius = 0); 107 | 108 | void createDenoisingMask( 109 | std::vector& vAllEventsPtr, 110 | cv::Mat& mask, 111 | size_t row, size_t col); 112 | 113 | void extractDenoisedEvents( 114 | std::vector &vCloseEventsPtr, 115 | std::vector &vEdgeEventsPtr, 116 | cv::Mat& mask, 117 | size_t maxNum = 5000); 118 | 119 | private: 120 | ros::NodeHandle nh_, pnh_; 121 | 122 | // Subcribers 123 | ros::Subscriber events_left_sub_, events_right_sub_; 124 | ros::Subscriber stampedPose_sub_; 125 | message_filters::Subscriber TS_left_sub_, TS_right_sub_; 126 | 127 | // Publishers 128 | ros::Publisher pc_pub_; 129 | image_transport::ImageTransport it_; 130 | 131 | // Time-Surface sync policy 132 | typedef message_filters::sync_policies::ExactTime ExactSyncPolicy; 133 | message_filters::Synchronizer TS_sync_; 134 | 135 | // dynamic configuration (modify parameters online) 136 | boost::shared_ptr > server_; 137 | dynamic_reconfigure::Server::CallbackType dynamic_reconfigure_callback_; 138 | 139 | // offline data 140 | std::string dvs_frame_id_; 141 | std::string world_frame_id_; 142 | std::string calibInfoDir_; 143 | CameraSystem::Ptr camSysPtr_; 144 | 145 | // online data 146 | EventQueue events_left_, events_right_; 147 | TimeSurfaceHistory TS_history_; 148 | StampedTimeSurfaceObs TS_obs_; 149 | StampTransformationMap st_map_; 150 | std::shared_ptr tf_; 151 | size_t TS_id_; 152 | ros::Time tf_lastest_common_time_; 153 | 154 | // system 155 | eMVStereoMode msm_; 156 | DepthProblemConfig::Ptr dpConfigPtr_; 157 | DepthProblemSolver dpSolver_; 158 | DepthFusion dFusor_; 159 | DepthRegularization dRegularizor_; 160 | Visualization visualizor_; 161 | EventMatcher em_; 162 | EventBM ebm_; 163 | 164 | // data transfer 165 | std::vector vEventsPtr_left_, vEventsPtr_right_;// for EM 166 | ros::Time t_lowBound_, t_upBound_; // for EM 167 | std::vector vALLEventsPtr_left_;// for BM 168 | std::vector vCloseEventsPtr_left_;// for BM 169 | std::vector vDenoisedEventsPtr_left_;// for BM 170 | size_t totalNumCount_;// for both 171 | std::vector vEventsPtr_left_SGM_;// for SGM 172 | 173 | // result 174 | PointCloud::Ptr pc_; 175 | DepthFrame::Ptr depthFramePtr_; 176 | std::deque > dqvDepthPoints_; 177 | 178 | // inter-thread management 179 | std::mutex data_mutex_; 180 | std::promise mapping_thread_promise_, reset_promise_; 181 | std::future mapping_thread_future_, reset_future_; 182 | 183 | /**** MVStereo parameters ***/ 184 | // range and visualization parameters 185 | double invDepth_min_range_; 186 | double invDepth_max_range_; 187 | double cost_vis_threshold_; 188 | size_t patch_area_; 189 | double residual_vis_threshold_; 190 | double stdVar_vis_threshold_; 191 | size_t age_max_range_; 192 | size_t age_vis_threshold_; 193 | int fusion_radius_; 194 | std::string FusionStrategy_; 195 | int maxNumFusionFrames_; 196 | int maxNumFusionPoints_; 197 | // module parameters 198 | size_t PROCESS_EVENT_NUM_; 199 | size_t TS_HISTORY_LENGTH_; 200 | size_t mapping_rate_hz_; 201 | // options 202 | bool changed_frame_rate_; 203 | bool bRegularization_; 204 | bool resetButton_; 205 | bool bDenoising_; 206 | 207 | // Event Matching (EM [26]) parameters 208 | double EM_Slice_Thickness_; 209 | double EM_Time_THRESHOLD_; 210 | double EM_EPIPOLAR_THRESHOLD_; 211 | double EM_TS_NCC_THRESHOLD_; 212 | size_t EM_patch_size_X_; 213 | size_t EM_patch_size_Y_; 214 | size_t EM_numEventMatching_; 215 | size_t EM_patch_intensity_threshold_; 216 | double EM_patch_valid_ratio_; 217 | 218 | // Event Block Matching (BM) parameters 219 | double BM_half_slice_thickness_; 220 | size_t BM_MAX_NUM_EVENTS_PER_MATCHING_; 221 | size_t BM_patch_size_X_; 222 | size_t BM_patch_size_Y_; 223 | size_t BM_min_disparity_; 224 | size_t BM_max_disparity_; 225 | size_t BM_step_; 226 | double BM_ZNCC_Threshold_; 227 | bool BM_bUpDownConfiguration_; 228 | 229 | // SGM [45] parameters 230 | int num_disparities_; 231 | int block_size_; 232 | int P1_; 233 | int P2_; 234 | int uniqueness_ratio_; 235 | cv::Ptr sgbm_; 236 | 237 | ros::Publisher pose_pub_; 238 | image_transport::Publisher invDepthMap_pub_, stdVarMap_pub_, ageMap_pub_, costMap_pub_; 239 | // For counting the total number of fusion 240 | size_t TotalNumFusion_; 241 | }; 242 | } 243 | 244 | #endif //ESVO_CORE_ESVO_MVSTEREO_H -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/esvo_Mapping.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_MAPPING_H 2 | #define ESVO_CORE_MAPPING_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | namespace esvo_core 41 | { 42 | using namespace core; 43 | class esvo_Mapping 44 | { 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | esvo_Mapping(const ros::NodeHandle& nh, 48 | const ros::NodeHandle& nh_private); 49 | virtual ~esvo_Mapping(); 50 | 51 | // mapping 52 | void MappingLoop(std::promise prom_mapping, std::future future_reset); 53 | void MappingAtTime(const ros::Time& t); 54 | bool InitializationAtTime(const ros::Time& t); 55 | bool dataTransferring(); 56 | 57 | // callback functions 58 | void stampedPoseCallback(const geometry_msgs::PoseStampedConstPtr &ps_msg); 59 | void eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg, EventQueue& EQ); 60 | void timeSurfaceCallback( 61 | const sensor_msgs::ImageConstPtr& time_surface_left, 62 | const sensor_msgs::ImageConstPtr& time_surface_right); 63 | void onlineParameterChangeCallback(DVS_MappingStereoConfig &config, uint32_t level); 64 | 65 | // utils 66 | bool getPoseAt(const ros::Time& t, Transformation& Tr, const std::string& source_frame); 67 | void clearEventQueue(EventQueue& EQ); 68 | void reset(); 69 | 70 | /*** publish results ***/ 71 | void publishMappingResults( 72 | DepthMap::Ptr depthMapPtr, 73 | Transformation tr, 74 | ros::Time t); 75 | void publishPointCloud( 76 | DepthMap::Ptr& depthMapPtr, 77 | Transformation & tr, 78 | ros::Time& t); 79 | void publishImage( 80 | const cv::Mat &image, 81 | const ros::Time & t, 82 | image_transport::Publisher & pub, 83 | std::string encoding = "bgr8"); 84 | 85 | /*** event processing ***/ 86 | void createEdgeMask( 87 | std::vector& vEventsPtr, 88 | PerspectiveCamera::Ptr& camPtr, 89 | cv::Mat& edgeMap, 90 | std::vector >& vEdgeletCoordinates, 91 | bool bUndistortEvents = true, 92 | size_t radius = 0); 93 | 94 | void createDenoisingMask( 95 | std::vector& vAllEventsPtr, 96 | cv::Mat& mask, 97 | size_t row, size_t col);// reserve in this file 98 | 99 | void extractDenoisedEvents( 100 | std::vector &vCloseEventsPtr, 101 | std::vector &vEdgeEventsPtr, 102 | cv::Mat& mask, 103 | size_t maxNum = 5000); 104 | 105 | /************************ member variables ************************/ 106 | private: 107 | ros::NodeHandle nh_, pnh_; 108 | 109 | // Subscribers 110 | ros::Subscriber events_left_sub_, events_right_sub_; 111 | ros::Subscriber stampedPose_sub_; 112 | message_filters::Subscriber TS_left_sub_, TS_right_sub_; 113 | 114 | // Publishers 115 | ros::Publisher pc_pub_, gpc_pub_; 116 | image_transport::ImageTransport it_; 117 | double t_last_pub_pc_; 118 | 119 | // Time-Surface sync policy 120 | typedef message_filters::sync_policies::ExactTime ExactSyncPolicy; 121 | message_filters::Synchronizer TS_sync_; 122 | 123 | // dynamic configuration (modify parameters online) 124 | boost::shared_ptr > server_; 125 | dynamic_reconfigure::Server::CallbackType dynamic_reconfigure_callback_; 126 | 127 | // offline data 128 | std::string dvs_frame_id_; 129 | std::string world_frame_id_; 130 | std::string calibInfoDir_; 131 | CameraSystem::Ptr camSysPtr_; 132 | 133 | // online data 134 | EventQueue events_left_, events_right_; 135 | TimeSurfaceHistory TS_history_; 136 | StampedTimeSurfaceObs TS_obs_; 137 | StampTransformationMap st_map_; 138 | std::shared_ptr tf_; 139 | size_t TS_id_; 140 | ros::Time tf_lastest_common_time_; 141 | 142 | // system 143 | std::string ESVO_System_Status_; 144 | DepthProblemConfig::Ptr dpConfigPtr_; 145 | DepthProblemSolver dpSolver_; 146 | DepthFusion dFusor_; 147 | DepthRegularization dRegularizor_; 148 | Visualization visualizor_; 149 | EventBM ebm_; 150 | 151 | // data transfer 152 | std::vector vALLEventsPtr_left_;// for BM 153 | std::vector vCloseEventsPtr_left_;// for BM 154 | std::vector vDenoisedEventsPtr_left_;// for BM 155 | size_t totalNumCount_;// count the number of events involved 156 | std::vector vEventsPtr_left_SGM_;// for SGM 157 | 158 | // result 159 | PointCloud::Ptr pc_, pc_near_, pc_global_; 160 | DepthFrame::Ptr depthFramePtr_; 161 | std::deque > dqvDepthPoints_; 162 | 163 | // inter-thread management 164 | std::mutex data_mutex_; 165 | std::promise mapping_thread_promise_, reset_promise_; 166 | std::future mapping_thread_future_, reset_future_; 167 | 168 | /**** mapping parameters ***/ 169 | // range and visualization threshold 170 | double invDepth_min_range_; 171 | double invDepth_max_range_; 172 | double cost_vis_threshold_; 173 | size_t patch_area_; 174 | double residual_vis_threshold_; 175 | double stdVar_vis_threshold_; 176 | size_t age_max_range_; 177 | size_t age_vis_threshold_; 178 | int fusion_radius_; 179 | std::string FusionStrategy_; 180 | int maxNumFusionFrames_; 181 | int maxNumFusionPoints_; 182 | size_t INIT_SGM_DP_NUM_Threshold_; 183 | // module parameters 184 | size_t PROCESS_EVENT_NUM_; 185 | size_t TS_HISTORY_LENGTH_; 186 | size_t mapping_rate_hz_; 187 | // options 188 | bool changed_frame_rate_; 189 | bool bRegularization_; 190 | bool resetButton_; 191 | bool bDenoising_; 192 | bool bVisualizeGlobalPC_; 193 | // visualization parameters 194 | double visualizeGPC_interval_; 195 | double visualize_range_; 196 | size_t numAddedPC_threshold_; 197 | // Event Block Matching (BM) parameters 198 | double BM_half_slice_thickness_; 199 | size_t BM_patch_size_X_; 200 | size_t BM_patch_size_Y_; 201 | size_t BM_min_disparity_; 202 | size_t BM_max_disparity_; 203 | size_t BM_step_; 204 | double BM_ZNCC_Threshold_; 205 | bool BM_bUpDownConfiguration_; 206 | 207 | // SGM parameters (Used by Initialization) 208 | int num_disparities_; 209 | int block_size_; 210 | int P1_; 211 | int P2_; 212 | int uniqueness_ratio_; 213 | cv::Ptr sgbm_; 214 | 215 | /**********************************************************/ 216 | /******************** For test & debug ********************/ 217 | /**********************************************************/ 218 | image_transport::Publisher invDepthMap_pub_, stdVarMap_pub_, ageMap_pub_, costMap_pub_; 219 | 220 | // For counting the total number of fusion 221 | size_t TotalNumFusion_; 222 | }; 223 | } 224 | 225 | #endif //ESVO_CORE_MAPPING_H 226 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/esvo_Tracking.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_TRACKING_H 2 | #define ESVO_CORE_TRACKING_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | namespace esvo_core 35 | { 36 | using namespace core; 37 | enum TrackingStatus 38 | { 39 | IDLE, 40 | WORKING 41 | }; 42 | 43 | class esvo_Tracking 44 | { 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | esvo_Tracking(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private); 48 | virtual ~esvo_Tracking(); 49 | 50 | // functions regarding tracking 51 | void TrackingLoop(); 52 | bool refDataTransferring(); 53 | bool curDataTransferring();// These two data transferring functions are decoupled because the data are not updated at the same frequency. 54 | 55 | // topic callback functions 56 | void refMapCallback(const sensor_msgs::PointCloud2::ConstPtr& msg); 57 | void timeSurfaceCallback( 58 | const sensor_msgs::ImageConstPtr& time_surface_left, 59 | const sensor_msgs::ImageConstPtr& time_surface_right); 60 | void eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg); 61 | 62 | // results 63 | void publishPose(const ros::Time& t, Transformation& tr); 64 | void publishPath(const ros::Time& t, Transformation& tr); 65 | void saveTrajectory(const std::string &resultDir); 66 | 67 | // utils 68 | void reset(); 69 | void clearEventQueue(); 70 | void stampedPoseCallback(const geometry_msgs::PoseStampedConstPtr &msg); 71 | bool getPoseAt( 72 | const ros::Time &t, 73 | esvo_core::Transformation &Tr,// T_world_something 74 | const std::string& source_frame ); 75 | 76 | private: 77 | ros::NodeHandle nh_, pnh_; 78 | image_transport::ImageTransport it_; 79 | 80 | // subscribers and publishers 81 | ros::Subscriber events_left_sub_; 82 | ros::Subscriber map_sub_; 83 | message_filters::Subscriber TS_left_sub_, TS_right_sub_; 84 | ros::Subscriber stampedPose_sub_; 85 | image_transport::Publisher reprojMap_pub_left_; 86 | 87 | // publishers 88 | ros::Publisher pose_pub_, path_pub_; 89 | 90 | // results 91 | nav_msgs::Path path_; 92 | std::list, Eigen::aligned_allocator > > lPose_; 93 | std::list lTimestamp_; 94 | 95 | // Time Surface sync policy 96 | typedef message_filters::sync_policies::ExactTime ExactSyncPolicy; 97 | message_filters::Synchronizer TS_sync_; 98 | 99 | // offline data 100 | std::string dvs_frame_id_; 101 | std::string world_frame_id_; 102 | std::string calibInfoDir_; 103 | CameraSystem::Ptr camSysPtr_; 104 | 105 | // inter-thread management 106 | std::mutex data_mutex_; 107 | 108 | // online data 109 | EventQueue events_left_; 110 | TimeSurfaceHistory TS_history_; 111 | size_t TS_id_; 112 | std::shared_ptr tf_; 113 | RefPointCloudMap refPCMap_; 114 | RefFrame ref_; 115 | CurFrame cur_; 116 | 117 | /**** offline parameters ***/ 118 | size_t tracking_rate_hz_; 119 | size_t TS_HISTORY_LENGTH_; 120 | size_t REF_HISTORY_LENGTH_; 121 | bool bSaveTrajectory_; 122 | bool bVisualizeTrajectory_; 123 | std::string resultPath_; 124 | 125 | Eigen::Matrix T_world_ref_; 126 | Eigen::Matrix T_world_cur_; 127 | 128 | /*** system objects ***/ 129 | RegProblemType rpType_; 130 | TrackingStatus ets_; 131 | std::string ESVO_System_Status_; 132 | RegProblemConfig::Ptr rpConfigPtr_; 133 | RegProblemSolverLM rpSolver_; 134 | }; 135 | } 136 | 137 | 138 | #endif //ESVO_CORE_TRACKING_H 139 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/optimization/OptimizationFunctor.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_OPTIMIZATION_OPTIMIZATIONFUNCTOR_H 2 | #define ESVO_CORE_OPTIMIZATION_OPTIMIZATIONFUNCTOR_H 3 | 4 | #include 5 | #include 6 | 7 | namespace esvo_core 8 | { 9 | namespace optimization 10 | { 11 | 12 | /** 13 | * Generic functor base for use with the Eigen-nonlinear optimization 14 | * toolbox. Please refer to the Eigen-documentation for further information. 15 | */ 16 | template 17 | struct OptimizationFunctor 18 | { 19 | /** undocumented */ 20 | typedef _Scalar Scalar; 21 | /** undocumented */ 22 | enum 23 | { 24 | InputsAtCompileTime = NX, 25 | ValuesAtCompileTime = NY 26 | }; 27 | /** undocumented */ 28 | typedef Eigen::Matrix InputType; 29 | /** undocumented */ 30 | typedef Eigen::Matrix ValueType; 31 | /** undocumented */ 32 | typedef Eigen::Matrix JacobianType; 33 | 34 | /** undocumented */ 35 | const int m_inputs; 36 | /** undocumented */ 37 | int m_values; 38 | 39 | /** undocumented */ 40 | OptimizationFunctor() : 41 | m_inputs(InputsAtCompileTime), 42 | m_values(ValuesAtCompileTime) {} 43 | /** undocumented */ 44 | OptimizationFunctor(int inputs, int values) : 45 | m_inputs(inputs), 46 | m_values(values) {} 47 | 48 | /** undocumented */ 49 | int inputs() const 50 | { 51 | return m_inputs; 52 | } 53 | /** undocumented */ 54 | int values() const 55 | { 56 | return m_values; 57 | } 58 | 59 | void resetNumberValues( int values ) 60 | { 61 | m_values = values; 62 | } 63 | }; 64 | 65 | } 66 | } 67 | #endif //ESVO_CORE_OPTIMIZATION_OPTIMIZATIONFUNCTOR_H 68 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/tools/TicToc.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_TOOLS_TICTOC_H 2 | #define ESVO_CORE_TOOLS_TICTOC_H 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace esvo_core 11 | { 12 | namespace tools 13 | { 14 | class TicToc 15 | { 16 | public: 17 | TicToc() 18 | { 19 | tic(); 20 | } 21 | 22 | void tic() 23 | { 24 | start = std::chrono::system_clock::now(); 25 | } 26 | 27 | double toc() 28 | { 29 | end = std::chrono::system_clock::now(); 30 | std::chrono::duration elapsed_seconds = end - start; 31 | return elapsed_seconds.count() * 1000; // returns millisec 32 | } 33 | 34 | private: 35 | std::chrono::time_point start, end; 36 | }; 37 | } 38 | } 39 | #endif //ESVO_CORE_TOOLS_TICTOC_H 40 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/tools/Visualization.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_TOOLS_VISUALIZATION_H 2 | #define ESVO_CORE_TOOLS_VISUALIZATION_H 3 | 4 | #include 5 | 6 | namespace esvo_core 7 | { 8 | using namespace container; 9 | namespace tools 10 | { 11 | enum VisMapType 12 | { 13 | InvDepthMap, 14 | StdVarMap, 15 | CostMap, 16 | AgeMap 17 | }; 18 | class Visualization 19 | { 20 | public: 21 | Visualization(); 22 | 23 | virtual ~Visualization(); 24 | 25 | void plot_map( 26 | DepthMap::Ptr &depthMapPtr, 27 | VisMapType vmType, 28 | cv::Mat &img, 29 | double max_range, 30 | double min_range, 31 | double visualization_threshold1, 32 | double visualization_threshold2 = 0.0); 33 | 34 | void plot_eventMap( 35 | std::vector& vEventPtr, 36 | cv::Mat & eventMap, 37 | size_t row, size_t col); 38 | 39 | void plot_events( 40 | std::vector, 41 | Eigen::aligned_allocator > > & vEvents, 42 | cv::Mat & event_img, 43 | size_t row, size_t col); 44 | 45 | void DrawPoint( 46 | double val, 47 | double max_range, 48 | double min_range, 49 | const Eigen::Vector2d &location, 50 | cv::Mat &img ); 51 | 52 | public: 53 | //the rgb values for a jet colormap with 256 values 54 | static const float r[]; 55 | static const float g[]; 56 | static const float b[]; 57 | }; 58 | } 59 | } 60 | 61 | #endif //ESVO_CORE_TOOLS_VISUALIZATION_H 62 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/tools/cayley.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_TOOLS_CAYLEY_H 2 | #define ESVO_CORE_TOOLS_CAYLEY_H 3 | 4 | #include 5 | namespace esvo_core 6 | { 7 | namespace tools 8 | { 9 | Eigen::Matrix3d cayley2rot( const Eigen::Vector3d & cayley); 10 | Eigen::Vector3d rot2cayley( const Eigen::Matrix3d & R ); 11 | }// namespace tools 12 | }// namespace esvo_core 13 | 14 | 15 | #endif //ESVO_CORE_TOOLS_CAYLEY_H 16 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/tools/params_helper.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_PARAMS_HELPER_H 2 | #define ESVO_CORE_PARAMS_HELPER_H 3 | 4 | #pragma once 5 | #include 6 | #include 7 | 8 | namespace esvo_core 9 | { 10 | namespace tools 11 | { 12 | //inline 13 | //bool hasParam(const std::string &name) 14 | //{ 15 | // return ros::param::has(name); 16 | //} 17 | // 18 | //template 19 | //T getParam(const std::string &name, const T &defaultValue) 20 | //{ 21 | // T v; 22 | // if (ros::param::get(name, v)) 23 | // { 24 | // ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v); 25 | // return v; 26 | // } 27 | // else 28 | // ROS_WARN_STREAM("Cannot find value for parameter: " << name << ", assigning default: " << defaultValue); 29 | // return defaultValue; 30 | //} 31 | // 32 | //template 33 | //T getParam(const std::string &name) 34 | //{ 35 | // T v; 36 | // if (ros::param::get(name, v)) 37 | // { 38 | // ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v); 39 | // return v; 40 | // } 41 | // else 42 | // ROS_ERROR_STREAM("Cannot find value for parameter: " << name); 43 | // return T(); 44 | //} 45 | 46 | template 47 | T param(const ros::NodeHandle &nh, const std::string &name, const T &defaultValue) 48 | { 49 | if (nh.hasParam(name)) 50 | { 51 | T v; 52 | nh.param(name, v, defaultValue); 53 | ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v); 54 | return v; 55 | } 56 | ROS_WARN_STREAM("Cannot find value for parameter: " << name << ", assigning default: " << defaultValue); 57 | return defaultValue; 58 | } 59 | } // tools 60 | } // esvo_core 61 | 62 | #endif //ESVO_CORE_PARAMS_HELPER_H 63 | -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/tools/sobel.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_TOOLS_SOBEL_H 2 | #define ESVO_CORE_TOOLS_SOBEL_H 3 | 4 | #include 5 | #include 6 | 7 | using namespace std; 8 | namespace esvo_core 9 | { 10 | namespace tools 11 | { 12 | class Sobel 13 | { 14 | public: 15 | Sobel(size_t kernel_size); 16 | virtual ~Sobel(); 17 | 18 | double grad_x(Eigen::Matrix3d& src); 19 | double grad_y(Eigen::Matrix3d& src); 20 | void grad_xy(Eigen::Matrix3d& src, Eigen::Vector2d& grad); 21 | double convolve( 22 | const Eigen::Matrix3d& kernel, 23 | const Eigen::Matrix3d& src); 24 | private: 25 | size_t kernel_size_; 26 | static const Eigen::Matrix3d sobel_3x3_x, sobel_3x3_y; 27 | static const Eigen::Matrix sobel_5x5_x, sobel_5x5_y; 28 | }; 29 | } 30 | } 31 | #endif //ESVO_CORE_TOOLS_SOBEL_H -------------------------------------------------------------------------------- /esvo_core/include/esvo_core/tools/utils.h: -------------------------------------------------------------------------------- 1 | #ifndef ESVO_CORE_TOOLS_UTILS_H 2 | #define ESVO_CORE_TOOLS_UTILS_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | using namespace std; 30 | namespace esvo_core 31 | { 32 | namespace tools 33 | { 34 | // TUNE this according to your platform's computational capability. 35 | #define NUM_THREAD_TRACKING 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 | size_t numElement = patch.rows() * patch.cols(); 79 | mean = patch.array().sum() / numElement; 80 | Eigen::MatrixXd sub = patch.array() - mean; 81 | sigma = sqrt((sub.array() * sub.array()).sum() / numElement) + 1e-6; 82 | } 83 | 84 | static inline void normalizePatch( 85 | Eigen::MatrixXd& patch_src, 86 | Eigen::MatrixXd& patch_dst) 87 | { 88 | double mean = 0; 89 | double sigma = 0; 90 | meanStdDev(patch_src,mean,sigma); 91 | patch_dst = (patch_src.array() - mean) / sigma; 92 | } 93 | 94 | // recursively create a directory 95 | static inline void _mkdir(const char *dir) 96 | { 97 | char tmp[256]; 98 | char *p = NULL; 99 | size_t len; 100 | 101 | snprintf(tmp, sizeof(tmp),"%s",dir); 102 | len = strlen(tmp); 103 | if(tmp[len - 1] == '/') 104 | tmp[len - 1] = 0; 105 | for(p = tmp + 1; *p; p++) 106 | if(*p == '/') { 107 | *p = 0; 108 | mkdir(tmp, S_IRWXU); 109 | *p = '/'; 110 | } 111 | mkdir(tmp, S_IRWXU); 112 | } 113 | 114 | }// tools 115 | }// esvo_core 116 | #endif //ESVO_CORE_TOOLS_UTILS_H 117 | -------------------------------------------------------------------------------- /esvo_core/launch/mvstereo/mvstereo_rpg.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | "marker" 40 | "map" 41 | $(arg calibInfoDirStr) 42 | 43 | 44 | 45 | 46 | 47 | 49 | 51 | 52 | -------------------------------------------------------------------------------- /esvo_core/launch/mvstereo/mvstereo_upenn.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | "dvs" 40 | "map" 41 | $(arg calibInfoDirStr) 42 | 43 | 44 | 45 | 46 | 47 | 49 | 51 | 52 | -------------------------------------------------------------------------------- /esvo_core/launch/system/DSEC/system_zurich_city_04_a.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | "dvs" 41 | "map" 42 | $(arg calibInfoDirStr) 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | "dvs" 57 | "map" 58 | $(arg calibInfoDirStr) 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /esvo_core/launch/system/system_hkust.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | "dvs" 41 | "map" 42 | $(arg calibInfoDirStr) 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | "dvs" 57 | "map" 58 | $(arg calibInfoDirStr) 59 | 60 | 61 | 62 | 63 | 64 | 66 | 68 | 69 | -------------------------------------------------------------------------------- /esvo_core/launch/system/system_rpg.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | "dvs" 41 | "map" 42 | $(arg calibInfoDirStr) 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | "dvs" 57 | "map" 58 | $(arg calibInfoDirStr) 59 | 60 | 61 | 62 | 63 | 64 | 66 | 68 | 69 | -------------------------------------------------------------------------------- /esvo_core/launch/system/system_upenn.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | "dvs" 41 | "map" 42 | $(arg calibInfoDirStr) 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | "dvs" 57 | "map" 58 | $(arg calibInfoDirStr) 59 | 60 | 61 | 62 | 63 | 64 | 66 | 68 | 69 | -------------------------------------------------------------------------------- /esvo_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | esvo_core 4 | 0.0.0 5 | The esvo_core package. 6 | 7 | 8 | 9 | Yi Zhou 10 | 11 | 12 | 13 | 14 | GPLv3 15 | 16 | catkin 17 | catkin_simple 18 | 19 | dvs_msgs 20 | glog_catkin 21 | gflags_catkin 22 | cv_bridge 23 | minkindr 24 | minkindr_conversions 25 | image_geometry 26 | image_transport 27 | pcl_ros 28 | tf_conversions 29 | tf2 30 | tf2_ros 31 | roscpp 32 | dynamic_reconfigure 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /esvo_core/src/container/CameraSystem.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | namespace esvo_core 6 | { 7 | namespace container 8 | { 9 | PerspectiveCamera::PerspectiveCamera() 10 | { 11 | } 12 | PerspectiveCamera::~PerspectiveCamera() {} 13 | 14 | void PerspectiveCamera::setIntrinsicParameters( 15 | size_t width, 16 | size_t height, 17 | std::string &cameraName, 18 | std::string &distortion_model, 19 | std::vector &vD, 20 | std::vector &vK, 21 | std::vector &vRectMat, 22 | std::vector &vP) 23 | { 24 | width_ = width; 25 | height_ = height; 26 | cameraName_ = cameraName; 27 | distortion_model_ = distortion_model; 28 | D_ = Eigen::Matrix(vD.data()); 29 | K_ = Eigen::Matrix(vK.data()); 30 | RectMat_ = Eigen::Matrix(vRectMat.data()); 31 | P_ = Eigen::Matrix(vP.data()); 32 | 33 | preComputeRectifiedCoordinate(); 34 | } 35 | 36 | void PerspectiveCamera::preComputeRectifiedCoordinate() 37 | { 38 | precomputed_rectified_points_ = Eigen::Matrix2Xd(2, height_ * width_); 39 | 40 | cv::Mat_ RawCoordinates(1, width_ * height_); 41 | for (int y = 0; y < height_; y++) 42 | { 43 | for (int x = 0; x < width_; x++) 44 | { 45 | int index = y * width_ + x; 46 | RawCoordinates(index) = cv::Point2f((float) x, (float) y); 47 | } 48 | } 49 | 50 | cv::Mat_ RectCoordinates(1, height_ * width_); 51 | cv::Mat cvKmat(3, 3, CV_64F); 52 | cv::Mat cvDistCoeff(1, 4, CV_64F); 53 | cv::Mat cvRectMat(3, 3, CV_64F); 54 | cv::Mat cvPmat(3, 4, CV_64F); 55 | 56 | cv::eigen2cv(K_, cvKmat); 57 | cv::eigen2cv(D_, cvDistCoeff); 58 | cv::eigen2cv(RectMat_, cvRectMat); 59 | cv::eigen2cv(P_, cvPmat); 60 | if (distortion_model_ == "plumb_bob") 61 | { 62 | cv::undistortPoints(RawCoordinates, RectCoordinates, cvKmat, cvDistCoeff, cvRectMat, cvPmat); 63 | #if CV_MAJOR_VERSION >= 3 64 | cv::Size sensor_size(width_, height_); 65 | cv::initUndistortRectifyMap(cvKmat, cvDistCoeff,cvRectMat, cvPmat, 66 | sensor_size, CV_32FC1, undistort_map1_, undistort_map2_); 67 | cv::Mat cvSrcMask = cvSrcMask.ones(height_, width_, CV_32F); 68 | cv::Mat cvDstMask = cvSrcMask.zeros(height_, width_, CV_32F); 69 | cv::remap(cvSrcMask, cvDstMask, undistort_map1_, undistort_map2_, cv::INTER_LINEAR); 70 | cv::threshold(cvDstMask, cvDstMask, 0.999, 255, cv::THRESH_BINARY); 71 | cvDstMask.convertTo(cvDstMask, CV_8U); 72 | cv::cv2eigen(cvDstMask, UndistortRectify_mask_); 73 | // LOG(INFO) << "#################### UndistortRectify_mask_.size: " << UndistortRectify_mask_.size(); 74 | #else 75 | ROS_ERROR_ONCE("You need OpenCV >= 3.0 to use the equidistant camera model."); 76 | ROS_ERROR_ONCE("Will not publish rectified images."); 77 | #endif 78 | } 79 | else if (distortion_model_ == "equidistant") 80 | { 81 | cv::fisheye::undistortPoints( 82 | RawCoordinates, RectCoordinates, cvKmat, cvDistCoeff, cvRectMat, cvPmat); 83 | #if CV_MAJOR_VERSION >= 3 84 | cv::Size sensor_size(width_, height_); 85 | cv::fisheye::initUndistortRectifyMap(cvKmat, cvDistCoeff, cvRectMat, cvPmat, 86 | sensor_size, CV_32FC1, undistort_map1_, undistort_map2_); 87 | cv::Mat cvSrcMask = cvSrcMask.ones(height_, width_, CV_32F); 88 | cv::Mat cvDstMask = cvSrcMask.zeros(height_, width_, CV_32F); 89 | cv::remap(cvSrcMask, cvDstMask, undistort_map1_, undistort_map2_, cv::INTER_LINEAR); 90 | cv::threshold(cvDstMask, cvDstMask, 0.1, 255, cv::THRESH_BINARY); 91 | cvDstMask.convertTo(cvDstMask, CV_8U); 92 | cv::cv2eigen(cvDstMask, UndistortRectify_mask_); 93 | 94 | // LOG(INFO) << "#################### UndistortRectify_mask_.size: " << UndistortRectify_mask_.size(); 95 | #else 96 | ROS_ERROR_ONCE("You need OpenCV >= 3.0 to use the equidistant camera model."); 97 | ROS_ERROR_ONCE("Will not publish rectified images."); 98 | #endif 99 | } 100 | else 101 | { 102 | std::cout << "wrong distortion model is provided." << std::endl; 103 | exit(-1); 104 | } 105 | 106 | for (size_t i = 0; i < height_ * width_; i++) 107 | { 108 | precomputed_rectified_points_.col(i) = Eigen::Matrix( 109 | RectCoordinates(i).x, RectCoordinates(i).y); 110 | } 111 | } 112 | 113 | Eigen::Matrix 114 | PerspectiveCamera::getRectifiedUndistortedCoordinate(int xcoor, int ycoor) 115 | { 116 | size_t index = ycoor * width_ + xcoor; 117 | return precomputed_rectified_points_.block<2, 1>(0, index); 118 | } 119 | 120 | void 121 | PerspectiveCamera::cam2World( 122 | const Eigen::Vector2d &x, 123 | double invDepth, 124 | Eigen::Vector3d &p) 125 | { 126 | double z = 1.0 / invDepth; 127 | Eigen::Matrix x_ss; 128 | x_ss << x(0), 129 | x(1), 130 | 1, 131 | 1; 132 | Eigen::Vector4d temp; 133 | temp << 0, 0, 0, z;// this z is correct 134 | Eigen::Matrix P_tilde; 135 | P_tilde.block<3, 4>(0, 0) = P_; 136 | P_tilde.block<1, 4>(3, 0) = temp; 137 | Eigen::Vector4d p_s = z * P_tilde.inverse() * x_ss; 138 | p = p_s.block<3, 1>(0, 0) / p_s(3); // This normalization by the last element is not necessary. 139 | } 140 | 141 | void 142 | PerspectiveCamera::world2Cam( 143 | const Eigen::Vector3d &p, 144 | Eigen::Vector2d &x) 145 | { 146 | Eigen::Vector3d x_hom = P_.block<3, 3>(0, 0) * p + P_.block<3, 1>(0, 3); 147 | x = x_hom.head(2) / x_hom(2); 148 | } 149 | 150 | /************************************************************/ 151 | /************************************************************/ 152 | CameraSystem::CameraSystem(const std::string& calibInfoDir, bool bPrintCalibInfo) 153 | { 154 | cam_left_ptr_ = std::shared_ptr(new PerspectiveCamera()); 155 | cam_right_ptr_ = std::shared_ptr(new PerspectiveCamera()); 156 | loadCalibInfo(calibInfoDir, bPrintCalibInfo); 157 | computeBaseline(); 158 | } 159 | CameraSystem::~CameraSystem() {} 160 | 161 | void CameraSystem::computeBaseline() 162 | { 163 | Eigen::Vector3d temp = cam_right_ptr_->P_.block<3,3>(0,0).inverse() * 164 | cam_right_ptr_->P_.block<3,1>(0,3); 165 | baseline_ = temp.norm(); 166 | } 167 | 168 | void CameraSystem::loadCalibInfo(const std::string &cameraSystemDir, bool bPrintCalibInfo) 169 | { 170 | const std::string left_cam_calib_dir(cameraSystemDir + "/left.yaml"); 171 | const std::string right_cam_calib_dir(cameraSystemDir + "/right.yaml"); 172 | YAML::Node leftCamCalibInfo = YAML::LoadFile(left_cam_calib_dir); 173 | YAML::Node rightCamCalibInfo = YAML::LoadFile(right_cam_calib_dir); 174 | 175 | // load calib (left) 176 | size_t width = leftCamCalibInfo["image_width"].as(); 177 | size_t height = leftCamCalibInfo["image_height"].as(); 178 | std::string cameraNameLeft = leftCamCalibInfo["camera_name"].as(); 179 | std::string cameraNameRight = rightCamCalibInfo["camera_name"].as(); 180 | std::string distortion_model = leftCamCalibInfo["distortion_model"].as(); 181 | std::vector vD_left, vK_left, vRectMat_left, vP_left; 182 | std::vector vD_right, vK_right, vRectMat_right, vP_right; 183 | std::vector vT_right_left; 184 | 185 | vD_left = leftCamCalibInfo["distortion_coefficients"]["data"].as< std::vector >(); 186 | vK_left = leftCamCalibInfo["camera_matrix"]["data"].as< std::vector >(); 187 | vRectMat_left = leftCamCalibInfo["rectification_matrix"]["data"].as< std::vector >(); 188 | vP_left = leftCamCalibInfo["projection_matrix"]["data"].as< std::vector >(); 189 | 190 | vD_right = rightCamCalibInfo["distortion_coefficients"]["data"].as< std::vector >(); 191 | vK_right = rightCamCalibInfo["camera_matrix"]["data"].as< std::vector >(); 192 | vRectMat_right = rightCamCalibInfo["rectification_matrix"]["data"].as< std::vector >(); 193 | vP_right = rightCamCalibInfo["projection_matrix"]["data"].as< std::vector >(); 194 | 195 | vT_right_left = leftCamCalibInfo["T_right_left"]["data"].as< std::vector >(); 196 | 197 | cam_left_ptr_->setIntrinsicParameters( 198 | width, height, 199 | cameraNameLeft, 200 | distortion_model, 201 | vD_left, vK_left, vRectMat_left, vP_left); 202 | cam_right_ptr_->setIntrinsicParameters( 203 | width, height, 204 | cameraNameRight, 205 | distortion_model, 206 | vD_right, vK_right, vRectMat_right, vP_right); 207 | 208 | T_right_left_ = Eigen::Matrix(vT_right_left.data()); 209 | 210 | if(bPrintCalibInfo) 211 | printCalibInfo(); 212 | } 213 | 214 | void CameraSystem::printCalibInfo() 215 | { 216 | LOG(INFO) << "============================================" << std::endl; 217 | LOG(INFO) << "Left Camera" << std::endl; 218 | LOG(INFO) << "--image_width: " << cam_left_ptr_->width_; 219 | LOG(INFO) << "--image_height: " << cam_left_ptr_->height_; 220 | LOG(INFO) << "--distortion model: " << cam_left_ptr_->distortion_model_; 221 | LOG(INFO) << "--distortion_coefficients:\n" << cam_left_ptr_->D_; 222 | LOG(INFO) << "--rectification_matrix:\n" << cam_left_ptr_->RectMat_; 223 | LOG(INFO) << "--projection_matrix:\n" << cam_left_ptr_->P_; 224 | LOG(INFO) << "--T_right_left:\n" << T_right_left_; 225 | 226 | LOG(INFO) << "============================================" << std::endl; 227 | LOG(INFO) << "Right Camera:" << std::endl; 228 | LOG(INFO) << "--image_width: " << cam_right_ptr_->width_; 229 | LOG(INFO) << "--image_height: " << cam_right_ptr_->height_; 230 | LOG(INFO) << "--distortion model:" << cam_right_ptr_->distortion_model_; 231 | LOG(INFO) << "--distortion_coefficients:\n" << cam_right_ptr_->D_; 232 | LOG(INFO) << "--rectification_matrix:\n" << cam_right_ptr_->RectMat_; 233 | LOG(INFO) << "--projection_matrix:\n" << cam_right_ptr_->P_; 234 | LOG(INFO) << "============================================" << std::endl; 235 | } 236 | 237 | } // container 238 | 239 | } //esvo_core 240 | -------------------------------------------------------------------------------- /esvo_core/src/container/DepthPoint.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace esvo_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 | } -------------------------------------------------------------------------------- /esvo_core/src/container/EventPoint.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace esvo_core 4 | { 5 | namespace container 6 | { 7 | EventPoint::EventPoint() 8 | { 9 | row_ = 0; 10 | col_ = 0; 11 | ts_ = ros::Time(); 12 | polarity_ = 0; 13 | } 14 | 15 | EventPoint::EventPoint(size_t row, size_t col) 16 | { 17 | row_ = row; 18 | col_ = col; 19 | ts_ = ros::Time(); 20 | polarity_ = 0; 21 | } 22 | 23 | EventPoint::EventPoint(size_t row, size_t col, ros::Time &ts, uint8_t polarity) 24 | { 25 | row_ = row; 26 | col_ = col; 27 | ts_ = ts; 28 | polarity_ = polarity; 29 | } 30 | 31 | EventPoint::~EventPoint() 32 | {} 33 | 34 | size_t 35 | EventPoint::row() const 36 | { 37 | return row_; 38 | } 39 | 40 | size_t 41 | EventPoint::col() const 42 | { 43 | return col_; 44 | } 45 | 46 | ros::Time 47 | EventPoint::ts() const 48 | { 49 | return ts_; 50 | } 51 | 52 | uint8_t 53 | EventPoint::polarity() const 54 | { 55 | return polarity_; 56 | } 57 | 58 | bool 59 | EventPoint::valid() const 60 | { 61 | return ts_.toSec() > 0; 62 | } 63 | 64 | void 65 | EventPoint::copy(const EventPoint ©) 66 | { 67 | ts_= copy.ts_; 68 | polarity_ = copy.polarity_; 69 | } 70 | 71 | } 72 | } -------------------------------------------------------------------------------- /esvo_core/src/container/ResidualItem.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | esvo_core::container::ResidualItem::ResidualItem() = default; 4 | 5 | esvo_core::container::ResidualItem::ResidualItem( 6 | const double x, 7 | const double y, 8 | const double z) 9 | { 10 | initialize(x,y,z); 11 | } 12 | 13 | void esvo_core::container::ResidualItem::initialize( 14 | const double x, 15 | const double y, 16 | const double z) 17 | { 18 | p_ = Eigen::Vector3d(x,y,z); 19 | // bOutlier_ = false; 20 | // variance_ = 1.0; 21 | } -------------------------------------------------------------------------------- /esvo_core/src/core/DepthProblem.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace esvo_core 4 | { 5 | namespace core 6 | { 7 | DepthProblem::DepthProblem( 8 | const DepthProblemConfig::Ptr &dpConfig_ptr, 9 | const CameraSystem::Ptr &camSysPtr) : 10 | optimization::OptimizationFunctor(1, 0), 11 | dpConfigPtr_(dpConfig_ptr), 12 | camSysPtr_(camSysPtr) 13 | { 14 | 15 | } 16 | 17 | void DepthProblem::setProblem( 18 | Eigen::Vector2d & coor, 19 | Eigen::Matrix & T_world_virtual, 20 | StampedTimeSurfaceObs* pStampedTsObs) 21 | { 22 | coordinate_ = coor; 23 | T_world_virtual_ = T_world_virtual; 24 | pStampedTsObs_ = pStampedTsObs; 25 | 26 | vT_left_virtual_.clear(); 27 | vT_left_virtual_.reserve(1); 28 | Eigen::Matrix T_left_world = pStampedTsObs_->second.tr_.inverse().getTransformationMatrix(); 29 | Eigen::Matrix T_left_virtual = T_left_world * T_world_virtual_; 30 | vT_left_virtual_.push_back(T_left_virtual.block<3,4>(0,0)); 31 | resetNumberValues(dpConfigPtr_->patchSize_X_ * dpConfigPtr_->patchSize_Y_); 32 | } 33 | 34 | int DepthProblem::operator()( const Eigen::VectorXd &x, Eigen::VectorXd & fvec ) const 35 | { 36 | size_t wx = dpConfigPtr_->patchSize_X_; 37 | size_t wy = dpConfigPtr_->patchSize_Y_; 38 | size_t patchSize = wx * wy; 39 | int numValid = 0; 40 | 41 | Eigen::Vector2d x1_s, x2_s; 42 | if(!warping(coordinate_, x(0), vT_left_virtual_[0], x1_s, x2_s)) 43 | { 44 | if(strcmp(dpConfigPtr_->LSnorm_.c_str(), "l2") == 0) 45 | for(size_t i = 0; i < patchSize; i++) 46 | fvec[i] = 255; 47 | else if(strcmp(dpConfigPtr_->LSnorm_.c_str(), "zncc") == 0) 48 | for(size_t i = 0; i < patchSize; i++) 49 | fvec[i] = 2 / sqrt(patchSize); 50 | else if(strcmp(dpConfigPtr_->LSnorm_.c_str(), "Tdist") == 0) 51 | for(size_t i = 0; i < patchSize; i++) 52 | { 53 | double residual = 255; 54 | double weight = (dpConfigPtr_->td_nu_ + 1) / (dpConfigPtr_->td_nu_ + std::pow(residual / dpConfigPtr_->td_scale_, 2)); 55 | fvec[i] = sqrt(weight) * residual; 56 | } 57 | else 58 | exit(-1); 59 | return numValid; 60 | } 61 | 62 | Eigen::MatrixXd tau1, tau2; 63 | if (patchInterpolation(pStampedTsObs_->second.TS_left_, x1_s, tau1) 64 | && patchInterpolation(pStampedTsObs_->second.TS_right_, x2_s, tau2)) 65 | { 66 | // compute temporal residual 67 | if (strcmp(dpConfigPtr_->LSnorm_.c_str(), "l2") == 0) 68 | { 69 | for(size_t y = 0; y < wy; y++) 70 | for(size_t x = 0; x < wx; x++) 71 | { 72 | size_t index = y * wx + x; 73 | fvec[index] = tau1(y,x) - tau2(y,x); 74 | } 75 | } 76 | else if(strcmp(dpConfigPtr_->LSnorm_.c_str(), "zncc") == 0) 77 | { 78 | for(size_t y = 0; y < wy; y++) 79 | for(size_t x = 0; x < wx; x++) 80 | { 81 | size_t index = y * wx + x; 82 | double mu1, sigma1, mu2, sigma2; 83 | tools::meanStdDev(tau1, mu1, sigma1); 84 | tools::meanStdDev(tau2, mu2, sigma2); 85 | fvec[index] = ((tau1(y,x) - mu1) / sigma1 - (tau2(y,x) - mu2) / sigma2) / sqrt(patchSize); 86 | } 87 | } 88 | else if(strcmp(dpConfigPtr_->LSnorm_.c_str(), "Tdist") == 0) 89 | { 90 | std::vector vResidual(patchSize); 91 | std::vector vResidualSquared(patchSize); 92 | double scaleSquaredTmp1 = dpConfigPtr_->td_scaleSquared_; 93 | double scaleSquaredTmp2 = -1.0; 94 | bool first_iteration = true; 95 | // loop for scale until it converges 96 | while(fabs(scaleSquaredTmp2 - scaleSquaredTmp1) / scaleSquaredTmp1 > 0.05 || first_iteration) 97 | { 98 | if(!first_iteration) 99 | scaleSquaredTmp1 = scaleSquaredTmp2; 100 | 101 | double sum_scaleSquared = 0; 102 | for(size_t y = 0; y < wy; y++) 103 | { 104 | for (size_t x = 0; x < wx; x++) 105 | { 106 | size_t index = y * wx + x; 107 | if (first_iteration) 108 | { 109 | vResidual[index] = tau1(y, x) - tau2(y, x); 110 | vResidualSquared[index] = std::pow(vResidual[index], 2); 111 | } 112 | if (vResidual[index] != 0) 113 | sum_scaleSquared += vResidualSquared[index] * (dpConfigPtr_->td_nu_ + 1) / 114 | (dpConfigPtr_->td_nu_ + vResidualSquared[index] / scaleSquaredTmp1); 115 | } 116 | } 117 | if(sum_scaleSquared == 0) 118 | { 119 | scaleSquaredTmp2 = dpConfigPtr_->td_scaleSquared_; 120 | break; 121 | } 122 | scaleSquaredTmp2 = sum_scaleSquared / patchSize; 123 | first_iteration = false; 124 | } 125 | 126 | // assign reweighted residual 127 | for(size_t y = 0; y < wy; y++) 128 | { 129 | for (size_t x = 0; x < wx; x++) 130 | { 131 | size_t index = y * wx + x; 132 | double weight = (dpConfigPtr_->td_nu_ + 1) / (dpConfigPtr_->td_nu_ + vResidualSquared[index] / scaleSquaredTmp2); 133 | fvec[index] = sqrt(weight) * vResidual[index]; 134 | } 135 | } 136 | } 137 | else 138 | exit(-1); 139 | numValid = 1; 140 | } 141 | else 142 | { 143 | if(strcmp(dpConfigPtr_->LSnorm_.c_str(), "l2") == 0) 144 | for(size_t i = 0; i < patchSize; i++) 145 | fvec[i] = 255; 146 | else if(strcmp(dpConfigPtr_->LSnorm_.c_str(), "zncc") == 0) 147 | for(size_t i = 0; i < wx * wy; i++) 148 | fvec[i] = 2 / sqrt(patchSize); 149 | else if(strcmp(dpConfigPtr_->LSnorm_.c_str(), "Tdist") == 0) 150 | for(size_t i = 0; i < patchSize; i++) 151 | { 152 | double residual = 255; 153 | double weight = (dpConfigPtr_->td_nu_ + 1) / (dpConfigPtr_->td_nu_ + std::pow(residual / dpConfigPtr_->td_scale_, 2)); 154 | fvec[i] = sqrt(weight) * residual; 155 | } 156 | else 157 | exit(-1); 158 | } 159 | return numValid; 160 | } 161 | 162 | bool DepthProblem::warping( 163 | const Eigen::Vector2d &x, 164 | double d, 165 | const Eigen::Matrix &T_left_virtual, 166 | Eigen::Vector2d &x1_s, 167 | Eigen::Vector2d &x2_s) const 168 | { 169 | // back-project to 3D 170 | Eigen::Vector3d p_rv; 171 | camSysPtr_->cam_left_ptr_->cam2World(x, d, p_rv); 172 | // transfer to left DVS coordinate 173 | Eigen::Vector3d p_left = T_left_virtual.block<3, 3>(0, 0) * p_rv + T_left_virtual.block<3, 1>(0, 3); 174 | // project onto left and right DVS image plane 175 | Eigen::Vector3d x1_hom = camSysPtr_->cam_left_ptr_->P_.block<3, 3>(0, 0) * p_left + 176 | camSysPtr_->cam_left_ptr_->P_.block<3, 1>(0, 3); 177 | Eigen::Vector3d x2_hom = camSysPtr_->cam_right_ptr_->P_.block<3, 3>(0, 0) * p_left + 178 | camSysPtr_->cam_right_ptr_->P_.block<3, 1>(0, 3); 179 | x1_s = x1_hom.block<2, 1>(0, 0) / x1_hom(2); 180 | x2_s = x2_hom.block<2, 1>(0, 0) / x2_hom(2); 181 | 182 | int wx = dpConfigPtr_->patchSize_X_; 183 | int wy = dpConfigPtr_->patchSize_Y_; 184 | int width = camSysPtr_->cam_left_ptr_->width_; 185 | int height = camSysPtr_->cam_left_ptr_->height_; 186 | if (x1_s(0) < (wx - 1) / 2 || x1_s(0) > width - (wx - 1) / 2 || x1_s(1) < (wy - 1) / 2 || x1_s(1) > height - (wy - 1) / 2) 187 | return false; 188 | if (x2_s(0) < (wx - 1) / 2 || x2_s(0) > width - (wx - 1) / 2 || x2_s(1) < (wy - 1) / 2 || x2_s(1) > height - (wy - 1) / 2) 189 | return false; 190 | return true; 191 | } 192 | 193 | bool DepthProblem::patchInterpolation( 194 | const Eigen::MatrixXd &img, 195 | const Eigen::Vector2d &location, 196 | Eigen::MatrixXd &patch, 197 | bool debug) const 198 | { 199 | int wx = dpConfigPtr_->patchSize_X_; 200 | int wy = dpConfigPtr_->patchSize_Y_; 201 | // compute SrcPatch_UpLeft coordinate and SrcPatch_DownRight coordinate 202 | // check patch boundary is inside img boundary 203 | Eigen::Vector2i SrcPatch_UpLeft, SrcPatch_DownRight; 204 | SrcPatch_UpLeft << floor(location[0]) - (wx - 1) / 2, floor(location[1]) - (wy - 1) / 2; 205 | SrcPatch_DownRight << floor(location[0]) + (wx - 1) / 2, floor(location[1]) + (wy - 1) / 2; 206 | 207 | if (SrcPatch_UpLeft[0] < 0 || SrcPatch_UpLeft[1] < 0) 208 | { 209 | if(debug) 210 | { 211 | LOG(INFO) << "patchInterpolation 1: " << SrcPatch_UpLeft.transpose(); 212 | } 213 | return false; 214 | } 215 | if (SrcPatch_DownRight[0] >= img.cols() || SrcPatch_DownRight[1] >= img.rows()) 216 | { 217 | if(debug) 218 | { 219 | LOG(INFO) << "patchInterpolation 2: " << SrcPatch_DownRight.transpose(); 220 | } 221 | return false; 222 | } 223 | 224 | // compute q1 q2 q3 q4 225 | Eigen::Vector2d double_indices; 226 | double_indices << location[1], location[0]; 227 | 228 | std::pair lower_indices(floor(double_indices[0]), floor(double_indices[1])); 229 | std::pair upper_indices(lower_indices.first + 1, lower_indices.second + 1); 230 | 231 | double q1 = upper_indices.second - double_indices[1];// x 232 | double q2 = double_indices[1] - lower_indices.second;// x 233 | double q3 = upper_indices.first - double_indices[0];// y 234 | double q4 = double_indices[0] - lower_indices.first;// y 235 | 236 | // extract Src patch, size (wy+1) * (wx+1) 237 | int wx2 = wx + 1; 238 | int wy2 = wy + 1; 239 | if (SrcPatch_UpLeft[1] + wy >= img.rows() || SrcPatch_UpLeft[0] + wx >= img.cols()) 240 | { 241 | if(debug) 242 | { 243 | LOG(INFO) << "patchInterpolation 3: " << SrcPatch_UpLeft.transpose() 244 | << ", location: " << location.transpose() 245 | << ", floor(location[0]): " << floor(location[0]) 246 | << ", (wx - 1) / 2: " << (wx - 1) / 2 247 | << ", ans: " << floor(location[0]) - (wx - 1) / 2 248 | << ", wx: " << wx << " wy: " << wy 249 | << ", img.row: " << img.rows() << " img.col: " << img.cols(); 250 | } 251 | return false; 252 | } 253 | Eigen::MatrixXd SrcPatch = img.block(SrcPatch_UpLeft[1], SrcPatch_UpLeft[0], wy2, wx2); 254 | 255 | // Compute R, size (wy+1) * wx. 256 | Eigen::MatrixXd R; 257 | R = q1 * SrcPatch.block(0, 0, wy2, wx) + q2 * SrcPatch.block(0, 1, wy2, wx); 258 | 259 | // Compute F, size wy * wx. 260 | patch = q3 * R.block(0, 0, wy, wx) + q4 * R.block(1, 0, wy, wx); 261 | return true; 262 | } 263 | 264 | }// core 265 | }// esvo_core -------------------------------------------------------------------------------- /esvo_core/src/core/DepthProblemSolver.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | //#define DEPTH_PROBLEM_SOLVER_LOG 8 | namespace esvo_core 9 | { 10 | namespace core 11 | { 12 | DepthProblemSolver::DepthProblemSolver( 13 | CameraSystem::Ptr & camSysPtr, 14 | std::shared_ptr & dpConfigPtr, 15 | DepthProblemType dpType, 16 | size_t numThread ) 17 | : 18 | camSysPtr_(camSysPtr), 19 | dpConfigPtr_(dpConfigPtr), 20 | dpType_(dpType), 21 | NUM_THREAD_(numThread) 22 | {} 23 | 24 | DepthProblemSolver::~DepthProblemSolver() 25 | { 26 | } 27 | 28 | void DepthProblemSolver::solve( 29 | std::vector* pvEMP, 30 | StampedTimeSurfaceObs* pStampedTsObs, 31 | std::vector &vdp ) 32 | { 33 | // TicToc tt; 34 | // tt.tic(); 35 | //distribute the loads 36 | std::vector jobs(NUM_THREAD_); 37 | for(size_t i = 0;i < NUM_THREAD_;i++) 38 | { 39 | jobs[i].i_thread_ = i; 40 | jobs[i].pvEMP_ = pvEMP; 41 | jobs[i].pStamped_TS_obs_ = pStampedTsObs; 42 | if(dpType_ == NUMERICAL) 43 | jobs[i].numDiff_dProblemPtr_ = std::make_shared >(dpConfigPtr_, camSysPtr_); 44 | else if(dpType_ == ANALYTICAL) 45 | jobs[i].dProblemPtr_ = std::make_shared(dpConfigPtr_, camSysPtr_); //NOTE: The ANALYTICAL version is not provided in this version. 46 | else 47 | { 48 | LOG(ERROR) << "Wrong Depth Problem Type is assigned!!!"; 49 | exit(-1); 50 | } 51 | jobs[i].vdpPtr_ = std::make_shared >(); 52 | } 53 | // LOG(INFO) << "(DepthProblemSolver) distribute the loads: " << tt.toc() << " ms."; 54 | // tt.tic(); 55 | // 56 | std::vector threads; 57 | for(size_t i = 0; i < NUM_THREAD_;i++) 58 | threads.emplace_back(std::bind(&DepthProblemSolver::solve_multiple_problems, this, jobs[i])); 59 | for( auto & thread : threads) 60 | { 61 | if(thread.joinable()) 62 | thread.join(); 63 | } 64 | // 65 | vdp.clear(); 66 | size_t numPoints = 0; 67 | for(size_t i = 0;i < NUM_THREAD_;i++) 68 | { 69 | #ifdef DEPTH_PROBLEM_SOLVER_LOG 70 | LOG(INFO) << "The " << i << " thread reconstructs " << jobs[i].vdpPtr_->size() << " points"; 71 | #endif 72 | numPoints += jobs[i].vdpPtr_->size(); 73 | } 74 | vdp.reserve(numPoints); 75 | for(size_t i = 0;i < NUM_THREAD_;i++) 76 | vdp.insert(vdp.end(), jobs[i].vdpPtr_->begin(), jobs[i].vdpPtr_->end()); 77 | // LOG(INFO) << "(DepthProblemSolver) copy results: " << tt.toc() << " ms."; 78 | } 79 | 80 | void DepthProblemSolver::solve_multiple_problems(Job & job) 81 | { 82 | size_t i_thread = job.i_thread_; 83 | size_t numEvent = job.pvEMP_->size(); 84 | job.vdpPtr_->clear(); 85 | job.vdpPtr_->reserve(numEvent / NUM_THREAD_ + 1); 86 | 87 | StampedTimeSurfaceObs* pStampedTsObs = job.pStamped_TS_obs_; 88 | 89 | // loop through vdp and call solve_single_problem 90 | for(size_t i = i_thread; i < numEvent; i+=NUM_THREAD_) 91 | { 92 | Eigen::Vector2d coor = (*job.pvEMP_)[i].x_left_; 93 | Eigen::Matrix T_world_virtual = (*job.pvEMP_)[i].trans_.getTransformationMatrix(); 94 | double d_init = (*job.pvEMP_)[i].invDepth_; 95 | 96 | double result[3]; 97 | bool bProblemSolved = false; 98 | if(dpType_ == NUMERICAL) 99 | { 100 | job.numDiff_dProblemPtr_->setProblem(coor, T_world_virtual, pStampedTsObs); 101 | bProblemSolved = solve_single_problem_numerical(d_init,job.numDiff_dProblemPtr_, result); 102 | } 103 | else if(dpType_ == ANALYTICAL) 104 | { 105 | LOG(ERROR) << "Not supported in this release!!! Please use NUMERICAL."; 106 | exit(-1); 107 | } 108 | else 109 | { 110 | LOG(ERROR) << "Wrong Depth Problem Type is assigned!!!"; 111 | exit(-1); 112 | } 113 | 114 | if(bProblemSolved) 115 | { 116 | DepthPoint dp(std::floor(coor(1)), std::floor(coor(0))); 117 | dp.update_x(coor); 118 | Eigen::Vector3d p_cam; 119 | camSysPtr_->cam_left_ptr_->cam2World(coor, result[0], p_cam); 120 | dp.update_p_cam(p_cam); 121 | if(strcmp(dpConfigPtr_->LSnorm_.c_str(), "l2") == 0) 122 | dp.update(result[0], result[1]); 123 | else if(strcmp(dpConfigPtr_->LSnorm_.c_str(), "Tdist") == 0) 124 | { 125 | double scale2_rho = result[1] * (dpConfigPtr_->td_nu_ - 2) / dpConfigPtr_->td_nu_; 126 | dp.update_studentT(result[0], scale2_rho, result[1], dpConfigPtr_->td_nu_); 127 | } 128 | else 129 | exit(-1); 130 | 131 | dp.residual() = result[2]; 132 | dp.updatePose(T_world_virtual); 133 | job.vdpPtr_->push_back(dp); 134 | } 135 | } 136 | } 137 | 138 | bool DepthProblemSolver::solve_single_problem_numerical( 139 | double d_init, 140 | std::shared_ptr< Eigen::NumericalDiff > & dProblemPtr, 141 | double* result) 142 | { 143 | Eigen::VectorXd x(1); 144 | x << d_init; 145 | 146 | Eigen::LevenbergMarquardt, double> lm(*(dProblemPtr.get())); 147 | lm.resetParameters(); 148 | lm.parameters.ftol = 1e-6;//1.E10*Eigen::NumTraits::epsilon(); 149 | lm.parameters.xtol = 1e-6;//1.E10*Eigen::NumTraits::epsilon(); 150 | lm.parameters.maxfev = dpConfigPtr_->MAX_ITERATION_ * 3; 151 | 152 | if(lm.minimizeInit(x) == Eigen::LevenbergMarquardtSpace::ImproperInputParameters) 153 | { 154 | LOG(ERROR) << "ImproperInputParameters for LM (Mapping)." << std::endl; 155 | return false; 156 | } 157 | 158 | size_t iteration = 0; 159 | int optimizationState = 0; 160 | 161 | while(true) 162 | { 163 | Eigen::LevenbergMarquardtSpace::Status status = lm.minimizeOneStep(x); 164 | 165 | iteration++; 166 | if(iteration >= dpConfigPtr_->MAX_ITERATION_) 167 | break; 168 | 169 | bool terminate = false; 170 | if(status == 2 || status == 3) 171 | { 172 | switch (optimizationState) 173 | { 174 | case 0: 175 | { 176 | optimizationState++; 177 | break; 178 | } 179 | case 1: 180 | { 181 | terminate = true; 182 | break; 183 | } 184 | } 185 | } 186 | if(terminate) 187 | break; 188 | } 189 | 190 | // Since there is no way to set a optimization bound 191 | // on x in Eigen (as far as I know), a handy outlier rejection is applied here. 192 | if(x(0) <= 0.001)// we cannot see that far, right? 193 | return false; 194 | 195 | // update 196 | result[0] = x(0); 197 | // calculate the variance according to 198 | // https://android.googlesource.com/platform/external/eigen/+/jb-mr2-release/unsupported/test/NonLinearOptimization.cpp 199 | Eigen::internal::covar(lm.fjac, lm.permutation.indices()); 200 | if(dpConfigPtr_->LSnorm_ == "l2") 201 | { 202 | double fnorm = lm.fvec.blueNorm(); 203 | double covfac = fnorm * fnorm / (dProblemPtr->values() - dProblemPtr->inputs()); 204 | Eigen::MatrixXd cov = covfac * lm.fjac.topLeftCorner<1,1>(); 205 | result[1] = cov(0,0); 206 | } 207 | if(dpConfigPtr_->LSnorm_ == "Tdist") 208 | { 209 | Eigen::MatrixXd invSumJtT = lm.fjac.topLeftCorner<1,1>(); 210 | result[1] = std::pow(dpConfigPtr_->td_stdvar_,2) * invSumJtT(0,0); 211 | } 212 | result[2] = lm.fnorm * lm.fnorm; 213 | return true; 214 | } 215 | 216 | void 217 | DepthProblemSolver::pointCulling( 218 | std::vector &vdp, 219 | double std_variance_threshold, 220 | double cost_threshold, 221 | double invDepth_min_range, 222 | double invDepth_max_range) 223 | { 224 | std::vector vdp_culled; 225 | vdp_culled.reserve(vdp.size()); 226 | std::vector vDepth; 227 | vDepth.reserve(10000); 228 | for(size_t i = 0; i < vdp.size();i++) 229 | { 230 | if(vdp[i].variance() <= pow(std_variance_threshold,2) && 231 | vdp[i].residual() <= cost_threshold && 232 | vdp[i].valid() && 233 | vdp[i].invDepth() >= invDepth_min_range && 234 | vdp[i].invDepth() <= invDepth_max_range) 235 | { 236 | vdp_culled.push_back(vdp[i]); 237 | vDepth.emplace_back(1.0 / vdp[i].invDepth()); 238 | } 239 | } 240 | vdp = vdp_culled; 241 | #ifdef DEPTH_PROBLEM_SOLVER_LOG 242 | LOG(INFO) << "(culling) max depth: " << *std::max_element(vDepth.begin(), vDepth.end()); 243 | #endif 244 | } 245 | 246 | DepthProblemType DepthProblemSolver::getProblemType() 247 | { 248 | return dpType_; 249 | } 250 | 251 | }//end of namespace core 252 | }//end of namespace esvo_core 253 | 254 | -------------------------------------------------------------------------------- /esvo_core/src/core/DepthRegularization.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace esvo_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 | }// esvo_core -------------------------------------------------------------------------------- /esvo_core/src/core/RegProblemSolverLM.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace esvo_core 5 | { 6 | namespace core 7 | { 8 | RegProblemSolverLM::RegProblemSolverLM( 9 | esvo_core::CameraSystem::Ptr &camSysPtr, 10 | shared_ptr &rpConfigPtr, 11 | esvo_core::core::RegProblemType rpType, 12 | size_t numThread): 13 | camSysPtr_(camSysPtr), 14 | rpConfigPtr_(rpConfigPtr), 15 | rpType_(rpType), 16 | NUM_THREAD_(numThread), 17 | bPrint_(false), 18 | bVisualize_(true) 19 | { 20 | if(rpType_ == REG_NUMERICAL) 21 | { 22 | numDiff_regProblemPtr_ = 23 | std::make_shared >(camSysPtr_, rpConfigPtr_, NUM_THREAD_); 24 | } 25 | else if(rpType_ == REG_ANALYTICAL) 26 | { 27 | regProblemPtr_ = std::make_shared(camSysPtr_, rpConfigPtr_, NUM_THREAD_); 28 | } 29 | else 30 | { 31 | LOG(ERROR) << "Wrong Registration Problem Type is assigned!!!"; 32 | exit(-1); 33 | } 34 | z_min_ = 1.0 / rpConfigPtr_->invDepth_max_range_; 35 | z_max_ = 1.0 / rpConfigPtr_->invDepth_min_range_; 36 | 37 | lmStatics_.nPoints_ = 0; 38 | lmStatics_.nfev_ = 0; 39 | lmStatics_.nIter_ = 0; 40 | } 41 | 42 | RegProblemSolverLM::~RegProblemSolverLM() 43 | {} 44 | 45 | bool RegProblemSolverLM::resetRegProblem(RefFrame* ref, CurFrame* cur) 46 | { 47 | if(cur->numEventsSinceLastObs_ < rpConfigPtr_->MIN_NUM_EVENTS_) 48 | { 49 | LOG(INFO) << "resetRegProblem RESET fails for no enough events coming in."; 50 | LOG(INFO) << "However, the system remains to work."; 51 | } 52 | if( ref->vPointXYZPtr_.size() < rpConfigPtr_->BATCH_SIZE_ ) 53 | { 54 | LOG(INFO) << "resetRegProblem RESET fails for no enough point cloud in the local map."; 55 | LOG(INFO) << "The system will be re-initialized"; 56 | return false; 57 | } 58 | // LOG(INFO) << "resetRegProblem RESET succeeds."; 59 | if(rpType_ == REG_NUMERICAL) 60 | { 61 | numDiff_regProblemPtr_->setProblem(ref, cur, false); 62 | // LOG(INFO) << "numDiff_regProblemPtr_->setProblem(ref, cur, false) -----------------"; 63 | } 64 | if(rpType_ == REG_ANALYTICAL) 65 | { 66 | regProblemPtr_->setProblem(ref, cur, true); 67 | // LOG(INFO) << "regProblemPtr_->setProblem(ref, cur, true) -----------------"; 68 | } 69 | 70 | lmStatics_.nPoints_ = 0; 71 | lmStatics_.nfev_ = 0; 72 | lmStatics_.nIter_ = 0; 73 | return true; 74 | } 75 | 76 | bool RegProblemSolverLM::solve_numerical() 77 | { 78 | Eigen::LevenbergMarquardt, double> lm(*numDiff_regProblemPtr_.get()); 79 | lm.resetParameters(); 80 | lm.parameters.ftol = 1e-3; 81 | lm.parameters.xtol = 1e-3; 82 | lm.parameters.maxfev = rpConfigPtr_->MAX_ITERATION_ * 8; 83 | 84 | size_t iteration = 0; 85 | size_t nfev = 0; 86 | while(true) 87 | { 88 | if(iteration >= rpConfigPtr_->MAX_ITERATION_) 89 | break; 90 | numDiff_regProblemPtr_->setStochasticSampling( 91 | (iteration % numDiff_regProblemPtr_->numBatches_) * rpConfigPtr_->BATCH_SIZE_, rpConfigPtr_->BATCH_SIZE_); 92 | Eigen::VectorXd x(6); 93 | x.fill(0.0); 94 | if(lm.minimizeInit(x) == Eigen::LevenbergMarquardtSpace::ImproperInputParameters) 95 | { 96 | LOG(ERROR) << "ImproperInputParameters for LM (Tracking)." << std::endl; 97 | return false; 98 | } 99 | 100 | Eigen::LevenbergMarquardtSpace::Status status = lm.minimizeOneStep(x); 101 | numDiff_regProblemPtr_->addMotionUpdate(x); 102 | 103 | iteration++; 104 | nfev += lm.nfev; 105 | 106 | /*************************** Visualization ************************/ 107 | if(bVisualize_)// will slow down the tracker's performance a little bit 108 | { 109 | size_t width = camSysPtr_->cam_left_ptr_->width_; 110 | size_t height = camSysPtr_->cam_left_ptr_->height_; 111 | cv::Mat reprojMap_left = cv::Mat(cv::Size(width, height), CV_8UC1, cv::Scalar(0)); 112 | cv::eigen2cv(numDiff_regProblemPtr_->cur_->pTsObs_->TS_negative_left_, reprojMap_left); 113 | reprojMap_left.convertTo(reprojMap_left, CV_8UC1); 114 | cv::cvtColor(reprojMap_left, reprojMap_left, CV_GRAY2BGR); 115 | 116 | // project 3D points to current frame 117 | Eigen::Matrix3d R_cur_ref = numDiff_regProblemPtr_->R_.transpose(); 118 | Eigen::Vector3d t_cur_ref = -numDiff_regProblemPtr_->R_.transpose() * numDiff_regProblemPtr_->t_; 119 | 120 | size_t numVisualization = std::min(numDiff_regProblemPtr_->ResItems_.size(), (size_t)2000); 121 | for(size_t i = 0; i < numVisualization; i++) 122 | { 123 | ResidualItem & ri = numDiff_regProblemPtr_->ResItems_[i]; 124 | Eigen::Vector3d p_3D = R_cur_ref * ri.p_ + t_cur_ref; 125 | Eigen::Vector2d p_img_left; 126 | camSysPtr_->cam_left_ptr_->world2Cam(p_3D, p_img_left); 127 | double z = ri.p_[2]; 128 | visualizor_.DrawPoint(1.0 / z, 1.0 / z_min_, 1.0 / z_max_, 129 | Eigen::Vector2d(p_img_left(0), p_img_left(1)), reprojMap_left); 130 | } 131 | std_msgs::Header header; 132 | header.stamp = numDiff_regProblemPtr_->cur_->t_; 133 | sensor_msgs::ImagePtr msg = cv_bridge::CvImage(header, "bgr8", reprojMap_left).toImageMsg(); 134 | reprojMap_pub_->publish(msg); 135 | } 136 | /*************************** Visualization ************************/ 137 | if(status == 2 || status == 3) 138 | break; 139 | } 140 | // LOG(INFO) << "LM Finished ..................."; 141 | numDiff_regProblemPtr_->setPose(); 142 | lmStatics_.nPoints_ = numDiff_regProblemPtr_->numPoints_; 143 | lmStatics_.nfev_ = nfev; 144 | lmStatics_.nIter_ = iteration; 145 | return 0; 146 | } 147 | 148 | bool RegProblemSolverLM::solve_analytical() 149 | { 150 | Eigen::LevenbergMarquardt lm(*regProblemPtr_.get()); 151 | lm.resetParameters(); 152 | lm.parameters.ftol = 1e-3; 153 | lm.parameters.xtol = 1e-3; 154 | lm.parameters.maxfev = rpConfigPtr_->MAX_ITERATION_ * 8; 155 | 156 | size_t iteration = 0; 157 | size_t nfev = 0; 158 | while(true) 159 | { 160 | if(iteration >= rpConfigPtr_->MAX_ITERATION_) 161 | break; 162 | regProblemPtr_->setStochasticSampling( 163 | (iteration % regProblemPtr_->numBatches_) * rpConfigPtr_->BATCH_SIZE_, rpConfigPtr_->BATCH_SIZE_); 164 | Eigen::VectorXd x(6); 165 | x.fill(0.0); 166 | if(lm.minimizeInit(x) == Eigen::LevenbergMarquardtSpace::ImproperInputParameters) 167 | { 168 | LOG(ERROR) << "ImproperInputParameters for LM (Tracking)." << std::endl; 169 | return false; 170 | } 171 | Eigen::LevenbergMarquardtSpace::Status status = lm.minimizeOneStep(x); 172 | regProblemPtr_->addMotionUpdate(x); 173 | 174 | iteration++; 175 | nfev += lm.nfev; 176 | if(status == 2 || status == 3) 177 | break; 178 | } 179 | 180 | /*************************** Visualization ************************/ 181 | if(bVisualize_) // will slow down the tracker a little bit 182 | { 183 | size_t width = camSysPtr_->cam_left_ptr_->width_; 184 | size_t height = camSysPtr_->cam_left_ptr_->height_; 185 | cv::Mat reprojMap_left = cv::Mat(cv::Size(width, height), CV_8UC1, cv::Scalar(0)); 186 | cv::eigen2cv(regProblemPtr_->cur_->pTsObs_->TS_negative_left_, reprojMap_left); 187 | reprojMap_left.convertTo(reprojMap_left, CV_8UC1); 188 | cv::cvtColor(reprojMap_left, reprojMap_left, CV_GRAY2BGR); 189 | 190 | // project 3D points to current frame 191 | Eigen::Matrix3d R_cur_ref = regProblemPtr_->R_.transpose(); 192 | Eigen::Vector3d t_cur_ref = -regProblemPtr_->R_.transpose() * regProblemPtr_->t_; 193 | 194 | size_t numVisualization = std::min(regProblemPtr_->ResItems_.size(), (size_t)2000); 195 | for(size_t i = 0; i < numVisualization; i++) 196 | { 197 | ResidualItem & ri = regProblemPtr_->ResItems_[i]; 198 | Eigen::Vector3d p_3D = R_cur_ref * ri.p_ + t_cur_ref; 199 | Eigen::Vector2d p_img_left; 200 | camSysPtr_->cam_left_ptr_->world2Cam(p_3D, p_img_left); 201 | double z = ri.p_[2]; 202 | visualizor_.DrawPoint(1.0 / z, 1.0 / z_min_, 1.0 / z_max_, 203 | Eigen::Vector2d(p_img_left(0), p_img_left(1)), reprojMap_left); 204 | } 205 | std_msgs::Header header; 206 | header.stamp = regProblemPtr_->cur_->t_; 207 | sensor_msgs::ImagePtr msg = cv_bridge::CvImage(header, "bgr8", reprojMap_left).toImageMsg(); 208 | reprojMap_pub_->publish(msg); 209 | } 210 | /*************************** Visualization ************************/ 211 | 212 | regProblemPtr_->setPose(); 213 | lmStatics_.nPoints_ = regProblemPtr_->numPoints_; 214 | lmStatics_.nfev_ = nfev; 215 | lmStatics_.nIter_ = iteration; 216 | return 0; 217 | } 218 | 219 | void RegProblemSolverLM::setRegPublisher( 220 | image_transport::Publisher* reprojMap_pub) 221 | { 222 | reprojMap_pub_ = reprojMap_pub; 223 | } 224 | 225 | }//namespace core 226 | }//namespace esvo_core 227 | 228 | -------------------------------------------------------------------------------- /esvo_core/src/esvo_MVStereoNode.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char** argv) 4 | { 5 | ros::init(argc, argv, "esvo_MVStereo"); 6 | ros::NodeHandle nh; 7 | ros::NodeHandle nh_private("~"); 8 | 9 | esvo_core::esvo_MVStereo mvs(nh, nh_private); 10 | ros::spin(); 11 | return 0; 12 | } -------------------------------------------------------------------------------- /esvo_core/src/esvo_MappingNode.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char** argv) 4 | { 5 | ros::init(argc, argv, "esvo_Mapping"); 6 | ros::NodeHandle nh; 7 | ros::NodeHandle nh_private("~"); 8 | 9 | esvo_core::esvo_Mapping mapper(nh, nh_private); 10 | ros::spin(); 11 | return 0; 12 | } 13 | 14 | -------------------------------------------------------------------------------- /esvo_core/src/esvo_TrackingNode.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char** argv) 4 | { 5 | ros::init(argc, argv, "esvo_Tracking"); 6 | ros::NodeHandle nh; 7 | ros::NodeHandle nh_private("~"); 8 | 9 | esvo_core::esvo_Tracking tracker(nh, nh_private); 10 | ros::spin(); 11 | return 0; 12 | } 13 | 14 | -------------------------------------------------------------------------------- /esvo_core/src/tools/cayley.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | Eigen::Matrix3d 4 | esvo_core::tools::cayley2rot(const Eigen::Vector3d &cayley) 5 | { 6 | Eigen::Matrix3d R; 7 | double scale = 1+pow(cayley[0],2)+pow(cayley[1],2)+pow(cayley[2],2); 8 | 9 | R(0,0) = 1+pow(cayley[0],2)-pow(cayley[1],2)-pow(cayley[2],2); 10 | R(0,1) = 2*(cayley[0]*cayley[1]-cayley[2]); 11 | R(0,2) = 2*(cayley[0]*cayley[2]+cayley[1]); 12 | R(1,0) = 2*(cayley[0]*cayley[1]+cayley[2]); 13 | R(1,1) = 1-pow(cayley[0],2)+pow(cayley[1],2)-pow(cayley[2],2); 14 | R(1,2) = 2*(cayley[1]*cayley[2]-cayley[0]); 15 | R(2,0) = 2*(cayley[0]*cayley[2]-cayley[1]); 16 | R(2,1) = 2*(cayley[1]*cayley[2]+cayley[0]); 17 | R(2,2) = 1-pow(cayley[0],2)-pow(cayley[1],2)+pow(cayley[2],2); 18 | 19 | R = (1/scale) * R; 20 | return R; 21 | } 22 | 23 | Eigen::Vector3d 24 | esvo_core::tools::rot2cayley(const Eigen::Matrix3d &R) 25 | { 26 | Eigen::Matrix3d C1; 27 | Eigen::Matrix3d C2; 28 | Eigen::Matrix3d C; 29 | C1 = R-Eigen::Matrix3d::Identity(); 30 | C2 = R+Eigen::Matrix3d::Identity(); 31 | C = C1 * C2.inverse(); 32 | 33 | Eigen::Vector3d cayley; 34 | cayley[0] = -C(1,2); 35 | cayley[1] = C(0,2); 36 | cayley[2] = -C(0,1); 37 | 38 | return cayley; 39 | } 40 | -------------------------------------------------------------------------------- /esvo_core/src/tools/sobel.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace esvo_core 4 | { 5 | namespace tools 6 | { 7 | Sobel::Sobel(size_t kernel_size):kernel_size_(kernel_size) 8 | {} 9 | 10 | Sobel::~Sobel() 11 | {} 12 | 13 | double Sobel::grad_x(Eigen::Matrix3d& src) 14 | { 15 | return convolve(sobel_3x3_x, src) / 8; 16 | } 17 | 18 | double Sobel::grad_y(Eigen::Matrix3d& src) 19 | { 20 | return convolve(sobel_3x3_y, src) / 8; 21 | } 22 | 23 | void Sobel::grad_xy(Eigen::Matrix3d& src, Eigen::Vector2d& grad) 24 | { 25 | grad << grad_x(src), grad_y(src); 26 | } 27 | 28 | double Sobel::convolve( 29 | const Eigen::Matrix3d& kernel, 30 | const Eigen::Matrix3d& src) 31 | { 32 | return kernel.cwiseProduct(src).sum(); 33 | } 34 | 35 | const Eigen::Matrix3d Sobel::sobel_3x3_x 36 | = (Eigen::Matrix3d() << -1, 0, 1, 37 | -2, 0, 2, 38 | -1, 0, 1).finished(); 39 | const Eigen::Matrix3d Sobel::sobel_3x3_y 40 | = (Eigen::Matrix3d() << -1, -2, -1, 41 | 0, 0, 0, 42 | 1, 2, 1).finished(); 43 | const Eigen::Matrix Sobel::sobel_5x5_x 44 | = (Eigen::Matrix() << -5, -4, 0, 4, 5, 45 | -8, -10, 0, 10, 8, 46 | -10, -20, 0, 20, 10, 47 | -8, -10, 0, 10, 8, 48 | -5, -4, 0, 4, 5).finished();; 49 | const Eigen::Matrix Sobel::sobel_5x5_y 50 | = (Eigen::Matrix() << -5, -8, -10, -8, -5, 51 | -4, -10, -20, -10, -4, 52 | 0, 0, 0, 0, 0, 53 | 4, 10, 20, 10, 4, 54 | 5, 8, 10, 8, 5).finished(); 55 | }//tools 56 | }//esvo_core -------------------------------------------------------------------------------- /esvo_time_surface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(esvo_time_surface) 3 | 4 | # To be consistent with the configuration in esvo_core 5 | set(CMAKE_CXX_STANDARD 14) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | # SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") 8 | set(CMAKE_CXX_FLAGS "-O3") 9 | 10 | find_package(catkin_simple REQUIRED) 11 | catkin_simple(ALL_DEPS_REQUIRED) 12 | 13 | # SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") 14 | 15 | find_package(OpenCV REQUIRED) 16 | 17 | include_directories(include ${catkin_INCLUDE_DIRS}) 18 | 19 | # make the executable 20 | cs_add_executable(esvo_time_surface 21 | src/TimeSurface.cpp 22 | src/TimeSurface_node.cpp 23 | ) 24 | 25 | # link the executable to the necesarry libs 26 | target_link_libraries(esvo_time_surface 27 | ${catkin_LIBRARIES} 28 | ${OpenCV_LIBRARIES} 29 | ) 30 | 31 | cs_install() -------------------------------------------------------------------------------- /esvo_time_surface/cfg/parameters.yaml: -------------------------------------------------------------------------------- 1 | use_sim_time: True 2 | ignore_polarity: True 3 | time_surface_mode: 0 # 0: Backward; 1: Forward 4 | decay_ms: 30 5 | median_blur_kernel_size: 1 6 | max_event_queue_len: 20 -------------------------------------------------------------------------------- /esvo_time_surface/include/esvo_time_surface/TicToc.h: -------------------------------------------------------------------------------- 1 | #ifndef esvo_tictoc_H_ 2 | #define esvo_tictoc_H_ 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace esvo_time_surface 11 | { 12 | class TicToc 13 | { 14 | public: 15 | TicToc() 16 | { 17 | tic(); 18 | } 19 | 20 | void tic() 21 | { 22 | start = std::chrono::system_clock::now(); 23 | } 24 | 25 | double toc() 26 | { 27 | end = std::chrono::system_clock::now(); 28 | std::chrono::duration elapsed_seconds = end - start; 29 | return elapsed_seconds.count() * 1000; 30 | } 31 | 32 | private: 33 | std::chrono::time_point start, end; 34 | }; 35 | } 36 | 37 | #endif // esvo_tictoc_H_ 38 | -------------------------------------------------------------------------------- /esvo_time_surface/include/esvo_time_surface/TimeSurface.h: -------------------------------------------------------------------------------- 1 | #ifndef esvo_time_surface_H_ 2 | #define esvo_time_surface_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | namespace esvo_time_surface 24 | { 25 | #define NUM_THREAD_TS 1 26 | using EventQueue = std::deque; 27 | 28 | class EventQueueMat 29 | { 30 | public: 31 | EventQueueMat(int width, int height, int queueLen) 32 | { 33 | width_ = width; 34 | height_ = height; 35 | queueLen_ = queueLen; 36 | eqMat_ = std::vector(width_ * height_, EventQueue()); 37 | } 38 | 39 | void insertEvent(const dvs_msgs::Event& e) 40 | { 41 | if(!insideImage(e.x, e.y)) 42 | return; 43 | else 44 | { 45 | EventQueue& eq = getEventQueue(e.x, e.y); 46 | eq.push_back(e); 47 | while(eq.size() > queueLen_) 48 | eq.pop_front(); 49 | } 50 | } 51 | 52 | bool getMostRecentEventBeforeT( 53 | const size_t x, 54 | const size_t y, 55 | const ros::Time& t, 56 | dvs_msgs::Event* ev) 57 | { 58 | if(!insideImage(x, y)) 59 | return false; 60 | 61 | EventQueue& eq = getEventQueue(x, y); 62 | if(eq.empty()) 63 | return false; 64 | 65 | for(auto it = eq.rbegin(); it != eq.rend(); ++it) 66 | { 67 | const dvs_msgs::Event& e = *it; 68 | if(e.ts < t) 69 | { 70 | *ev = *it; 71 | return true; 72 | } 73 | } 74 | return false; 75 | } 76 | 77 | void clear() 78 | { 79 | eqMat_.clear(); 80 | } 81 | 82 | bool insideImage(const size_t x, const size_t y) 83 | { 84 | return !(x < 0 || x >= width_ || y < 0 || y >= height_); 85 | } 86 | 87 | inline EventQueue& getEventQueue(const size_t x, const size_t y) 88 | { 89 | return eqMat_[x + width_ * y]; 90 | } 91 | 92 | size_t width_; 93 | size_t height_; 94 | size_t queueLen_; 95 | std::vector eqMat_; 96 | }; 97 | 98 | class TimeSurface 99 | { 100 | struct Job 101 | { 102 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 103 | EventQueueMat* pEventQueueMat_; 104 | cv::Mat* pTimeSurface_; 105 | size_t start_col_, end_col_; 106 | size_t start_row_, end_row_; 107 | size_t i_thread_; 108 | ros::Time external_sync_time_; 109 | double decay_sec_; 110 | }; 111 | 112 | public: 113 | TimeSurface(ros::NodeHandle & nh, ros::NodeHandle nh_private); 114 | virtual ~TimeSurface(); 115 | 116 | private: 117 | ros::NodeHandle nh_; 118 | // core 119 | void init(int width, int height); 120 | void createTimeSurfaceAtTime(const ros::Time& external_sync_time);// single thread version (This is enough for DAVIS240C and DAVIS346) 121 | void createTimeSurfaceAtTime_hyperthread(const ros::Time& external_sync_time); // hyper thread version (This is for higher resolution) 122 | void thread(Job& job); 123 | 124 | // callbacks 125 | void syncCallback(const std_msgs::TimeConstPtr& msg); 126 | void eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg); 127 | void cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg); 128 | 129 | // utils 130 | void clearEventQueue(); 131 | 132 | // calibration parameters 133 | cv::Mat camera_matrix_, dist_coeffs_; 134 | cv::Mat rectification_matrix_, projection_matrix_; 135 | std::string distortion_model_; 136 | cv::Mat undistort_map1_, undistort_map2_; 137 | Eigen::Matrix2Xd precomputed_rectified_points_; 138 | 139 | // sub & pub 140 | ros::Subscriber event_sub_; 141 | ros::Subscriber camera_info_sub_; 142 | ros::Subscriber sync_topic_; 143 | image_transport::Publisher time_surface_pub_; 144 | 145 | // online parameters 146 | bool bCamInfoAvailable_; 147 | bool bUse_Sim_Time_; 148 | cv::Size sensor_size_; 149 | ros::Time sync_time_; 150 | bool bSensorInitialized_; 151 | 152 | // offline parameters 153 | double decay_ms_; 154 | bool ignore_polarity_; 155 | int median_blur_kernel_size_; 156 | int max_event_queue_length_; 157 | int events_maintained_size_; 158 | 159 | // containers 160 | EventQueue events_; 161 | std::shared_ptr pEventQueueMat_; 162 | 163 | // thread mutex 164 | std::mutex data_mutex_; 165 | 166 | // Time Surface Mode 167 | // Backward: First Apply exp decay on the raw image plane, then get the value 168 | // at each pixel in the rectified image plane by looking up the 169 | // corresponding one (float coordinates) with bi-linear interpolation. 170 | // Forward: First warp the raw events to the rectified image plane, then 171 | // apply the exp decay on the four neighbouring (involved) pixel coordinate. 172 | enum TimeSurfaceMode 173 | { 174 | BACKWARD,// used in the T-RO20 submission 175 | FORWARD 176 | } time_surface_mode_; 177 | }; 178 | } // namespace esvo_time_surface 179 | #endif // esvo_time_surface_H_ -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/dsec/zurich_city_04_a/calib_info.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 19 | 20 | 36 | 37 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/dsec/zurich_city_04_a/zurich_city_04_a.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/hkust/hkust_calib_info.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 13 | 14 | 24 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/hkust/hkust_lab.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/rpg/rpg_bin.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/rpg/rpg_boxes.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/rpg/rpg_calib_info.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 13 | 14 | 24 | 25 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/rpg/rpg_desk.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/rpg/rpg_monitor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/upenn/upenn_indoor_flying1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/rosbag_launcher/upenn/upenn_indoor_flying3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /esvo_time_surface/launch/stereo_time_surface.launch: -------------------------------------------------------------------------------- 1 | 2 | true 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 28 | 29 | -------------------------------------------------------------------------------- /esvo_time_surface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | esvo_time_surface 4 | 0.0.0 5 | The esvo_time_surface package. 6 | Yi Zhou 7 | 8 | 9 | 10 | 11 | GPLv3 12 | 13 | catkin 14 | catkin_simple 15 | 16 | roscpp 17 | eigen_catkin 18 | sensor_msgs 19 | cv_bridge 20 | image_transport 21 | glog_catkin 22 | gflags_catkin 23 | dvs_msgs 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /esvo_time_surface/src/TimeSurface_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char* argv[]) 4 | { 5 | ros::init(argc, argv, "esvo_time_surface"); 6 | 7 | ros::NodeHandle nh; 8 | ros::NodeHandle nh_private("~"); 9 | 10 | esvo_time_surface::TimeSurface ts(nh, nh_private); 11 | 12 | ros::spin(); 13 | 14 | return 0; 15 | } 16 | -------------------------------------------------------------------------------- /events_repacking_helper/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(events_repacking_helper) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin_simple REQUIRED COMPONENTS 11 | rosbag 12 | roscpp 13 | rospy 14 | std_msgs 15 | ) 16 | 17 | catkin_simple() 18 | 19 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O0") 20 | 21 | # make the executable 22 | cs_add_executable(EventMessageEditor src/EventMessageEditor.cpp) 23 | target_link_libraries(EventMessageEditor ${catkin_LIBARIES}) -------------------------------------------------------------------------------- /events_repacking_helper/README.md: -------------------------------------------------------------------------------- 1 | # events_repacking_helper 2 | 3 | This package provides an example of preparing a bag file for ESVO. A bag file recorded by a stereo event camera (e.g., a pair of DAVIS sensors) typically consists of the following topics: 4 | 5 | -- /davis/left/events 6 | -- /davis/right/events 7 | 8 | -- /davis/left/imu 9 | -- /davis/right/imu 10 | 11 | -- /davis/left/camera_info 12 | -- /davis/right/camera_info 13 | 14 | -- /davis/left/image_raw (optional for visualization) 15 | -- /davis/right/image_raw (optional for visualization) 16 | 17 | ## Preparation 18 | 19 | ### 1. Extract event messages from the original bag, and change the streaming rate to 1000 Hz. 20 | 21 | Set the input and output paths as arguments in the file `repacking.launch`, and then run 22 | 23 | `$ roslaunch events_repacking_helper repacking.launch` 24 | 25 | This command will return a bag file (e.g., output.bag.events) which only contains the re-packed stereo event messages. 26 | 27 | ### 2. Extract other needed topics from the original bag using [srv_tools](https://github.com/srv/srv_tools). 28 | 29 | `$ cd path_to_/srv_tools/bag_tools/scripts` 30 | 31 | `$ python extract_topics.py source_bag_name output_bag_name [topic names]` 32 | 33 | This command will return a bag file (e.g., output.bag.extracted) which contains other necessary topics except for events. 34 | 35 | ### 3. Merge above output bags. 36 | 37 | `$ python merge.py output.bag.events output.bag.extracted --output output_bag_name` 38 | 39 | ### 4. Remove hot pixels using [DVS_Hot_Pixel_Filter](https://github.com/cedric-scheerlinck/dvs_tools/tree/master/dvs_hot_pixel_filter) (optional). 40 | 41 | `$ rosrun dvs_hot_pixel_filter hot_pixel_filter path_to_input.bag` 42 | 43 | This command will generate a bag file named xxx.bag.filtered, which is good to feed to ESVO. 44 | -------------------------------------------------------------------------------- /events_repacking_helper/launch/repacking.launch: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /events_repacking_helper/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | events_repacking_helper 4 | 0.0.0 5 | The events_repacking_helper package 6 | 7 | 8 | zhouyi 9 | 10 | 11 | 12 | 13 | GPLv3 14 | 15 | catkin 16 | rosbag 17 | roscpp 18 | rospy 19 | std_msgs 20 | dvs_msgs 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /events_repacking_helper/src/EventMessageEditor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | struct EventMessageEditor 9 | { 10 | EventMessageEditor( 11 | double frequency, 12 | std::string & messageTopic) 13 | :bFirstMessage_(true) 14 | { 15 | eArray_ = dvs_msgs::EventArray(); 16 | duration_threshold_ = 1 / frequency; 17 | message_topic_ = messageTopic; 18 | } 19 | 20 | void resetBuffer(ros::Time startTimeStamp) 21 | { 22 | start_time_ = startTimeStamp; 23 | end_time_ = ros::Time(start_time_.toSec() + duration_threshold_); 24 | eArray_.events.clear(); 25 | eArray_.header.stamp = end_time_; 26 | } 27 | 28 | void resetArraySize(size_t width, size_t height) 29 | { 30 | eArray_.width = width; 31 | eArray_.height = height; 32 | } 33 | 34 | void insertEvent( 35 | dvs_msgs::Event & e, 36 | rosbag::Bag* bag) 37 | { 38 | if(bFirstMessage_) 39 | { 40 | resetBuffer(e.ts); 41 | bFirstMessage_ = false; 42 | } 43 | 44 | if(e.ts.toSec() >= end_time_.toSec()) 45 | { 46 | bag->write(message_topic_.c_str(), eArray_.header.stamp, eArray_); 47 | resetBuffer(end_time_); 48 | } 49 | eArray_.events.push_back(e); 50 | } 51 | 52 | // variables 53 | dvs_msgs::EventArray eArray_; 54 | double duration_threshold_; 55 | ros::Time start_time_, end_time_; 56 | bool bFirstMessage_; 57 | std::string message_topic_; 58 | }; 59 | 60 | 61 | int main(int argc, char* argv[]) 62 | { 63 | ros::init(argc, argv, "event_message_editor"); 64 | 65 | std::string src_bag_path(argv[1]); 66 | std::string dst_bag_path(argv[2]); 67 | rosbag::Bag bag_src, bag_dst; 68 | bag_src.open(src_bag_path.c_str(), rosbag::bagmode::Read); 69 | bag_dst.open(dst_bag_path.c_str(), rosbag::bagmode::Write); 70 | 71 | if(!bag_src.isOpen()) 72 | { 73 | ROS_INFO("No rosbag is found in the give path."); 74 | exit(-1); 75 | } 76 | else 77 | { 78 | ROS_INFO("***********Input Bag File Name ***********"); 79 | ROS_INFO(argv[1]); 80 | ROS_INFO("******************************************"); 81 | } 82 | 83 | if(!bag_dst.isOpen()) 84 | { 85 | ROS_INFO("The dst bag is not opened."); 86 | exit(-1); 87 | } 88 | else 89 | { 90 | ROS_INFO("***********Output Bag File Name ***********"); 91 | ROS_INFO(argv[2]); 92 | ROS_INFO("***************************"); 93 | } 94 | 95 | const double frequency = 1000; 96 | 97 | // process events 98 | std::vector topics; 99 | topics.push_back(std::string("/davis_left/events")); 100 | topics.push_back(std::string("/davis_right/events")); 101 | std::vector topics_rename; 102 | topics_rename.push_back(std::string("/davis/left/events")); 103 | topics_rename.push_back(std::string("/davis/right/events")); 104 | for(size_t i = 0;i < topics.size(); i++) 105 | { 106 | rosbag::View view(bag_src, rosbag::TopicQuery(topics[i])); 107 | EventMessageEditor eArrayEditor(frequency, topics_rename[i]); 108 | 109 | // topic loop 110 | for(rosbag::MessageInstance const m: view) 111 | { 112 | dvs_msgs::EventArray::ConstPtr msg = m.instantiate(); 113 | eArrayEditor.resetArraySize(msg->width, msg->height); 114 | // message loop 115 | for(const dvs_msgs::Event& e : msg->events) 116 | { 117 | eArrayEditor.insertEvent(const_cast(e), &bag_dst); 118 | } 119 | } 120 | } 121 | 122 | bag_src.close(); 123 | bag_dst.close(); 124 | return 0; 125 | } 126 | 127 | 128 | -------------------------------------------------------------------------------- /stereo_rig_design/Stereo_Rig_Base.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKUST-Aerial-Robotics/ESVO/1639a43ca3c5411b73a083c654f1aef73d8e0319/stereo_rig_design/Stereo_Rig_Base.pdf --------------------------------------------------------------------------------