├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── Config_Scene_01.yaml ├── Config_Scene_02.yaml ├── Config_Scene_03.yaml ├── Config_Scene_04.yaml ├── Config_Scene_05.yaml ├── Config_Scene_06.yaml ├── Config_Scene_07.yaml ├── Config_Scene_08.yaml ├── Config_Scene_09.yaml └── Config_Scene_10.yaml ├── cover.png ├── img ├── Scene_01_map.png ├── Scene_01_preview.png ├── Scene_02_map.png ├── Scene_02_preview.png ├── Scene_03_map.png ├── Scene_03_preview.png ├── Scene_04_map.png ├── Scene_04_preview.png ├── Scene_05_map.png ├── Scene_05_preview.png ├── Scene_06_map.png ├── Scene_06_preview.png ├── Scene_07_map.png ├── Scene_07_preview.png ├── Scene_08_map.png ├── Scene_08_preview.png ├── Scene_09_map.png ├── Scene_09_preview.png ├── Scene_10_map.png ├── Scene_10_preview.png └── System_Framework.png ├── include ├── core │ ├── CommonLib.h │ ├── IMU.h │ ├── LandMark.h │ ├── Mat.h │ ├── NightVoyagerOptions.h │ ├── Pose.h │ ├── Posev.h │ ├── Type.h │ └── Vec.h ├── feature_tracker │ ├── Feature.h │ ├── FeatureDatabase.h │ ├── FeatureHelper.h │ ├── FeatureInitializer.h │ ├── Grider_FAST.h │ ├── Grider_GRID.h │ └── TrackKLT.h ├── initializer │ ├── InertialInitializer.h │ ├── InitializerHelper.h │ └── StaticInitializer.h ├── msckf_iekf │ ├── Propagator.h │ ├── State.h │ ├── StateHelper.h │ ├── UpdaterHelper.h │ ├── UpdaterMAP.h │ ├── UpdaterMSCKF.h │ ├── UpdaterOdom.h │ ├── UpdaterPlane.h │ ├── UpdaterSLAM.h │ ├── UpdaterZeroVelocity.h │ └── VILManager.h ├── prior_pose │ └── PriorPoseManager.h ├── streetlight_matcher │ ├── BIMatcher.h │ ├── DLMatcher.h │ ├── HungaryEstimator.h │ ├── PcdManager.h │ ├── StreetlightFeature.h │ ├── StreetlightFeatureDatabase.h │ └── StreetlightMatcher.h ├── third_party │ └── p3p_solver.h ├── tracking_recover │ └── TrackingRecover.h ├── utils │ ├── Print.h │ └── Transform.h └── visualizer │ ├── CameraPoseVisualization.h │ └── Visualizer.h ├── launch └── night_voyager.launch ├── msg ├── BoundingBox.msg └── BoundingBoxes.msg ├── package.xml ├── rviz └── Night_Voyager.rviz ├── src ├── feature_tracker │ ├── Feature.cpp │ ├── FeatureDatabase.cpp │ ├── FeatureInitializer.cpp │ └── TrackKLT.cpp ├── initializer │ ├── InertialInitializer.cpp │ └── StaticInitializer.cpp ├── main.cpp ├── msckf_iekf │ ├── Propagator.cpp │ ├── StateHelper.cpp │ ├── UpdaterHelper.cpp │ ├── UpdaterMAP.cpp │ ├── UpdaterMSCKF.cpp │ ├── UpdaterOdom.cpp │ ├── UpdaterPlane.cpp │ ├── UpdaterSLAM.cpp │ ├── UpdaterZeroVelocity.cpp │ └── VILManager.cpp ├── streetlight_matcher │ ├── BIMatcher.cpp │ ├── DLMatcher.cpp │ ├── HungaryEstimator.cpp │ └── StreetlightFeatureDatabase.cpp ├── third_party │ └── p3p_solver.cpp ├── tracking_recover │ └── TrackingRecover.cpp ├── utils │ └── Print.cpp └── visualizer │ ├── CameraPoseVisualization.cpp │ └── Visualizer.cpp ├── supp └── Supplementary_Material.pdf └── thirdparty └── FindTBB.cmake /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(night_voyager) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | set(CMAKE_CXX_STANDARD 14) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | set(CMAKE_CXX_EXTENSIONS OFF) 10 | 11 | list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/thirdparty") 12 | 13 | add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") 14 | # Enable compile optimizations 15 | # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") 16 | 17 | # # Enable debug flags (use if you want to debug in gdb) 18 | # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -Wall -Wuninitialized -fno-omit-frame-pointer") 19 | set(CMAKE_BUILD_TYPE Release) 20 | 21 | set(CMAKE_CXX_FLAGS_DEBUG "-g3 -O0 -Wall -Wextra -fno-omit-frame-pointer") 22 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -Wextra -fomit-frame-pointer -DNDEBUG") 23 | 24 | # if(NOT CMAKE_BUILD_TYPE) 25 | # set(CMAKE_BUILD_TYPE Debug) 26 | # endif() 27 | 28 | 29 | ## Find catkin macros and libraries 30 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 31 | ## is used, also find other catkin packages 32 | find_package(catkin REQUIRED COMPONENTS 33 | cv_bridge 34 | geometry_msgs 35 | nav_msgs 36 | pcl_ros 37 | roscpp 38 | tf 39 | rospy 40 | sensor_msgs 41 | std_msgs 42 | image_transport 43 | visualization_msgs 44 | message_generation 45 | ) 46 | 47 | find_package(Eigen3 REQUIRED) 48 | find_package(PCL 1.8 REQUIRED) 49 | find_package(OpenCV 3.2.0 REQUIRED) 50 | find_package(TBB REQUIRED) 51 | find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) 52 | find_package(catkin QUIET COMPONENTS roscpp) 53 | message(STATUS "OPENCV: " ${OpenCV_VERSION} " | BOOST: " ${Boost_VERSION}) 54 | 55 | ## Generate messages in the 'msg' folder 56 | add_message_files( 57 | FILES 58 | BoundingBox.msg 59 | BoundingBoxes.msg 60 | ) 61 | 62 | ## Generate added messages and services with any dependencies listed here 63 | generate_messages( 64 | DEPENDENCIES 65 | std_msgs 66 | sensor_msgs 67 | nav_msgs 68 | geometry_msgs 69 | ) 70 | 71 | catkin_package( 72 | # INCLUDE_DIRS include 73 | # LIBRARIES Night_Voyager 74 | CATKIN_DEPENDS message_runtime roscpp rospy sensor_msgs std_msgs nav_msgs geometry_msgs image_transport visualization_msgs tf cv_bridge 75 | DEPENDS EIGEN3 PCL 76 | ) 77 | 78 | ########### 79 | ## Build ## 80 | ########### 81 | 82 | ## Specify additional locations of header files 83 | ## Your package locations should be listed before other locations 84 | file(GLOB_RECURSE INCLUDE_DIRS "${CMAKE_CURRENT_SOURCE_DIR}/include") 85 | include_directories( 86 | include 87 | ${catkin_INCLUDE_DIRS} 88 | ${OpenCV_INCLUDE_DIRS} 89 | ${EIGEN3_INCLUDE_DIR} 90 | ${PCL_INCLUDE_DIRS} 91 | ${INCLUDE_DIRS} 92 | ${TBB_INCLUDE_DIRS} 93 | ${tf_INCLUDE_DIRS} 94 | ) 95 | 96 | # list(APPEND thirdparty_libraries 97 | # ${catkin_LIBRARIES} 98 | # ${OpenCV_LIBRARIES} 99 | # ${Boost_LIBRARIES} 100 | # ${PCL_LIBRARIES} 101 | # ) 102 | 103 | file(GLOB_RECURSE NIGHT_VOYAGER_SRCS "${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp") 104 | 105 | list(FILTER NIGHT_VOYAGER_SRCS EXCLUDE REGEX ".*main\\.cpp$") 106 | 107 | # add_library(${PROJECT_NAME} SHARED ${NIGHT_VOYAGER_LIBRARIES}) 108 | # target_link_libraries( 109 | # ${PROJECT_NAME} 110 | # ${catkin_LIBRARIES} 111 | # ${OpenCV_LIBRARIES} 112 | # ${Boost_LIBRARIES} 113 | # ${PCL_LIBRARIES} 114 | # ${cv_bridge_LIBRARIES} 115 | # ${image_transport_LIBRARIES} 116 | # ${TBB_LIBRARIES} 117 | # ${tf_LIBRARIES} 118 | # ) 119 | # target_include_directories(${PROJECT_NAME} PUBLIC include) 120 | 121 | add_executable(night_voyager_node src/main.cpp ${NIGHT_VOYAGER_SRCS}) 122 | add_dependencies(night_voyager_node ${PROJECT_NAME}_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) 123 | target_link_libraries(night_voyager_node ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${Boost_LIBRARIES} ${PCL_LIBRARIES} 124 | ${cv_bridge_LIBRARIES} ${image_transport_LIBRARIES} ${TBB_LIBRARIES} ${tf_LIBRARIES}) 125 | 126 | -------------------------------------------------------------------------------- /config/Config_Scene_01.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | img_topic: /camera/color/image_raw/compressed 3 | box_topic: /yolov7_bbox 4 | odom_topic: /odom 5 | imu_topic: /livox/imu 6 | 7 | camera: 8 | cam_fx: 910.777 9 | cam_fy: 910.656 10 | cam_cx: 639.846 11 | cam_cy: 355.401 12 | res_x: 1280 13 | res_y: 720 14 | distortion: [0, 0, 0, 0] 15 | dist_model: radtan 16 | 17 | imu: 18 | gyro_std: [1.6912455461587871e-03, 1.2118225332973804e-03, 1.6146741327812619e-03] 19 | accel_std: [7.2510837330793689e-04, 8.8071690484254771e-04, 1.3471694990898537e-03] 20 | accel_bias_std: [3.0608877651619066e-05, 2.0168661087254066e-05, 3.7608575535597191e-05] 21 | gyro_bias_std: [8.1307125981080534e-05, 4.7064776326160538e-05, 9.4599609702988706e-05] 22 | Rci: [-0.134582, -0.990831, -0.0119257, 23 | -0.0427573, 0.0178307, -0.998926, 24 | 0.98998, -0.133927, -0.0447649] 25 | pci: [0.008065706, -0.013901938, -0.044590552] 26 | Roi: [0.98998, -0.133927, -0.0447649, 27 | 0.134582, 0.990831, 0.0119257, 28 | 0.0427573,-0.0178307, 0.998926] 29 | accel_norm: 9.81 30 | gravity_mag: 9.81 31 | 32 | odom: 33 | vel_std: [0.01, 0.01, 0.01] 34 | 35 | state: 36 | which_group: 2 # -2: msckf, -1: no group, 0: features are grouped in IMU state, 1: features are grouped in relative transform, 37 | # 2: features are grouped between clone state and relative transform 38 | max_clone_size: 11 39 | max_slam_features: 50 40 | max_slam_in_update: 25 41 | max_msckf_in_update: 1000 42 | 43 | update: 44 | do_update_msckf: true 45 | do_update_slam: true 46 | do_update_map: true 47 | do_update_odom: true 48 | do_update_plane: true 49 | use_virtual_center: true 50 | use_match_extension: true 51 | msckf_iekf_sigma_px: 4 52 | msckf_chi2_multipler: 1 53 | slam_sigma_px: 4 54 | slam_chi2_multipler: 1 55 | zupt_chi2_multipler: 1 56 | map_sigma_px: 1.5 57 | map_chi2_multipler_dl: 8 58 | map_chi2_multipler_bi: 8 59 | odom_chi2_multipler: 5 60 | plane_chi2_multipler_loc: 1.0 61 | plane_chi2_multipler_prior: 1.0 62 | plane_distance_weight_loc: 0.02 63 | plane_distance_weight_prior: 0.02 64 | plane_distance_threshold_loc: 1.5 65 | plane_distance_threshold_prior: 5.0 66 | 67 | match: 68 | z_th: 55 69 | update_z_th: 75 70 | extend: 20 71 | large_extend: 20 72 | grey_th: 248 73 | grey_th_low: 235 74 | alpha: 0.2 75 | ang_th_dl: 0.995 76 | ang_th_bi: 0.9998 77 | large_off: 5 78 | dl_area: 40 79 | dl_filter: true 80 | bi_filter: true 81 | remain_match: false 82 | 83 | 84 | init: 85 | viwo_init: 86 | init_window_time: 1.0 87 | init_imu_thresh: 1.0 88 | gravity_mag: 9.81 89 | init_max_features: 50 90 | map_init: 91 | search_dist_scope1: 75 92 | search_dist_scope2: 75 93 | sine_th: 0.05 94 | init_use_binary: true 95 | init_grey_threshold: 252 96 | init_grey_threshold_low: 230 97 | outlier_score: 10 98 | max_st_dist: 65 99 | preset_outliers: 0 100 | top_tuples_in_region: 100 101 | top_poses: 25 102 | expansion_proj: 20 103 | subscore_weight: 3 104 | prior_init_available: false 105 | prior_scale: 10.0 106 | prior_init: [0.0, 0.0, 0.0] 107 | 108 | feat_init: 109 | triangulate_1d: false 110 | refine_features: true 111 | max_runs: 5 112 | init_lamda: 1e-3 113 | max_lamda: 1e10 114 | min_dx: 1e-6 115 | min_dcost: 1e-6 116 | lam_mult: 10 117 | min_dist: 0.25 118 | max_dist: 75 119 | max_baseline: 200 120 | max_cond_number: 20000 121 | 122 | tracker: 123 | histogram_method: 1 # 0: NONE, 1: HISTOGRAM, 2: CLAHE 124 | num_features: 150 125 | fast_threshold: 20 126 | grid_x: 32 127 | grid_y: 18 128 | min_px_dist: 8 129 | pyr_levels: 5 130 | win_size: 15 131 | 132 | zupt: 133 | try_zupt: true 134 | zupt_max_velocity: 0.2 135 | zupt_noise_multiplier: 10 136 | zupt_max_disparity: 0.5 137 | zupt_max_velocity_odom: 0.05 138 | zupt_only_at_beginning: false 139 | 140 | tracking_recover: 141 | dist_th: 7 142 | ang_th: 0.35 143 | lost_dist_th: 2 144 | lost_ang_th: 1.5 145 | z_th: 25 146 | extend: 20 147 | grey_thresh_low: 230 148 | expansion_proj: 40 149 | subscore_weight: 1 150 | variance: 64 151 | search_dist_scope: 45 152 | search_bar_height: 20 153 | area_th: 150 154 | expected_weight: 0.2 155 | outlier_cost: 0.5 156 | sigma_pix: 1 157 | chi2_multipler_dl_maylost: 50 158 | chi2_multipler_bi_maylost: 50 159 | chi2_multipler_dl_lost: 8 160 | chi2_multipler_bi_lost: 8 161 | prior_z_diff_th: 1 162 | 163 | save: 164 | save_total_state: true 165 | save_time_consume: true 166 | of_state_est: "log/state_estimation.txt" 167 | of_state_std: "log/state_deviation.txt" 168 | of_state_tum_loc: "log/state_tum_loc.txt" 169 | of_state_tum_global: "log/state_tum_global.txt" 170 | of_state_tracking_recover: "log/" -------------------------------------------------------------------------------- /config/Config_Scene_02.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | img_topic: /camera/color/image_raw/compressed 3 | box_topic: /yolov7_bbox 4 | odom_topic: /odom 5 | imu_topic: /livox/imu 6 | 7 | camera: 8 | cam_fx: 908.524 9 | cam_fy: 908.919 10 | cam_cx: 642.150 11 | cam_cy: 352.982 12 | res_x: 1280 13 | res_y: 720 14 | distortion: [0, 0, 0, 0] 15 | dist_model: radtan 16 | 17 | imu: 18 | gyro_std: [1.6912455461587871e-03, 1.2118225332973804e-03, 1.6146741327812619e-03] 19 | accel_std: [7.2510837330793689e-04, 8.8071690484254771e-04, 1.3471694990898537e-03] 20 | accel_bias_std: [3.0608877651619066e-05, 2.0168661087254066e-05, 3.7608575535597191e-05] 21 | gyro_bias_std: [8.1307125981080534e-05, 4.7064776326160538e-05, 9.4599609702988706e-05] 22 | Rci: [0.00208164, -0.999858, -0.0167461, 23 | 0.430466, 0.0160109, -0.902465, 24 | 0.902605, -0.00532965, 0.430438] 25 | pci: [0.071399057, -0.093224676, -0.035436303] 26 | Roi: [0.902605, -0.00532965, 0.430438, 27 | -0.00208164, 0.999858, 0.0167461, 28 | -0.430466, -0.0160109, 0.902465] 29 | accel_norm: 9.81 30 | gravity_mag: 9.81 31 | 32 | odom: 33 | vel_std: [0.01, 0.01, 0.01] 34 | 35 | state: 36 | which_group: 2 # -2: msckf, -1: no group, 0: features are grouped in IMU state, 1: features are grouped in relative transform, 37 | # 2: features are grouped between clone state and relative transform 38 | max_clone_size: 11 39 | max_slam_features: 50 40 | max_slam_in_update: 25 41 | max_msckf_in_update: 1000 42 | 43 | update: 44 | do_update_msckf: true 45 | do_update_slam: true 46 | do_update_map: true # true 47 | do_update_odom: true 48 | do_update_plane: true # true 49 | use_virtual_center: true 50 | use_match_extension: true 51 | msckf_iekf_sigma_px: 4 52 | msckf_chi2_multipler: 1 53 | slam_sigma_px: 4 54 | slam_chi2_multipler: 1 55 | zupt_chi2_multipler: 1 56 | map_sigma_px: 1.5 57 | map_chi2_multipler_dl: 8 58 | map_chi2_multipler_bi: 8 59 | odom_chi2_multipler: 5 60 | plane_chi2_multipler_loc: 1.0 61 | plane_chi2_multipler_prior: 1.0 62 | plane_distance_weight_loc: 0.02 63 | plane_distance_weight_prior: 0.02 64 | plane_distance_threshold_loc: 1.5 65 | plane_distance_threshold_prior: 5.0 66 | 67 | match: 68 | z_th: 55 69 | update_z_th: 75 70 | extend: 20 71 | grey_th: 248 72 | grey_th_low: 235 73 | alpha: 0.2 74 | ang_th_dl: 0.995 75 | ang_th_bi: 0.9998 76 | large_off: 5 77 | dl_area: 40 78 | dl_filter: true 79 | bi_filter: true 80 | remain_match: false 81 | 82 | init: 83 | viwo_init: 84 | init_window_time: 1.0 85 | init_imu_thresh: 1.0 86 | gravity_mag: 9.81 87 | init_max_features: 50 88 | map_init: 89 | search_dist_scope1: 75 90 | search_dist_scope2: 75 91 | sine_th: 0.5 92 | init_use_binary: true 93 | init_grey_threshold: 252 94 | init_grey_threshold_low: 230 95 | outlier_score: 10 96 | max_st_dist: 45 97 | preset_outliers: 0 98 | top_tuples_in_region: 100 99 | top_poses: 25 100 | expansion_proj: 20 101 | subscore_weight: 3 102 | prior_init_available: false 103 | prior_scale: 10.0 104 | prior_init: [0.0, 0.0, 0.0] 105 | 106 | feat_init: 107 | triangulate_1d: false 108 | refine_features: true 109 | max_runs: 5 110 | init_lamda: 1e-3 111 | max_lamda: 1e10 112 | min_dx: 1e-6 113 | min_dcost: 1e-6 114 | lam_mult: 10 115 | min_dist: 0.25 116 | max_dist: 75 117 | max_baseline: 200 118 | max_cond_number: 10000 # 10000 119 | 120 | tracker: 121 | histogram_method: 1 # 0: NONE, 1: HISTOGRAM, 2: CLAHE 122 | num_features: 150 123 | fast_threshold: 20 124 | grid_x: 8 125 | grid_y: 6 126 | min_px_dist: 8 127 | pyr_levels: 5 128 | win_size: 15 129 | 130 | zupt: 131 | try_zupt: true 132 | zupt_max_velocity: 0.2 133 | zupt_noise_multiplier: 10 134 | zupt_max_disparity: 0.5 135 | zupt_max_velocity_odom: 0.05 136 | zupt_only_at_beginning: false 137 | 138 | tracking_recover: 139 | dist_th: 30 140 | ang_th: 0.35 141 | lost_dist_th: 5 142 | lost_ang_th: 1.5 143 | z_th: 25 144 | extend: 20 145 | grey_thresh_low: 230 146 | expansion_proj: 40 147 | subscore_weight: 1 148 | variance: 64 149 | search_dist_scope: 45 150 | search_bar_height: 20 151 | area_th: 150 152 | expected_weight: 0.2 153 | outlier_cost: 0.5 154 | sigma_pix: 1 155 | chi2_multipler_dl_maylost: 50 156 | chi2_multipler_bi_maylost: 50 157 | chi2_multipler_dl_lost: 8 158 | chi2_multipler_bi_lost: 8 159 | prior_z_diff_th: 1 160 | 161 | save: 162 | save_total_state: true 163 | save_time_consume: true 164 | of_state_est: "log/state_estimation.txt" 165 | of_state_std: "log/state_deviation.txt" 166 | of_state_tum_loc: "log/state_tum_loc.txt" 167 | of_state_tum_global: "log/state_tum_global.txt" 168 | of_state_tracking_recover: "log/" -------------------------------------------------------------------------------- /config/Config_Scene_03.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | img_topic: /camera/color/image_raw/compressed 3 | box_topic: /yolov7_bbox 4 | odom_topic: /odom 5 | imu_topic: /livox/imu 6 | 7 | camera: 8 | cam_fx: 908.524 9 | cam_fy: 908.919 10 | cam_cx: 642.150 11 | cam_cy: 352.982 12 | res_x: 1280 13 | res_y: 720 14 | distortion: [0, 0, 0, 0] 15 | dist_model: radtan 16 | 17 | imu: 18 | gyro_std: [1.6912455461587871e-03, 1.2118225332973804e-03, 1.6146741327812619e-03] 19 | accel_std: [7.2510837330793689e-04, 8.8071690484254771e-04, 1.3471694990898537e-03] 20 | accel_bias_std: [3.0608877651619066e-05, 2.0168661087254066e-05, 3.7608575535597191e-05] 21 | gyro_bias_std: [8.1307125981080534e-05, 4.7064776326160538e-05, 9.4599609702988706e-05] 22 | Rci: [0.00208164, -0.999858, -0.0167461, 23 | 0.430466, 0.0160109, -0.902465, 24 | 0.902605, -0.00532965, 0.430438] 25 | pci: [0.071399057, -0.093224676, -0.035436303] 26 | Roi: [0.902605, -0.00532965, 0.430438, 27 | -0.00208164, 0.999858, 0.0167461, 28 | -0.430466, -0.0160109, 0.902465] 29 | accel_norm: 9.81 30 | gravity_mag: 9.81 31 | 32 | odom: 33 | vel_std: [0.01, 0.01, 0.01] 34 | 35 | state: 36 | which_group: 2 # -2: msckf, -1: no group, 0: features are grouped in IMU state, 1: features are grouped in relative transform, 37 | # 2: features are grouped between clone state and relative transform 38 | max_clone_size: 11 39 | max_slam_features: 50 40 | max_slam_in_update: 25 41 | max_msckf_in_update: 1000 42 | 43 | update: 44 | do_update_msckf: true 45 | do_update_slam: true 46 | do_update_map: true 47 | do_update_odom: true 48 | do_update_plane: true 49 | use_virtual_center: true 50 | use_match_extension: true 51 | msckf_iekf_sigma_px: 4 52 | msckf_chi2_multipler: 1 53 | slam_sigma_px: 4 54 | slam_chi2_multipler: 1 55 | zupt_chi2_multipler: 1 56 | map_sigma_px: 1.5 57 | map_chi2_multipler_dl: 8 58 | map_chi2_multipler_bi: 8 59 | odom_chi2_multipler: 5 60 | plane_chi2_multipler_loc: 1.0 61 | plane_chi2_multipler_prior: 1.0 62 | plane_distance_weight_loc: 0.02 63 | plane_distance_weight_prior: 0.02 64 | plane_distance_threshold_loc: 1.5 65 | plane_distance_threshold_prior: 5.0 66 | 67 | match: 68 | z_th: 55 69 | update_z_th: 75 70 | extend: 20 71 | # large_extend: 20 72 | grey_th: 248 73 | grey_th_low: 235 74 | alpha: 0.2 75 | ang_th_dl: 0.995 76 | ang_th_bi: 0.9998 77 | large_off: 5 78 | dl_area: 40 79 | dl_filter: true 80 | bi_filter: true 81 | remain_match: false 82 | 83 | init: 84 | viwo_init: 85 | init_window_time: 1.0 86 | init_imu_thresh: 1.0 87 | gravity_mag: 9.81 88 | init_max_features: 50 89 | map_init: 90 | search_dist_scope1: 75 91 | search_dist_scope2: 75 92 | sine_th: 0.05 93 | init_use_binary: true 94 | init_grey_threshold: 252 95 | init_grey_threshold_low: 230 96 | outlier_score: 10 97 | max_st_dist: 65 98 | preset_outliers: 0 99 | top_tuples_in_region: 100 100 | top_poses: 25 101 | expansion_proj: 20 102 | subscore_weight: 3 103 | prior_init_available: false 104 | prior_scale: 10.0 105 | prior_init: [0.0, 0.0, 0.0] 106 | 107 | feat_init: 108 | triangulate_1d: false 109 | refine_features: true 110 | max_runs: 5 111 | init_lamda: 1e-3 112 | max_lamda: 1e10 113 | min_dx: 1e-6 114 | min_dcost: 1e-6 115 | lam_mult: 10 116 | min_dist: 0.25 117 | max_dist: 75 118 | max_baseline: 200 119 | max_cond_number: 20000 120 | 121 | tracker: 122 | histogram_method: 1 # 0: NONE, 1: HISTOGRAM, 2: CLAHE 123 | num_features: 150 124 | fast_threshold: 20 125 | grid_x: 32 126 | grid_y: 18 127 | min_px_dist: 8 128 | pyr_levels: 5 129 | win_size: 15 130 | 131 | zupt: 132 | try_zupt: true 133 | zupt_max_velocity: 0.2 134 | zupt_noise_multiplier: 10 135 | zupt_max_disparity: 0.5 136 | zupt_max_velocity_odom: 0.05 137 | zupt_only_at_beginning: false 138 | 139 | tracking_recover: 140 | dist_th: 20 141 | ang_th: 0.35 142 | lost_dist_th: 3 143 | lost_ang_th: 1.5 144 | z_th: 25 145 | extend: 20 146 | grey_thresh_low: 230 147 | expansion_proj: 40 148 | subscore_weight: 1 149 | variance: 64 150 | search_dist_scope: 45 151 | search_bar_height: 20 152 | area_th: 150 153 | expected_weight: 0.2 154 | outlier_cost: 0.5 155 | sigma_pix: 1 156 | chi2_multipler_dl_maylost: 50 #100 for PP VC, 25 FP 157 | chi2_multipler_bi_maylost: 50 #100 for PP VC, 25 FP 158 | chi2_multipler_dl_lost: 8 159 | chi2_multipler_bi_lost: 8 160 | prior_z_diff_th: 1 161 | 162 | save: 163 | save_total_state: true 164 | save_time_consume: true 165 | of_state_est: "log/state_estimation.txt" 166 | of_state_std: "log/state_deviation.txt" 167 | of_state_tum_loc: "log/state_tum_loc.txt" 168 | of_state_tum_global: "log/state_tum_global.txt" 169 | of_state_tracking_recover: "log/" -------------------------------------------------------------------------------- /config/Config_Scene_04.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | img_topic: /camera/color/image_raw/compressed 3 | box_topic: /yolov7_bbox 4 | odom_topic: /odom 5 | imu_topic: /livox/imu 6 | 7 | camera: 8 | cam_fx: 908.524 9 | cam_fy: 908.919 10 | cam_cx: 642.150 11 | cam_cy: 352.982 12 | res_x: 1280 13 | res_y: 720 14 | distortion: [0, 0, 0, 0] 15 | dist_model: radtan 16 | 17 | imu: 18 | gyro_std: [1.6912455461587871e-03, 1.2118225332973804e-03, 1.6146741327812619e-03] 19 | accel_std: [7.2510837330793689e-04, 8.8071690484254771e-04, 1.3471694990898537e-03] 20 | accel_bias_std: [3.0608877651619066e-05, 2.0168661087254066e-05, 3.7608575535597191e-05] 21 | gyro_bias_std: [8.1307125981080534e-05, 4.7064776326160538e-05, 9.4599609702988706e-05] 22 | Rci: [0.00208164, -0.999858, -0.0167461, 23 | 0.430466, 0.0160109, -0.902465, 24 | 0.902605, -0.00532965, 0.430438] 25 | pci: [0.071399057, -0.093224676, -0.035436303] 26 | Roi: [0.902605, -0.00532965, 0.430438, 27 | -0.00208164, 0.999858, 0.0167461, 28 | -0.430466, -0.0160109, 0.902465] 29 | accel_norm: 9.81 30 | gravity_mag: 9.81 31 | 32 | odom: 33 | vel_std: [0.01, 0.01, 0.01] 34 | 35 | state: 36 | which_group: 2 # -2: msckf, -1: no group, 0: features are grouped in IMU state, 1: features are grouped in relative transform, 37 | # 2: features are grouped between clone state and relative transform 38 | max_clone_size: 11 39 | max_slam_features: 50 40 | max_slam_in_update: 25 41 | max_msckf_in_update: 1000 42 | feat_rep_msckf: 0 43 | feat_rep_slam: 0 44 | 45 | update: 46 | do_update_msckf: true 47 | do_update_slam: true 48 | do_update_map: true 49 | do_update_odom: true 50 | do_update_plane: true 51 | use_virtual_center: true 52 | use_match_extension: true 53 | msckf_iekf_sigma_px: 4 54 | msckf_chi2_multipler: 1 55 | slam_sigma_px: 4 56 | slam_chi2_multipler: 1 57 | zupt_chi2_multipler: 1 58 | map_sigma_px: 1.5 59 | map_chi2_multipler_dl: 8 60 | map_chi2_multipler_bi: 8 61 | odom_chi2_multipler: 5 62 | plane_chi2_multipler_loc: 1.0 63 | plane_chi2_multipler_prior: 1.0 64 | plane_distance_weight_loc: 0.02 65 | plane_distance_weight_prior: 0.02 66 | plane_distance_threshold_loc: 1.5 67 | plane_distance_threshold_prior: 5.0 68 | 69 | match: 70 | z_th: 55 71 | update_z_th: 75 72 | extend: 20 73 | large_extend: 20 74 | grey_th: 248 75 | grey_th_low: 235 76 | alpha: 0.2 # 0.2 77 | ang_th_dl: 0.995 78 | ang_th_bi: 0.9998 79 | large_off: 10 80 | dl_area: 40 81 | dl_filter: true 82 | bi_filter: true 83 | remain_match: true 84 | 85 | init: 86 | viwo_init: 87 | init_window_time: 1.0 88 | init_imu_thresh: 1.0 89 | gravity_mag: 9.81 90 | init_max_features: 50 91 | map_init: 92 | search_dist_scope1: 75 93 | search_dist_scope2: 75 94 | sine_th: 0.05 95 | init_use_binary: true 96 | init_grey_threshold: 252 97 | init_grey_threshold_low: 230 98 | outlier_score: 10 99 | max_st_dist: 65 100 | preset_outliers: 0 101 | top_tuples_in_region: 100 102 | top_poses: 25 103 | expansion_proj: 20 104 | subscore_weight: 1 105 | prior_init_available: false 106 | prior_scale: 10.0 107 | prior_init: [0.0, 0.0, 0.0] 108 | 109 | feat_init: 110 | triangulate_1d: false 111 | refine_features: true 112 | max_runs: 5 113 | init_lamda: 1e-3 114 | max_lamda: 1e10 115 | min_dx: 1e-6 116 | min_dcost: 1e-6 117 | lam_mult: 10 118 | min_dist: 0.25 119 | max_dist: 75 120 | max_baseline: 200 121 | max_cond_number: 20000 122 | 123 | tracker: 124 | histogram_method: 1 # 0: NONE, 1: HISTOGRAM, 2: CLAHE 125 | num_features: 150 126 | fast_threshold: 20 127 | grid_x: 32 128 | grid_y: 18 129 | min_px_dist: 8 130 | pyr_levels: 5 131 | win_size: 15 132 | 133 | zupt: 134 | try_zupt: true 135 | zupt_max_velocity: 0.2 136 | zupt_noise_multiplier: 10 137 | zupt_max_disparity: 0.5 138 | zupt_max_velocity_odom: 0.05 139 | zupt_only_at_beginning: false 140 | 141 | tracking_recover: 142 | dist_th: 30 # 30 for tracking recovery 143 | ang_th: 0.35 144 | lost_dist_th: 1 145 | lost_ang_th: 0.5 146 | z_th: 15 147 | extend: 20 148 | grey_thresh_low: 230 149 | expansion_proj: 40 150 | subscore_weight: 1 151 | variance: 64 152 | search_dist_scope: 45 153 | search_bar_height: 20 154 | area_th: 150 155 | expected_weight: 0.2 156 | outlier_cost: 0.5 157 | sigma_pix: 1 158 | chi2_multipler_dl_maylost: 50 159 | chi2_multipler_bi_maylost: 50 160 | chi2_multipler_dl_lost: 8 161 | chi2_multipler_bi_lost: 8 162 | prior_z_diff_th: 1 163 | 164 | save: 165 | save_total_state: true 166 | save_time_consume: true 167 | of_state_est: "log/state_estimation.txt" 168 | of_state_std: "log/state_deviation.txt" 169 | of_state_tum_loc: "log/state_tum_loc.txt" 170 | of_state_tum_global: "log/state_tum_global.txt" 171 | of_state_tracking_recover: "log/" -------------------------------------------------------------------------------- /config/Config_Scene_05.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | img_topic: /camera/color/image_raw/compressed 3 | box_topic: /yolov7_bbox 4 | odom_topic: /odom 5 | imu_topic: /livox/imu 6 | 7 | camera: 8 | cam_fx: 910.777 9 | cam_fy: 910.656 10 | cam_cx: 639.846 11 | cam_cy: 355.401 12 | res_x: 1280 13 | res_y: 720 14 | distortion: [0, 0, 0, 0] 15 | dist_model: radtan 16 | 17 | imu: 18 | gyro_std: [1.6912455461587871e-03, 1.2118225332973804e-03, 1.6146741327812619e-03] 19 | accel_std: [7.2510837330793689e-04, 8.8071690484254771e-04, 1.3471694990898537e-03] 20 | accel_bias_std: [3.0608877651619066e-05, 2.0168661087254066e-05, 3.7608575535597191e-05] 21 | gyro_bias_std: [8.1307125981080534e-05, 4.7064776326160538e-05, 9.4599609702988706e-05] 22 | Rci: [-0.134582, -0.990831, -0.0119257, 23 | -0.0427573, 0.0178307, -0.998926, 24 | 0.98998, -0.133927, -0.0447649] 25 | pci: [0.008065706, -0.013901938, -0.044590552] 26 | Roi: [0.98998, -0.133927, -0.0447649, 27 | 0.134582, 0.990831, 0.0119257, 28 | 0.0427573,-0.0178307, 0.998926] 29 | accel_norm: 9.81 30 | gravity_mag: 9.81 31 | 32 | odom: 33 | vel_std: [0.01, 0.01, 0.01] 34 | 35 | state: 36 | which_group: 2 # -2: msckf, -1: no group, 0: features are grouped in IMU state, 1: features are grouped in relative transform, 37 | # 2: features are grouped between clone state and relative transform 38 | max_clone_size: 11 39 | max_slam_features: 50 40 | max_slam_in_update: 25 41 | max_msckf_in_update: 1000 42 | 43 | update: 44 | do_update_msckf: true 45 | do_update_slam: true 46 | do_update_map: true 47 | do_update_odom: true 48 | do_update_plane: true 49 | use_virtual_center: true 50 | use_match_extension: true 51 | msckf_iekf_sigma_px: 4 52 | msckf_chi2_multipler: 1 53 | slam_sigma_px: 4 54 | slam_chi2_multipler: 1 55 | zupt_chi2_multipler: 1 56 | map_sigma_px: 1.5 57 | map_chi2_multipler_dl: 8 58 | map_chi2_multipler_bi: 8 59 | odom_chi2_multipler: 5 60 | plane_chi2_multipler_loc: 1.0 61 | plane_chi2_multipler_prior: 1.0 62 | plane_distance_weight_loc: 0.02 63 | plane_distance_weight_prior: 0.02 64 | plane_distance_threshold_loc: 1.5 65 | plane_distance_threshold_prior: 5.0 66 | 67 | init: 68 | viwo_init: 69 | init_window_time: 1.0 70 | init_imu_thresh: 1.0 71 | gravity_mag: 9.81 72 | init_max_features: 50 73 | map_init: 74 | search_dist_scope1: 75 75 | search_dist_scope2: 75 76 | sine_th: 0.05 77 | init_use_binary: true 78 | init_grey_threshold: 252 79 | init_grey_threshold_low: 230 80 | outlier_score: 10 81 | max_st_dist: 65 82 | preset_outliers: 1 83 | top_tuples_in_region: 100 84 | top_poses: 75 85 | expansion_proj: 20 86 | subscore_weight: 5 87 | prior_init_available: false 88 | prior_scale: 10.0 89 | prior_init: [0.0, 0.0, 0.0] 90 | 91 | feat_init: 92 | triangulate_1d: false 93 | refine_features: true 94 | max_runs: 5 95 | init_lamda: 1e-3 96 | max_lamda: 1e10 97 | min_dx: 1e-6 98 | min_dcost: 1e-6 99 | lam_mult: 10 100 | min_dist: 0.25 101 | max_dist: 75 102 | max_baseline: 200 103 | max_cond_number: 20000 104 | 105 | tracker: 106 | histogram_method: 1 # 0: NONE, 1: HISTOGRAM, 2: CLAHE 107 | num_features: 150 108 | fast_threshold: 20 109 | grid_x: 32 110 | grid_y: 18 111 | min_px_dist: 8 112 | pyr_levels: 5 113 | win_size: 15 114 | 115 | match: 116 | z_th: 55 117 | update_z_th: 75 118 | extend: 20 119 | large_extend: 30 120 | grey_th: 248 121 | grey_th_low: 235 122 | alpha: 0.2 123 | ang_th_dl: 0.995 124 | ang_th_bi: 0.9998 125 | large_off: 5 126 | dl_filter: true 127 | bi_filter: true 128 | remain_match: false 129 | 130 | zupt: 131 | try_zupt: true 132 | zupt_max_velocity: 0.2 133 | zupt_noise_multiplier: 10 134 | zupt_max_disparity: 0.5 135 | zupt_max_velocity_odom: 0.05 136 | zupt_only_at_beginning: false 137 | 138 | tracking_recover: 139 | dist_th: 20 140 | ang_th: 0.35 141 | lost_dist_th: 2 142 | lost_ang_th: 1.5 143 | z_th: 25 144 | extend: 20 145 | grey_thresh_low: 230 146 | expansion_proj: 40 147 | subscore_weight: 1 148 | variance: 64 149 | search_dist_scope: 45 150 | search_bar_height: 20 151 | area_th: 150 152 | expected_weight: 0.2 153 | outlier_cost: 0.5 154 | sigma_pix: 1 155 | chi2_multipler_dl_maylost: 50 156 | chi2_multipler_bi_maylost: 50 157 | chi2_multipler_dl_lost: 8 158 | chi2_multipler_bi_lost: 8 159 | prior_z_diff_th: 1 160 | 161 | save: 162 | save_total_state: true 163 | save_time_consume: true 164 | of_state_est: "log/state_estimation.txt" 165 | of_state_std: "log/state_deviation.txt" 166 | of_state_tum_loc: "log/state_tum_loc.txt" 167 | of_state_tum_global: "log/state_tum_global.txt" 168 | of_state_tracking_recover: "log/" -------------------------------------------------------------------------------- /config/Config_Scene_06.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | img_topic: /camera/color/image_raw/compressed 3 | box_topic: /yolov7_bbox 4 | odom_topic: /odom 5 | imu_topic: /livox/imu 6 | 7 | camera: 8 | cam_fx: 910.777 9 | cam_fy: 910.656 10 | cam_cx: 639.846 11 | cam_cy: 355.401 12 | res_x: 1280 13 | res_y: 720 14 | distortion: [0, 0, 0, 0] 15 | dist_model: radtan 16 | 17 | imu: 18 | gyro_std: [1.6912455461587871e-03, 1.2118225332973804e-03, 1.6146741327812619e-03] 19 | accel_std: [7.2510837330793689e-04, 8.8071690484254771e-04, 1.3471694990898537e-03] 20 | accel_bias_std: [3.0608877651619066e-05, 2.0168661087254066e-05, 3.7608575535597191e-05] 21 | gyro_bias_std: [8.1307125981080534e-05, 4.7064776326160538e-05, 9.4599609702988706e-05] 22 | Rci: [-0.134582, -0.990831, -0.0119257, 23 | -0.0427573, 0.0178307, -0.998926, 24 | 0.98998, -0.133927, -0.0447649] 25 | pci: [0.008065706, -0.013901938, -0.044590552] 26 | Roi: [0.98998, -0.133927, -0.0447649, 27 | 0.134582, 0.990831, 0.0119257, 28 | 0.0427573,-0.0178307, 0.998926] 29 | accel_norm: 9.81 30 | gravity_mag: 9.81 31 | 32 | odom: 33 | vel_std: [0.01, 0.01, 0.01] 34 | 35 | state: 36 | which_group: 2 # -2: msckf, -1: no group, 0: features are grouped in IMU state, 1: features are grouped in relative transform, 37 | # 2: features are grouped between clone state and relative transform 38 | max_clone_size: 11 39 | max_slam_features: 50 40 | max_slam_in_update: 25 41 | max_msckf_in_update: 1000 42 | 43 | update: 44 | do_update_msckf: true 45 | do_update_slam: true 46 | do_update_map: true 47 | do_update_odom: true 48 | do_update_plane: true 49 | use_virtual_center: true 50 | use_match_extension: true 51 | msckf_iekf_sigma_px: 4 52 | msckf_chi2_multipler: 1 53 | slam_sigma_px: 4 54 | slam_chi2_multipler: 1 55 | zupt_chi2_multipler: 1 56 | map_sigma_px: 1.5 57 | map_chi2_multipler_dl: 8 58 | map_chi2_multipler_bi: 8 59 | odom_chi2_multipler: 5 60 | plane_chi2_multipler_loc: 1.0 61 | plane_chi2_multipler_prior: 1.0 62 | plane_distance_weight_loc: 0.02 63 | plane_distance_weight_prior: 0.02 64 | plane_distance_threshold_loc: 1.5 65 | plane_distance_threshold_prior: 5.0 66 | 67 | match: 68 | z_th: 55 69 | update_z_th: 75 70 | extend: 20 71 | grey_th: 248 72 | grey_th_low: 235 73 | alpha: 0.2 74 | ang_th_dl: 0.995 75 | ang_th_bi: 0.9998 76 | large_off: 5 77 | dl_area: 40 78 | dl_filter: true 79 | bi_filter: true 80 | remain_match: false 81 | 82 | init: 83 | viwo_init: 84 | init_window_time: 1.0 85 | init_imu_thresh: 1.0 86 | gravity_mag: 9.81 87 | init_max_features: 50 88 | map_init: 89 | search_dist_scope1: 75 90 | search_dist_scope2: 75 91 | sine_th: 0.05 92 | init_use_binary: true 93 | init_grey_threshold: 252 94 | init_grey_threshold_low: 230 95 | outlier_score: 30 96 | max_st_dist: 65 97 | preset_outliers: 0 98 | top_tuples_in_region: 100 99 | top_poses: 25 100 | expansion_proj: 20 101 | subscore_weight: 1 102 | prior_init_available: false 103 | prior_scale: 10.0 104 | prior_init: [0.0, 0.0, 0.0] 105 | 106 | feat_init: 107 | triangulate_1d: false 108 | refine_features: true 109 | max_runs: 5 110 | init_lamda: 1e-3 111 | max_lamda: 1e10 112 | min_dx: 1e-6 113 | min_dcost: 1e-6 114 | lam_mult: 10 115 | min_dist: 0.25 116 | max_dist: 75 117 | max_baseline: 200 118 | max_cond_number: 20000 119 | 120 | tracker: 121 | histogram_method: 1 # 0: NONE, 1: HISTOGRAM, 2: CLAHE 122 | num_features: 200 123 | fast_threshold: 20 124 | grid_x: 32 125 | grid_y: 18 126 | min_px_dist: 8 127 | pyr_levels: 5 128 | win_size: 15 129 | 130 | zupt: 131 | try_zupt: true 132 | zupt_max_velocity: 0.2 133 | zupt_noise_multiplier: 10 134 | zupt_max_disparity: 0.5 135 | zupt_max_velocity_odom: 0.05 136 | zupt_only_at_beginning: false 137 | 138 | tracking_recover: 139 | dist_th: 13 140 | ang_th: 0.35 141 | lost_dist_th: 2 142 | lost_ang_th: 1.5 143 | z_th: 25 144 | extend: 20 145 | grey_thresh_low: 230 146 | expansion_proj: 40 147 | subscore_weight: 1 148 | variance: 64 149 | search_dist_scope: 45 150 | search_bar_height: 20 151 | area_th: 150 152 | expected_weight: 0.2 153 | outlier_cost: 0.5 154 | sigma_pix: 1 155 | chi2_multipler_dl_maylost: 200 156 | chi2_multipler_bi_maylost: 200 157 | chi2_multipler_dl_lost: 8 158 | chi2_multipler_bi_lost: 8 159 | prior_z_diff_th: 1 160 | 161 | save: 162 | save_total_state: true 163 | save_time_consume: true 164 | of_state_est: "log/state_estimation.txt" 165 | of_state_std: "log/state_deviation.txt" 166 | of_state_tum_loc: "log/state_tum_loc.txt" 167 | of_state_tum_global: "log/state_tum_global.txt" 168 | of_state_tracking_recover: "log/" -------------------------------------------------------------------------------- /config/Config_Scene_07.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | img_topic: /camera/color/image_raw/compressed 3 | box_topic: /yolov7_bbox 4 | odom_topic: /odom 5 | imu_topic: /livox/imu 6 | 7 | camera: 8 | cam_fx: 908.524 9 | cam_fy: 908.919 10 | cam_cx: 642.150 11 | cam_cy: 352.982 12 | res_x: 1280 13 | res_y: 720 14 | distortion: [0, 0, 0, 0] 15 | dist_model: radtan 16 | 17 | imu: 18 | gyro_std: [1.6912455461587871e-03, 1.2118225332973804e-03, 1.6146741327812619e-03] 19 | accel_std: [7.2510837330793689e-04, 8.8071690484254771e-04, 1.3471694990898537e-03] 20 | accel_bias_std: [3.0608877651619066e-05, 2.0168661087254066e-05, 3.7608575535597191e-05] 21 | gyro_bias_std: [8.1307125981080534e-05, 4.7064776326160538e-05, 9.4599609702988706e-05] 22 | Rci: [0.00208164, -0.999858, -0.0167461, 23 | 0.430466, 0.0160109, -0.902465, 24 | 0.902605, -0.00532965, 0.430438] 25 | pci: [0.071399057, -0.093224676, -0.035436303] 26 | Roi: [0.902605, -0.00532965, 0.430438, 27 | -0.00208164, 0.999858, 0.0167461, 28 | -0.430466, -0.0160109, 0.902465] 29 | accel_norm: 9.81 30 | gravity_mag: 9.81 31 | 32 | odom: 33 | vel_std: [0.01, 0.01, 0.01] 34 | 35 | state: 36 | which_group: 2 # -2: msckf, -1: no group, 0: features are grouped in IMU state, 1: features are grouped in relative transform, 37 | # 2: features are grouped between clone state and relative transform 38 | max_clone_size: 11 39 | max_slam_features: 50 40 | max_slam_in_update: 25 41 | max_msckf_in_update: 1000 42 | 43 | update: 44 | do_update_msckf: true 45 | do_update_slam: true 46 | do_update_map: true 47 | do_update_odom: true 48 | do_update_plane: true 49 | use_virtual_center: true 50 | use_match_extension: true 51 | msckf_iekf_sigma_px: 4 52 | msckf_chi2_multipler: 1 53 | slam_sigma_px: 4 54 | slam_chi2_multipler: 1 55 | zupt_chi2_multipler: 1 56 | map_sigma_px: 1.5 57 | map_chi2_multipler_dl: 8 58 | map_chi2_multipler_bi: 8 59 | odom_chi2_multipler: 5 60 | plane_chi2_multipler_loc: 1.0 61 | plane_chi2_multipler_prior: 10.0 62 | plane_distance_weight_loc: 0.02 63 | plane_distance_weight_prior: 0.02 64 | plane_distance_threshold_loc: 1.5 65 | plane_distance_threshold_prior: 3.0 66 | 67 | match: 68 | z_th: 55 69 | update_z_th: 75 70 | extend: 20 71 | large_extend: 50 72 | grey_th: 248 73 | grey_th_low: 235 74 | alpha: 0.2 75 | ang_th_dl: 0.9998 # 0.9998 76 | ang_th_bi: 0.9998 # 0.9998 77 | large_off: 30 78 | dl_area: 40 79 | dl_filter: true 80 | bi_filter: true 81 | 82 | init: 83 | viwo_init: 84 | init_window_time: 1.0 85 | init_imu_thresh: 1.0 86 | gravity_mag: 9.81 87 | init_max_features: 50 88 | map_init: 89 | search_dist_scope1: 75 90 | search_dist_scope2: 75 91 | sine_th: 0.05 92 | init_use_binary: true 93 | init_grey_threshold: 252 94 | init_grey_threshold_low: 230 95 | outlier_score: 10 96 | max_st_dist: 65 97 | preset_outliers: 0 98 | top_tuples_in_region: 100 99 | top_poses: 25 100 | expansion_proj: 20 101 | subscore_weight: 3 102 | prior_init_available: false 103 | prior_scale: 10.0 104 | prior_init: [0.0, 0.0, 0.0] 105 | 106 | feat_init: 107 | triangulate_1d: false 108 | refine_features: true 109 | max_runs: 5 110 | init_lamda: 1e-3 111 | max_lamda: 1e10 112 | min_dx: 1e-6 113 | min_dcost: 1e-6 114 | lam_mult: 10 115 | min_dist: 0.25 116 | max_dist: 75 117 | max_baseline: 200 118 | max_cond_number: 20000 119 | 120 | tracker: 121 | histogram_method: 1 # 0: NONE, 1: HISTOGRAM, 2: CLAHE 122 | num_features: 200 123 | fast_threshold: 20 124 | grid_x: 32 125 | grid_y: 18 126 | min_px_dist: 8 127 | pyr_levels: 5 128 | win_size: 15 129 | 130 | zupt: 131 | try_zupt: true 132 | zupt_max_velocity: 0.2 133 | zupt_noise_multiplier: 10 134 | zupt_max_disparity: 0.5 135 | zupt_max_velocity_odom: 0.05 136 | zupt_only_at_beginning: false 137 | 138 | tracking_recover: 139 | dist_th: 10 # 5 140 | ang_th: 0.35 141 | lost_dist_th: 2 142 | lost_ang_th: 1.5 143 | z_th: 25 144 | extend: 20 145 | grey_thresh_low: 230 146 | expansion_proj: 40 147 | subscore_weight: 1 148 | variance: 64 149 | search_dist_scope: 45 150 | search_bar_height: 20 151 | area_th: 150 152 | expected_weight: 0.2 153 | outlier_cost: 0.5 154 | sigma_pix: 1 155 | chi2_multipler_dl_maylost: 200 156 | chi2_multipler_bi_maylost: 200 157 | chi2_multipler_dl_lost: 8 158 | chi2_multipler_bi_lost: 8 159 | prior_z_diff_th: 1 160 | 161 | save: 162 | save_total_state: true 163 | save_time_consume: true 164 | of_state_est: "log/state_estimation.txt" 165 | of_state_std: "log/state_deviation.txt" 166 | of_state_tum_loc: "log/state_tum_loc.txt" 167 | of_state_tum_global: "log/state_tum_global.txt" 168 | of_state_tracking_recover: "log/" -------------------------------------------------------------------------------- /config/Config_Scene_08.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | img_topic: /camera/color/image_raw/compressed 3 | box_topic: /yolov7_bbox 4 | odom_topic: /odom 5 | imu_topic: /livox/imu 6 | 7 | camera: 8 | cam_fx: 908.524 9 | cam_fy: 908.919 10 | cam_cx: 642.150 11 | cam_cy: 352.982 12 | res_x: 1280 13 | res_y: 720 14 | distortion: [0, 0, 0, 0] 15 | dist_model: radtan 16 | 17 | imu: 18 | gyro_std: [1.6912455461587871e-03, 1.2118225332973804e-03, 1.6146741327812619e-03] 19 | accel_std: [7.2510837330793689e-04, 8.8071690484254771e-04, 1.3471694990898537e-03] 20 | accel_bias_std: [3.0608877651619066e-05, 2.0168661087254066e-05, 3.7608575535597191e-05] 21 | gyro_bias_std: [8.1307125981080534e-05, 4.7064776326160538e-05, 9.4599609702988706e-05] 22 | Rci: [0.00208164, -0.999858, -0.0167461, 23 | 0.430466, 0.0160109, -0.902465, 24 | 0.902605, -0.00532965, 0.430438] 25 | pci: [0.071399057, -0.093224676, -0.035436303] 26 | Roi: [0.902605, -0.00532965, 0.430438, 27 | -0.00208164, 0.999858, 0.0167461, 28 | -0.430466, -0.0160109, 0.902465] 29 | accel_norm: 9.81 30 | gravity_mag: 9.81 31 | 32 | odom: 33 | vel_std: [0.01, 0.01, 0.01] 34 | 35 | state: 36 | which_group: 2 # -2: msckf, -1: no group, 0: features are grouped in IMU state, 1: features are grouped in relative transform, 37 | # 2: features are grouped between clone state and relative transform 38 | max_clone_size: 11 39 | max_slam_features: 50 40 | max_slam_in_update: 25 41 | max_msckf_in_update: 1000 42 | 43 | update: 44 | do_update_msckf: true 45 | do_update_slam: true 46 | do_update_map: true 47 | do_update_odom: true 48 | do_update_plane: true 49 | use_virtual_center: true 50 | use_match_extension: true 51 | msckf_iekf_sigma_px: 4 52 | msckf_chi2_multipler: 1 53 | slam_sigma_px: 4 54 | slam_chi2_multipler: 1 55 | zupt_chi2_multipler: 1 56 | map_sigma_px: 1.5 57 | map_chi2_multipler_dl: 8 58 | map_chi2_multipler_bi: 8 59 | odom_chi2_multipler: 5 60 | plane_chi2_multipler_loc: 1.0 61 | plane_chi2_multipler_prior: 10.0 62 | plane_distance_weight_loc: 0.02 63 | plane_distance_weight_prior: 0.02 64 | plane_distance_threshold_loc: 1.5 65 | plane_distance_threshold_prior: 5.0 66 | 67 | match: 68 | z_th: 55 69 | update_z_th: 75 70 | extend: 20 71 | grey_th: 248 72 | grey_th_low: 235 73 | alpha: 0.2 74 | ang_th_dl: 0.995 75 | ang_th_bi: 0.9998 76 | large_off: 30 # 30 77 | dl_area: 40 78 | dl_filter: true 79 | bi_filter: true 80 | remain_match: true 81 | 82 | init: 83 | viwo_init: 84 | init_window_time: 1.0 85 | init_imu_thresh: 1.0 86 | gravity_mag: 9.81 87 | init_max_features: 50 88 | map_init: 89 | search_dist_scope1: 75 90 | search_dist_scope2: 75 91 | sine_th: 0.1 92 | init_use_binary: true 93 | init_grey_threshold: 252 94 | init_grey_threshold_low: 230 95 | outlier_score: 10 96 | max_st_dist: 65 97 | preset_outliers: 0 98 | top_tuples_in_region: 100 99 | top_poses: 25 100 | expansion_proj: 20 101 | subscore_weight: 3 102 | prior_init_available: false 103 | prior_scale: 10.0 104 | prior_init: [0.0, 0.0, 0.0] 105 | 106 | feat_init: 107 | triangulate_1d: false 108 | refine_features: true 109 | max_runs: 5 110 | init_lamda: 1e-3 111 | max_lamda: 1e10 112 | min_dx: 1e-6 113 | min_dcost: 1e-6 114 | lam_mult: 10 115 | min_dist: 0.25 116 | max_dist: 75 117 | max_baseline: 200 118 | max_cond_number: 20000 119 | 120 | tracker: 121 | histogram_method: 1 # 0: NONE, 1: HISTOGRAM, 2: CLAHE 122 | num_features: 150 123 | fast_threshold: 20 124 | grid_x: 32 # 8 125 | grid_y: 18 # 6 126 | min_px_dist: 8 127 | pyr_levels: 5 # 4 128 | win_size: 15 129 | 130 | zupt: 131 | try_zupt: true 132 | zupt_max_velocity: 0.2 133 | zupt_noise_multiplier: 10 134 | zupt_max_disparity: 0.5 135 | zupt_max_velocity_odom: 0.05 136 | zupt_only_at_beginning: false 137 | 138 | tracking_recover: 139 | dist_th: 20 140 | ang_th: 0.35 141 | lost_dist_th: 5 142 | lost_ang_th: 1.5 143 | z_th: 25 144 | extend: 20 145 | grey_thresh_low: 230 146 | expansion_proj: 40 147 | subscore_weight: 1 148 | variance: 64 149 | search_dist_scope: 45 150 | search_bar_height: 60 151 | area_th: 150 152 | expected_weight: 0.2 153 | outlier_cost: 0.5 154 | sigma_pix: 1 155 | chi2_multipler_dl_maylost: 50 156 | chi2_multipler_bi_maylost: 50 157 | chi2_multipler_dl_lost: 8 158 | chi2_multipler_bi_lost: 8 159 | prior_z_diff_th: 1 160 | 161 | save: 162 | save_total_state: true 163 | save_time_consume: true 164 | of_state_est: "log/state_estimation.txt" 165 | of_state_std: "log/state_deviation.txt" 166 | of_state_tum_loc: "log/state_tum_loc.txt" 167 | of_state_tum_global: "log/state_tum_global.txt" 168 | of_state_tracking_recover: "log/" -------------------------------------------------------------------------------- /config/Config_Scene_09.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | img_topic: /camera/color/image_raw/compressed 3 | box_topic: /yolov7_bbox 4 | odom_topic: /odom 5 | imu_topic: /livox/imu 6 | 7 | camera: 8 | cam_fx: 908.524 9 | cam_fy: 908.919 10 | cam_cx: 642.150 11 | cam_cy: 352.982 12 | res_x: 1280 13 | res_y: 720 14 | distortion: [0, 0, 0, 0] 15 | dist_model: radtan 16 | 17 | imu: 18 | gyro_std: [1.6912455461587871e-03, 1.2118225332973804e-03, 1.6146741327812619e-03] 19 | accel_std: [7.2510837330793689e-04, 8.8071690484254771e-04, 1.3471694990898537e-03] 20 | accel_bias_std: [3.0608877651619066e-05, 2.0168661087254066e-05, 3.7608575535597191e-05] 21 | gyro_bias_std: [8.1307125981080534e-05, 4.7064776326160538e-05, 9.4599609702988706e-05] 22 | Rci: [0.00208164, -0.999858, -0.0167461, 23 | 0.430466, 0.0160109, -0.902465, 24 | 0.902605, -0.00532965, 0.430438] 25 | pci: [0.071399057, -0.093224676, -0.035436303] 26 | Roi: [0.902605, -0.00532965, 0.430438, 27 | -0.00208164, 0.999858, 0.0167461, 28 | -0.430466, -0.0160109, 0.902465] 29 | accel_norm: 9.81 30 | gravity_mag: 9.81 31 | 32 | odom: 33 | vel_std: [0.01, 0.01, 0.01] 34 | 35 | 36 | state: 37 | which_group: 2 # -2: msckf, -1: no group, 0: features are grouped in IMU state, 1: features are grouped in relative transform, 38 | # 2: features are grouped between clone state and relative transform 39 | max_clone_size: 11 40 | max_slam_features: 50 41 | max_slam_in_update: 25 42 | max_msckf_in_update: 1000 43 | 44 | update: 45 | do_update_msckf: true 46 | do_update_slam: true 47 | do_update_map: true 48 | do_update_odom: true 49 | do_update_plane: true 50 | use_virtual_center: true 51 | use_match_extension: true 52 | msckf_iekf_sigma_px: 4 53 | msckf_chi2_multipler: 1 54 | slam_sigma_px: 4 55 | slam_chi2_multipler: 1 56 | zupt_chi2_multipler: 1 57 | map_sigma_px: 1.5 58 | map_chi2_multipler_dl: 8 59 | map_chi2_multipler_bi: 8 60 | odom_chi2_multipler: 5 61 | plane_chi2_multipler_loc: 1.0 62 | plane_chi2_multipler_prior: 10.0 63 | plane_distance_weight_loc: 0.02 64 | plane_distance_weight_prior: 0.02 65 | plane_distance_threshold_loc: 1.5 66 | plane_distance_threshold_prior: 5.0 67 | 68 | match: 69 | z_th: 55 70 | update_z_th: 75 71 | extend: 20 72 | large_extend: 50 73 | grey_th: 248 74 | grey_th_low: 235 75 | alpha: 0.2 76 | ang_th_dl: 0.995 77 | ang_th_bi: 0.9998 78 | large_off: 30 79 | dl_area: 40 80 | dl_filter: true 81 | bi_filter: true 82 | remain_match: false 83 | 84 | init: 85 | viwo_init: 86 | init_window_time: 1.0 87 | init_imu_thresh: 1.0 88 | gravity_mag: 9.81 89 | init_max_features: 50 90 | map_init: 91 | search_dist_scope1: 75 92 | search_dist_scope2: 75 93 | sine_th: 0.05 94 | init_use_binary: true 95 | init_grey_threshold: 252 96 | init_grey_threshold_low: 230 97 | outlier_score: 10 98 | max_st_dist: 65 99 | preset_outliers: 0 100 | top_tuples_in_region: 100 101 | top_poses: 25 102 | expansion_proj: 20 103 | subscore_weight: 3 104 | prior_init_available: false 105 | prior_scale: 10.0 106 | prior_init: [0.0, 0.0, 0.0] 107 | 108 | feat_init: 109 | triangulate_1d: false 110 | refine_features: true 111 | max_runs: 5 112 | init_lamda: 1e-3 113 | max_lamda: 1e10 114 | min_dx: 1e-6 115 | min_dcost: 1e-6 116 | lam_mult: 10 117 | min_dist: 0.25 118 | max_dist: 75 # 75 119 | max_baseline: 200 120 | max_cond_number: 20000 # 20000 121 | 122 | tracker: 123 | histogram_method: 1 # 0: NONE, 1: HISTOGRAM, 2: CLAHE 124 | num_features: 150 125 | fast_threshold: 20 126 | grid_x: 32 127 | grid_y: 18 128 | min_px_dist: 8 129 | pyr_levels: 5 130 | win_size: 15 131 | 132 | zupt: 133 | try_zupt: true 134 | zupt_max_velocity: 0.2 135 | zupt_noise_multiplier: 10 136 | zupt_max_disparity: 0.5 137 | zupt_max_velocity_odom: 0.05 138 | zupt_only_at_beginning: false 139 | 140 | tracking_recover: 141 | dist_th: 20 142 | ang_th: 0.35 143 | lost_dist_th: 5 144 | lost_ang_th: 1.5 145 | z_th: 25 146 | extend: 20 147 | grey_thresh_low: 230 148 | expansion_proj: 40 149 | subscore_weight: 1 150 | variance: 64 151 | search_dist_scope: 45 152 | search_bar_height: 40 153 | area_th: 150 154 | expected_weight: 0.2 155 | outlier_cost: 0.1 156 | sigma_pix: 1 157 | chi2_multipler_dl_maylost: 100 158 | chi2_multipler_bi_maylost: 100 159 | chi2_multipler_dl_lost: 8 160 | chi2_multipler_bi_lost: 8 161 | prior_z_diff_th: 1 162 | 163 | save: 164 | save_total_state: true 165 | save_time_consume: true 166 | of_state_est: "log/state_estimation.txt" 167 | of_state_std: "log/state_deviation.txt" 168 | of_state_tum_loc: "log/state_tum_loc.txt" 169 | of_state_tum_global: "log/state_tum_global.txt" 170 | of_state_tracking_recover: "log/" -------------------------------------------------------------------------------- /config/Config_Scene_10.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | img_topic: /camera/color/image_raw/compressed 3 | box_topic: /yolov7_bbox 4 | odom_topic: /odom 5 | imu_topic: /livox/imu 6 | 7 | camera: 8 | cam_fx: 908.524 9 | cam_fy: 908.919 10 | cam_cx: 642.150 11 | cam_cy: 352.982 12 | res_x: 1280 13 | res_y: 720 14 | distortion: [0, 0, 0, 0] 15 | dist_model: radtan 16 | 17 | imu: 18 | gyro_std: [1.6912455461587871e-03, 1.2118225332973804e-03, 1.6146741327812619e-03] 19 | accel_std: [7.2510837330793689e-04, 8.8071690484254771e-04, 1.3471694990898537e-03] 20 | accel_bias_std: [3.0608877651619066e-05, 2.0168661087254066e-05, 3.7608575535597191e-05] 21 | gyro_bias_std: [8.1307125981080534e-05, 4.7064776326160538e-05, 9.4599609702988706e-05] 22 | Rci: [0.00208164, -0.999858, -0.0167461, 23 | 0.430466, 0.0160109, -0.902465, 24 | 0.902605, -0.00532965, 0.430438] 25 | pci: [0.071399057, -0.093224676, -0.035436303] 26 | Roi: [0.902605, -0.00532965, 0.430438, 27 | -0.00208164, 0.999858, 0.0167461, 28 | -0.430466, -0.0160109, 0.902465] 29 | accel_norm: 9.81 30 | gravity_mag: 9.81 31 | 32 | odom: 33 | vel_std: [0.01, 0.01, 0.01] 34 | 35 | state: 36 | which_group: 2 # -2: msckf, -1: no group, 0: features are grouped in IMU state, 1: features are grouped in relative transform, 37 | # 2: features are grouped between clone state and relative transform 38 | transform_as_group: false 39 | multistate_as_group: false 40 | max_clone_size: 11 41 | max_slam_features: 50 42 | max_slam_in_update: 25 43 | max_msckf_in_update: 1000 44 | 45 | update: 46 | do_update_msckf: true 47 | do_update_slam: true 48 | do_update_map: true 49 | do_update_odom: true 50 | do_update_plane: true 51 | use_virtual_center: true 52 | use_match_extension: true 53 | msckf_iekf_sigma_px: 4 54 | msckf_chi2_multipler: 1 55 | slam_sigma_px: 4 56 | slam_chi2_multipler: 1 57 | zupt_chi2_multipler: 1 58 | map_sigma_px: 1.5 59 | map_chi2_multipler_dl: 8 # 8 for all 60 | map_chi2_multipler_bi: 2 61 | odom_chi2_multipler: 5 62 | plane_chi2_multipler_loc: 1.0 63 | plane_chi2_multipler_prior: 10.0 64 | plane_distance_weight_loc: 0.02 65 | plane_distance_weight_prior: 0.02 66 | plane_distance_threshold_loc: 1.5 67 | plane_distance_threshold_prior: 5.0 68 | 69 | match: 70 | z_th: 55 71 | update_z_th: 75 72 | extend: 10 #20 73 | large_extend: 10 #20 74 | grey_th: 248 75 | grey_th_low: 235 76 | alpha: 0.2 77 | ang_th_dl: 0.995 78 | ang_th_bi: 0.9998 79 | large_off: 5 80 | dl_area: 40 81 | dl_filter: true 82 | bi_filter: true 83 | 84 | init: 85 | viwo_init: 86 | init_window_time: 1.0 87 | init_imu_thresh: 1.0 88 | gravity_mag: 9.81 89 | init_max_features: 50 90 | map_init: 91 | search_dist_scope1: 75 92 | search_dist_scope2: 75 93 | sine_th: 0.05 94 | init_use_binary: true 95 | init_grey_threshold: 252 96 | init_grey_threshold_low: 230 97 | outlier_score: 10 98 | max_st_dist: 65 99 | preset_outliers: 0 100 | top_tuples_in_region: 100 101 | top_poses: 25 102 | expansion_proj: 20 103 | subscore_weight: 3 104 | prior_init_available: false 105 | prior_scale: 10.0 106 | prior_init: [0.0, 0.0, 0.0] 107 | 108 | feat_init: 109 | triangulate_1d: false 110 | refine_features: true 111 | max_runs: 5 112 | init_lamda: 1e-3 113 | max_lamda: 1e10 114 | min_dx: 1e-6 115 | min_dcost: 1e-6 116 | lam_mult: 10 117 | min_dist: 0.25 118 | max_dist: 75 119 | max_baseline: 200 120 | max_cond_number: 10000 121 | 122 | tracker: 123 | histogram_method: 1 # 0: NONE, 1: HISTOGRAM, 2: CLAHE 124 | num_features: 150 125 | fast_threshold: 20 126 | grid_x: 32 127 | grid_y: 18 128 | min_px_dist: 8 129 | pyr_levels: 5 130 | win_size: 15 131 | 132 | zupt: 133 | try_zupt: true 134 | zupt_max_velocity: 0.2 135 | zupt_noise_multiplier: 10 136 | zupt_max_disparity: 0.5 137 | zupt_max_velocity_odom: 0.05 138 | zupt_only_at_beginning: false 139 | 140 | tracking_recover: 141 | dist_th: 10 142 | ang_th: 0.5 143 | lost_dist_th: 2 144 | lost_ang_th: 1.5 145 | z_th: 25 146 | extend: 20 147 | grey_thresh_low: 230 148 | expansion_proj: 40 149 | subscore_weight: 1 150 | variance: 64 151 | search_dist_scope: 45 152 | search_bar_height: 40 153 | area_th: 150 154 | expected_weight: 0.2 155 | outlier_cost: 0.5 156 | sigma_pix: 1 157 | chi2_multipler_dl_maylost: 75 # 50 for all, 75 for PP, VC 158 | chi2_multipler_bi_maylost: 50 # 50 159 | chi2_multipler_dl_lost: 8 # 8 for all 160 | chi2_multipler_bi_lost: 2 161 | prior_z_diff_th: 1 162 | 163 | save: 164 | save_total_state: true 165 | save_time_consume: true 166 | of_state_est: "log/state_estimation.txt" 167 | of_state_std: "log/state_deviation.txt" 168 | of_state_tum_loc: "log/state_tum_loc.txt" 169 | of_state_tum_global: "log/state_tum_global.txt" 170 | of_state_tracking_recover: "log/" -------------------------------------------------------------------------------- /cover.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/cover.png -------------------------------------------------------------------------------- /img/Scene_01_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/img/Scene_01_map.png -------------------------------------------------------------------------------- /img/Scene_01_preview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/img/Scene_01_preview.png -------------------------------------------------------------------------------- /img/Scene_02_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/img/Scene_02_map.png -------------------------------------------------------------------------------- /img/Scene_02_preview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/img/Scene_02_preview.png -------------------------------------------------------------------------------- /img/Scene_03_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/img/Scene_03_map.png -------------------------------------------------------------------------------- /img/Scene_03_preview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/img/Scene_03_preview.png -------------------------------------------------------------------------------- /img/Scene_04_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/img/Scene_04_map.png -------------------------------------------------------------------------------- /img/Scene_04_preview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/img/Scene_04_preview.png -------------------------------------------------------------------------------- /img/Scene_05_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/img/Scene_05_map.png -------------------------------------------------------------------------------- /img/Scene_05_preview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/img/Scene_05_preview.png -------------------------------------------------------------------------------- /img/Scene_06_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/img/Scene_06_map.png -------------------------------------------------------------------------------- /img/Scene_06_preview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/img/Scene_06_preview.png -------------------------------------------------------------------------------- /img/Scene_07_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/img/Scene_07_map.png -------------------------------------------------------------------------------- /img/Scene_07_preview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/img/Scene_07_preview.png -------------------------------------------------------------------------------- /img/Scene_08_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/img/Scene_08_map.png -------------------------------------------------------------------------------- /img/Scene_08_preview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/img/Scene_08_preview.png -------------------------------------------------------------------------------- /img/Scene_09_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/img/Scene_09_map.png -------------------------------------------------------------------------------- /img/Scene_09_preview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/img/Scene_09_preview.png -------------------------------------------------------------------------------- /img/Scene_10_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/img/Scene_10_map.png -------------------------------------------------------------------------------- /img/Scene_10_preview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/img/Scene_10_preview.png -------------------------------------------------------------------------------- /img/System_Framework.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/img/System_Framework.png -------------------------------------------------------------------------------- /include/core/IMU.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef IMU_H 12 | #define IMU_H 13 | 14 | #include "core/Posev.h" 15 | #include "core/Type.h" 16 | #include "core/Vec.h" 17 | #include "utils/Transform.h" 18 | 19 | namespace night_voyager { 20 | class IMU : public Type { 21 | public: 22 | IMU() : Type(15) { 23 | _pose = std::shared_ptr(new Posev()); 24 | _bg = std::shared_ptr(new Vec(3)); 25 | _ba = std::shared_ptr(new Vec(3)); 26 | 27 | // Set our default state value 28 | Eigen::MatrixXd imu0 = Eigen::MatrixXd::Identity(5, 7); 29 | set_value_internal(imu0); 30 | } 31 | 32 | ~IMU() {} 33 | 34 | /** 35 | * @brief Sets id used to track location of variable in the filter covariance 36 | * 37 | * Note that we update the sub-variables also. 38 | * 39 | * @param new_id entry in filter covariance corresponding to this variable 40 | */ 41 | void set_local_id(int new_id) override { 42 | _id = new_id; 43 | _pose->set_local_id(new_id); 44 | _bg->set_local_id(_pose->id() + ((new_id != -1) ? _pose->size() : 0)); 45 | _ba->set_local_id(_bg->id() + ((new_id != -1) ? _bg->size() : 0)); 46 | } 47 | 48 | /** 49 | * @brief Performs update operation using Hamilton update for orientation, then vector updates for 50 | * position, velocity, gyro bias, and accel bias (in that order). 51 | * 52 | * @param dx 15 DOF vector encoding update using the following order (q, p, v, bg, ba) 53 | */ 54 | void update(const Eigen::VectorXd &dx) override { 55 | 56 | assert(dx.rows() == _size); 57 | 58 | Eigen::MatrixXd newX = _value; 59 | 60 | // cout << "oldX: " << endl << newX << endl; 61 | newX.block(0, 0, 5, 5) = Exp_SEK3(dx.segment(0, 9)) * newX.block(0, 0, 5, 5); 62 | newX.block(0, 5, 3, 1) += dx.block(9, 0, 3, 1); 63 | newX.block(0, 6, 3, 1) += dx.block(12, 0, 3, 1); 64 | // cout << "newX: " << endl << newX << endl; 65 | 66 | set_value(newX); 67 | } 68 | 69 | /** 70 | * @brief Performs msckf update operation using Hamilton update for orientation, then vector updates for 71 | * position, velocity, gyro bias, and accel bias (in that order). 72 | * 73 | * @param dx 15 DOF vector encoding update using the following order (q, p, v, bg, ba) 74 | */ 75 | void msckf_update(const Eigen::VectorXd &dx) override { 76 | 77 | assert(dx.rows() == _size); 78 | 79 | Eigen::MatrixXd newX = _value; 80 | 81 | // cout << "oldX: " << endl << newX.block(0, 0, 3, 3) << endl; 82 | newX.block(0, 0, 3, 3) = newX.block(0, 0, 3, 3) * Exp_SO3(dx.block(0, 0, 3, 1)); 83 | newX.block(0, 3, 3, 1) += dx.block(3, 0, 3, 1); 84 | newX.block(0, 4, 3, 1) += dx.block(6, 0, 3, 1); 85 | newX.block(0, 5, 3, 1) += dx.block(9, 0, 3, 1); 86 | newX.block(0, 6, 3, 1) += dx.block(12, 0, 3, 1); 87 | // cout << "newX: " << endl << newX.block(0, 0, 3, 3) << endl; 88 | 89 | set_value(newX); 90 | } 91 | 92 | /** 93 | * @brief Sets the value of the estimate 94 | * @param new_value New value we should set 95 | */ 96 | void set_value(const Eigen::MatrixXd &new_value) override { set_value_internal(new_value); } 97 | 98 | std::shared_ptr clone() override { 99 | auto Clone = std::shared_ptr(new IMU()); 100 | Clone->set_value(value()); 101 | return Clone; 102 | } 103 | 104 | std::shared_ptr check_if_subvariable(const std::shared_ptr check) override { 105 | if (check == _pose) { 106 | return _pose; 107 | } else if (check == _pose->check_if_subvariable(check)) { 108 | return _pose->check_if_subvariable(check); 109 | } else if (check == _bg) { 110 | return _bg; 111 | } else if (check == _ba) { 112 | return _ba; 113 | } 114 | return nullptr; 115 | } 116 | 117 | /// Rotation access 118 | Eigen::Matrix Rot() const { return _pose->Rot(); } 119 | 120 | /// Quaternion access 121 | Eigen::Quaterniond quat() const { return _pose->quat(); } 122 | 123 | /// Position access 124 | Eigen::Matrix pos() const { return _pose->pos(); } 125 | 126 | /// Velocity access 127 | Eigen::Matrix vel() const { return _pose->vel(); } 128 | 129 | /// Pose + Velocity access 130 | Eigen::Matrix pose_vel() const { return _pose->value(); } 131 | 132 | /// Gyro bias access 133 | Eigen::Matrix bias_g() const { return _bg->value(); } 134 | 135 | /// Accel bias access 136 | Eigen::Matrix bias_a() const { return _ba->value(); } 137 | 138 | /// Posev type access 139 | std::shared_ptr posev() { return _pose; } 140 | 141 | /// Pose type access 142 | std::shared_ptr pose() { return _pose->pose(); } 143 | 144 | /// Quaternion type access 145 | std::shared_ptr R() { return _pose->R(); } 146 | 147 | /// Position type access 148 | std::shared_ptr p() { return _pose->p(); } 149 | 150 | /// Velocity type access 151 | std::shared_ptr v() { return _pose->v(); } 152 | 153 | /// Gyroscope bias access 154 | std::shared_ptr bg() { return _bg; } 155 | 156 | /// Acceleration bias access 157 | std::shared_ptr ba() { return _ba; } 158 | 159 | protected: 160 | /// Pose subvariable 161 | std::shared_ptr _pose; 162 | 163 | /// Gyroscope bias subvariable 164 | std::shared_ptr _bg; 165 | 166 | /// Acceleration bias subvariable 167 | std::shared_ptr _ba; 168 | 169 | /** 170 | * @brief Sets the value of the estimate 171 | * @param new_value New value we should set 172 | */ 173 | void set_value_internal(const Eigen::MatrixXd &new_value) { 174 | 175 | assert(new_value.rows() == 5); 176 | assert(new_value.cols() == 7); 177 | 178 | _pose->set_value(new_value.block(0, 0, 5, 5)); 179 | _bg->set_value(new_value.block(0, 5, 3, 1)); 180 | _ba->set_value(new_value.block(0, 6, 3, 1)); 181 | 182 | _value = new_value; 183 | } 184 | }; 185 | } // namespace night_voyager 186 | #endif -------------------------------------------------------------------------------- /include/core/LandMark.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef LANDMARK_H 12 | #define LANDMARK_H 13 | 14 | #include "core/Pose.h" 15 | #include "core/Vec.h" 16 | #include "utils/Print.h" 17 | #include 18 | 19 | namespace night_voyager { 20 | class Landmark : public Vec { 21 | 22 | public: 23 | /// Default constructor (feature is a Vec of size 3 or Vec of size 1) 24 | Landmark(int dim) : Vec(dim) {} 25 | 26 | /// Feature ID of this landmark (corresponds to frontend id) 27 | size_t _featid; 28 | 29 | /// Timestamp of anchor clone 30 | double _anchor_clone_timestamp = -1; 31 | 32 | /// Timestamp of pseudo anchor clone 33 | double pseudo_anchor_clone_timestamp = -1; 34 | 35 | /// Boolean if this landmark has had at least one anchor change 36 | bool has_had_anchor_change = false; 37 | 38 | /// Boolean if this landmark should be marginalized out 39 | bool should_marg = false; 40 | 41 | /// Number of times the update has failed for this feature (we should remove if it fails a couple times!) 42 | int update_fail_count = 0; 43 | 44 | std::shared_ptr anchor_frame; 45 | 46 | /** 47 | * @brief Overrides the default vector update rule 48 | * @param dx Additive error state correction 49 | */ 50 | void update(const Eigen::VectorXd &dx) override { 51 | // Update estimate 52 | assert(dx.rows() == _size); 53 | set_value(_value + dx); 54 | } 55 | 56 | /** 57 | * @brief Will return the position of the feature in the global frame of reference. 58 | * @return Position of feature either in global or anchor frame 59 | */ 60 | Eigen::Matrix get_xyz() const { return value(); } 61 | 62 | /** 63 | * @brief Will set the current value. 64 | * @param p_FinG Position of the feature either in global or anchor frame 65 | */ 66 | void set_from_xyz(Eigen::Matrix p_FinG) { 67 | set_value(p_FinG); 68 | return; 69 | } 70 | 71 | /** 72 | * @brief Performs all the cloning 73 | */ 74 | std::shared_ptr clone() override { 75 | auto Clone = std::shared_ptr(new Landmark(_size)); 76 | Clone->set_value(value()); 77 | return Clone; 78 | } 79 | }; 80 | } // namespace night_voyager 81 | #endif -------------------------------------------------------------------------------- /include/core/Mat.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef MAT_H 12 | #define MAT_H 13 | 14 | #include "core/Type.h" 15 | #include "utils/Transform.h" 16 | 17 | namespace night_voyager { 18 | class Mat : public Type { 19 | public: 20 | Mat() : Type(3) { _value = Eigen::Matrix3d::Identity(); } 21 | 22 | ~Mat() {} 23 | 24 | /** 25 | * @brief Implements the update operation through standard vector addition 26 | * 27 | * \f{align*}{ 28 | * \mathbf{v} &= \hat{\mathbf{v}} + \tilde{\mathbf{v}}_{dx} 29 | * \f} 30 | * 31 | * @param dx Additive error state correction 32 | */ 33 | void update(const Eigen::VectorXd &dx) override { 34 | assert(dx.rows() == _size); 35 | set_value(Exp_SO3(dx) * _value); 36 | } 37 | 38 | /** 39 | * @brief Implements the update operation through standard vector addition 40 | * 41 | * \f{align*}{ 42 | * \mathbf{v} &= \hat{\mathbf{v}} + \tilde{\mathbf{v}}_{dx} 43 | * \f} 44 | * 45 | * @param dx Additive error state correction 46 | */ 47 | void msckf_update(const Eigen::VectorXd &dx) override { 48 | assert(dx.rows() == _size); 49 | set_value(_value * Exp_SO3(dx)); 50 | } 51 | 52 | /** 53 | * @brief Performs all the cloning 54 | */ 55 | std::shared_ptr clone() override { 56 | auto Clone = std::shared_ptr(new Mat()); 57 | Clone->set_value(value()); 58 | return Clone; 59 | } 60 | }; 61 | } // namespace night_voyager 62 | #endif -------------------------------------------------------------------------------- /include/core/Pose.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef Pose_H 12 | #define Pose_H 13 | 14 | #include "core/Mat.h" 15 | #include "core/Type.h" 16 | #include "core/Vec.h" 17 | #include "utils/Transform.h" 18 | #include 19 | #include 20 | 21 | namespace night_voyager { 22 | 23 | class Pose : public Type { 24 | public: 25 | Pose() : Type(6) { 26 | // Initialize subvariables 27 | _R = std::shared_ptr(new Mat()); 28 | _p = std::shared_ptr(new Vec(3)); 29 | 30 | Eigen::Matrix pose0 = Eigen::Matrix::Identity(); 31 | set_value_internal(pose0); 32 | } 33 | 34 | ~Pose() {} 35 | 36 | /** 37 | * @brief Sets id used to track location of variable in the filter covariance 38 | * 39 | * Note that we update the sub-variables also. 40 | * 41 | * @param new_id entry in filter covariance corresponding to this variable 42 | */ 43 | void set_local_id(int new_id) override { 44 | _id = new_id; 45 | _R->set_local_id(new_id); 46 | _p->set_local_id(new_id + ((new_id != -1) ? _R->size() : 0)); 47 | } 48 | 49 | /** 50 | * @brief Update q and p using a the JPLQuat update for orientation and vector update for position 51 | * 52 | * @param dx Correction vector (orientation then position) 53 | */ 54 | void update(const Eigen::VectorXd &dx) override { 55 | 56 | assert(dx.rows() == _size); 57 | Eigen::MatrixXd X = _value; 58 | X = Exp_SEK3(dx) * X; 59 | 60 | set_value(X); 61 | } 62 | 63 | /** 64 | * @brief MSCKF Update q and p using a the HamiltonQuat update for orientation and vector update for position 65 | * 66 | * @param dx Correction vector (orientation then position) 67 | */ 68 | void msckf_update(const Eigen::VectorXd &dx) override { 69 | 70 | assert(dx.rows() == _size); 71 | Eigen::MatrixXd X = _value; 72 | X.block(0, 0, 3, 3) = X.block(0, 0, 3, 3) * Exp_SO3(dx.block(0, 0, 3, 1)); 73 | X.block(0, 3, 3, 1) += dx.block(3, 0, 3, 1); 74 | 75 | set_value(X); 76 | } 77 | 78 | /** 79 | * @brief Sets the value of the estimate 80 | * @param new_value New value we should set 81 | */ 82 | void set_value(const Eigen::MatrixXd &new_value) override { set_value_internal(new_value); } 83 | 84 | std::shared_ptr clone() override { 85 | auto Clone = std::shared_ptr(new Pose()); 86 | Clone->set_value(value()); 87 | return Clone; 88 | } 89 | 90 | std::shared_ptr check_if_subvariable(const std::shared_ptr check) override { 91 | if (check == _R) { 92 | return _R; 93 | } else if (check == _p) { 94 | return _p; 95 | } 96 | return nullptr; 97 | } 98 | 99 | /// Rotation access 100 | Eigen::Matrix Rot() const { return _R->value(); } 101 | 102 | /// Quaternion access 103 | Eigen::Quaterniond quat() const { 104 | Eigen::Matrix3d R = _R->value(); 105 | return Eigen::Quaterniond(R); 106 | } 107 | 108 | /// Position access 109 | Eigen::Matrix pos() const { return _p->value(); } 110 | 111 | // Quaternion type access 112 | std::shared_ptr R() { return _R; } 113 | 114 | // Position type access 115 | std::shared_ptr p() { return _p; } 116 | 117 | protected: 118 | std::shared_ptr _R; 119 | std::shared_ptr _p; 120 | 121 | /** 122 | * @brief Sets the value of the estimate 123 | * @param new_value New value we should set 124 | */ 125 | void set_value_internal(const Eigen::MatrixXd &new_value) { 126 | 127 | assert(new_value.rows() == 4); 128 | assert(new_value.cols() == 4); 129 | 130 | // Set orientation value 131 | _R->set_value(new_value.block(0, 0, 3, 3)); 132 | 133 | // Set position value 134 | _p->set_value(new_value.block(0, 3, 3, 1)); 135 | 136 | _value = new_value; 137 | } 138 | }; 139 | 140 | } // namespace night_voyager 141 | 142 | #endif -------------------------------------------------------------------------------- /include/core/Posev.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef POSEV_H 12 | #define POSEV_H 13 | 14 | #include "core/Pose.h" 15 | 16 | namespace night_voyager { 17 | 18 | class Posev : public Type { 19 | public: 20 | Posev() : Type(9) { 21 | // Initialize subvariables 22 | _pose = std::shared_ptr(new Pose()); 23 | _v = std::shared_ptr(new Vec(3)); 24 | 25 | Eigen::Matrix pose0 = Eigen::Matrix::Identity(); 26 | set_value_internal(pose0); 27 | } 28 | 29 | ~Posev() {} 30 | 31 | /** 32 | * @brief Sets id used to track location of variable in the filter covariance 33 | * 34 | * Note that we update the sub-variables also. 35 | * 36 | * @param new_id entry in filter covariance corresponding to this variable 37 | */ 38 | void set_local_id(int new_id) override { 39 | _id = new_id; 40 | _pose->set_local_id(new_id); 41 | _v->set_local_id(_pose->id() + ((new_id != -1) ? _pose->size() : 0)); 42 | } 43 | 44 | /** 45 | * @brief Update q and p using a the HamiltonQuat update for orientation and vector update for position 46 | * 47 | * @param dx Correction vector (orientation then position) 48 | */ 49 | void update(const Eigen::VectorXd &dx) override { 50 | 51 | assert(dx.rows() == _size); 52 | Eigen::MatrixXd X = _value; 53 | X = Exp_SEK3(dx) * X; 54 | 55 | set_value(X); 56 | } 57 | 58 | /** 59 | * @brief Update q and p using a the HamiltonQuat msckf update for orientation and vector update for position 60 | * 61 | * @param dx Correction vector (orientation then position) 62 | */ 63 | void msckf_update(const Eigen::VectorXd &dx) override { 64 | 65 | assert(dx.rows() == _size); 66 | Eigen::MatrixXd X = _value; 67 | X.block(0, 0, 3, 3) = X.block(0, 0, 3, 3) * Exp_SO3(dx.block(0, 0, 3, 1)); 68 | X.block(0, 3, 3, 1) += dx.block(3, 0, 3, 1); 69 | X.block(0, 4, 3, 1) += dx.block(6, 0, 3, 1); 70 | 71 | set_value(X); 72 | } 73 | 74 | /** 75 | * @brief Sets the value of the estimate 76 | * @param new_value New value we should set 77 | */ 78 | void set_value(const Eigen::MatrixXd &new_value) override { set_value_internal(new_value); } 79 | 80 | std::shared_ptr clone() override { 81 | auto Clone = std::shared_ptr(new Posev()); 82 | Clone->set_value(value()); 83 | return Clone; 84 | } 85 | 86 | std::shared_ptr check_if_subvariable(const std::shared_ptr check) override { 87 | if (check == _pose) { 88 | return _pose; 89 | } else if (check == _pose->check_if_subvariable(check)) { 90 | return _pose->check_if_subvariable(check); 91 | } else if (check == _v) { 92 | return _v; 93 | } 94 | return nullptr; 95 | } 96 | 97 | /// Rotation access 98 | Eigen::Matrix Rot() const { return _pose->Rot(); } 99 | 100 | /// Quaternion access 101 | Eigen::Quaterniond quat() const { return _pose->quat(); } 102 | 103 | /// Position access 104 | Eigen::Matrix pos() const { return _pose->pos(); } 105 | 106 | /// Velocity access 107 | Eigen::Matrix vel() const { return _v->value(); } 108 | 109 | /// Pose type access 110 | std::shared_ptr pose() { return _pose; } 111 | 112 | /// Quaternion type access 113 | std::shared_ptr R() { return _pose->R(); } 114 | 115 | /// Position type access 116 | std::shared_ptr p() { return _pose->p(); } 117 | 118 | /// Velocity type access 119 | std::shared_ptr v() { return _v; } 120 | 121 | protected: 122 | std::shared_ptr _pose; 123 | std::shared_ptr _v; 124 | 125 | /** 126 | * @brief Sets the value of the estimate 127 | * @param new_value New value we should set 128 | */ 129 | void set_value_internal(const Eigen::MatrixXd &new_value) { 130 | 131 | assert(new_value.rows() == 5); 132 | assert(new_value.cols() == 5); 133 | 134 | // Set orientation value 135 | _pose->set_value(new_value.block(0, 0, 4, 4)); 136 | 137 | // Set position value 138 | _v->set_value(new_value.block(0, 4, 3, 1)); 139 | 140 | _value = new_value; 141 | } 142 | }; 143 | 144 | } // namespace night_voyager 145 | 146 | #endif -------------------------------------------------------------------------------- /include/core/Type.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef TYPE_H 12 | #define TYPE_H 13 | 14 | #include 15 | #include 16 | 17 | namespace night_voyager { 18 | class Type { 19 | public: 20 | /** 21 | * @brief Default constructor for our Type 22 | * 23 | * @param size_ degrees of freedom of variable (i.e., the size of the error state) 24 | */ 25 | Type(int size_) { _size = size_; } 26 | 27 | virtual ~Type(){}; 28 | 29 | /** 30 | * @brief Sets id used to track location of variable in the filter covariance 31 | * 32 | * Note that the minimum ID is -1 which says that the state is not in our covariance. 33 | * If the ID is larger than -1 then this is the index location in the covariance matrix. 34 | * 35 | * @param new_id entry in filter covariance corresponding to this variable 36 | */ 37 | virtual void set_local_id(int new_id) { _id = new_id; } 38 | 39 | /** 40 | * @brief Access to variable id (i.e. its location in the covariance) 41 | */ 42 | int id() { return _id; } 43 | 44 | /** 45 | * @brief Access to variable size (i.e. its error state size) 46 | */ 47 | int size() { return _size; } 48 | 49 | /** 50 | * @brief Update variable due to perturbation of error state 51 | * 52 | * @param dx Perturbation used to update the variable through a defined "boxplus" operation 53 | */ 54 | virtual void update(const Eigen::VectorXd &dx) = 0; 55 | 56 | /** 57 | * @brief MSCKF Update variable due to perturbation of error state 58 | * 59 | * @param dx Perturbation used to update the variable through a defined "boxplus" operation 60 | */ 61 | virtual void msckf_update(const Eigen::VectorXd &dx) = 0; 62 | 63 | /** 64 | * @brief Access variable's estimate 65 | */ 66 | virtual const Eigen::MatrixXd &value() const { return _value; } 67 | 68 | /** 69 | * @brief Overwrite value of state's estimate 70 | * @param new_value New value that will overwrite state's value 71 | */ 72 | virtual void set_value(const Eigen::MatrixXd &new_value) { 73 | assert(_value.rows() == new_value.rows()); 74 | assert(_value.cols() == new_value.cols()); 75 | _value = new_value; 76 | } 77 | 78 | /** 79 | * @brief Create a clone of this variable 80 | */ 81 | virtual std::shared_ptr clone() = 0; 82 | 83 | /** 84 | * @brief Determine if pass variable is a sub-variable 85 | * 86 | * If the passed variable is a sub-variable or the current variable this will return it. 87 | * Otherwise it will return a nullptr, meaning that it was unable to be found. 88 | * 89 | * @param check Type pointer to compare our subvariables to 90 | */ 91 | virtual std::shared_ptr check_if_subvariable(const std::shared_ptr check) { return nullptr; } 92 | 93 | protected: 94 | /// Current best estimate 95 | Eigen::MatrixXd _value; 96 | 97 | /// Location of error state in covariance 98 | int _id = -1; 99 | 100 | /// Dimension of error state 101 | int _size = -1; 102 | }; 103 | } // namespace night_voyager 104 | #endif -------------------------------------------------------------------------------- /include/core/Vec.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef TYPE_VEC_H 12 | #define TYPE_VEC_H 13 | 14 | #include "core/Type.h" 15 | 16 | /** 17 | * @brief Derived Type class that implements vector variables 18 | */ 19 | namespace night_voyager { 20 | class Vec : public Type { 21 | 22 | public: 23 | /** 24 | * @brief Default constructor for Vec 25 | * @param dim Size of the vector (will be same as error state) 26 | */ 27 | Vec(int dim) : Type(dim) { _value = Eigen::VectorXd::Zero(dim); } 28 | 29 | ~Vec() {} 30 | 31 | /** 32 | * @brief Implements the update operation through standard vector addition 33 | * 34 | * \f{align*}{ 35 | * \mathbf{v} &= \hat{\mathbf{v}} + \tilde{\mathbf{v}}_{dx} 36 | * \f} 37 | * 38 | * @param dx Additive error state correction 39 | */ 40 | void update(const Eigen::VectorXd &dx) override { 41 | assert(dx.rows() == _size); 42 | set_value(_value + dx); 43 | } 44 | 45 | /** 46 | * @brief Implements the update operation through standard vector addition 47 | * 48 | * \f{align*}{ 49 | * \mathbf{v} &= \hat{\mathbf{v}} + \tilde{\mathbf{v}}_{dx} 50 | * \f} 51 | * 52 | * @param dx Additive error state correction 53 | */ 54 | void msckf_update(const Eigen::VectorXd &dx) override { 55 | assert(dx.rows() == _size); 56 | set_value(_value + dx); 57 | } 58 | 59 | /** 60 | * @brief Performs all the cloning 61 | */ 62 | std::shared_ptr clone() override { 63 | auto Clone = std::shared_ptr(new Vec(_size)); 64 | Clone->set_value(value()); 65 | return Clone; 66 | } 67 | }; 68 | } // namespace night_voyager 69 | #endif -------------------------------------------------------------------------------- /include/feature_tracker/Feature.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef FEATURE_H 12 | #define FEATURE_H 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | namespace night_voyager { 20 | class Feature { 21 | 22 | public: 23 | /// Unique ID of this feature 24 | size_t featid; 25 | 26 | /// If this feature should be deleted 27 | bool to_delete; 28 | 29 | /// UV coordinates that this feature has been seen from (mapped by camera ID) 30 | std::vector uvs; 31 | 32 | /// UV normalized coordinates that this feature has been seen from (mapped by camera ID) 33 | std::vector uvs_norm; 34 | 35 | /// Timestamps of each UV measurement (mapped by camera ID) 36 | std::vector timestamps; 37 | 38 | /// Timestamp of anchor clone 39 | double anchor_clone_timestamp; 40 | 41 | /// Timestamp of pseudo anchor clone 42 | double pseudo_anchor_clone_timestamp = -1; 43 | 44 | /// Triangulated position of this feature, in the anchor frame 45 | Eigen::Vector3d p_FinA; 46 | 47 | /// Triangulated position of this feature, in the global frame 48 | Eigen::Vector3d p_FinG; 49 | 50 | /** 51 | * @brief Remove measurements that do not occur at passed timestamps. 52 | * 53 | * Given a series of valid timestamps, this will remove all measurements that have not occurred at these times. 54 | * This would normally be used to ensure that the measurements that we have occur at our clone times. 55 | * 56 | * @param valid_times Vector of timestamps that our measurements must occur at 57 | */ 58 | void clean_old_measurements(const std::vector &valid_times); 59 | 60 | /** 61 | * @brief Remove measurements that occur at the invalid timestamps 62 | * 63 | * Given a series of invalid timestamps, this will remove all measurements that have occurred at these times. 64 | * 65 | * @param invalid_times Vector of timestamps that our measurements should not 66 | */ 67 | void clean_invalid_measurements(const std::vector &invalid_times); 68 | 69 | /** 70 | * @brief Remove measurements that are older then the specified timestamp. 71 | * 72 | * Given a valid timestamp, this will remove all measurements that have occured earlier then this. 73 | * 74 | * @param timestamp Timestamps that our measurements must occur after 75 | */ 76 | void clean_older_measurements(double timestamp); 77 | }; 78 | } // namespace night_voyager 79 | #endif -------------------------------------------------------------------------------- /include/feature_tracker/FeatureDatabase.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef FEATURE_DATABASE_H 12 | #define FEATURE_DATABASE_H 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace night_voyager { 21 | class Feature; 22 | 23 | class FeatureDatabase { 24 | 25 | public: 26 | /** 27 | * @brief Default constructor 28 | */ 29 | FeatureDatabase() {} 30 | 31 | /** 32 | * @brief Get a specified feature 33 | * @param id What feature we want to get 34 | * @param remove Set to true if you want to remove the feature from the database (you will need to handle the freeing of memory) 35 | * @return Either a feature object, or null if it is not in the database. 36 | */ 37 | std::shared_ptr get_feature(size_t id, bool remove = false); 38 | 39 | /** 40 | * @brief Get a specified feature clone (pointer is thread safe) 41 | * @param id What feature we want to get 42 | * @param feat Feature with data in it 43 | * @return True if the feature was found 44 | */ 45 | bool get_feature_clone(size_t id, Feature &feat); 46 | 47 | /** 48 | * @brief Update a feature object 49 | * @param id ID of the feature we will update 50 | * @param timestamp time that this measurement occured at 51 | * @param u raw u coordinate 52 | * @param v raw v coordinate 53 | * @param u_n undistorted/normalized u coordinate 54 | * @param v_n undistorted/normalized v coordinate 55 | * 56 | * This will update a given feature based on the passed ID it has. 57 | * It will create a new feature, if it is an ID that we have not seen before. 58 | */ 59 | void update_feature(size_t id, double timestamp, float u, float v, float u_n, float v_n); 60 | 61 | /** 62 | * @brief Get features that do not have newer measurement then the specified time. 63 | * 64 | * This function will return all features that do not a measurement at a time greater than the specified time. 65 | * For example this could be used to get features that have not been successfully tracked into the newest frame. 66 | * All features returned will not have any measurements occurring at a time greater then the specified. 67 | */ 68 | std::vector> features_not_containing_newer(double timestamp, bool remove = false, bool skip_deleted = false); 69 | 70 | /** 71 | * @brief Get features that has measurements older then the specified time. 72 | * 73 | * This will collect all features that have measurements occurring before the specified timestamp. 74 | * For example, we would want to remove all features older then the last clone/state in our sliding window. 75 | */ 76 | std::vector> features_containing_older(double timestamp, bool remove = false, bool skip_deleted = false); 77 | 78 | /** 79 | * @brief Get features that has measurements at the specified time. 80 | * 81 | * This function will return all features that have the specified time in them. 82 | * This would be used to get all features that occurred at a specific clone/state. 83 | */ 84 | std::vector> features_containing(double timestamp, bool remove = false, bool skip_deleted = false); 85 | 86 | /** 87 | * @brief This function will delete all features that have been used up. 88 | * 89 | * If a feature was unable to be used, it will still remain since it will not have a delete flag set 90 | */ 91 | void cleanup(); 92 | 93 | /** 94 | * @brief This function will delete all feature measurements that are older then the specified timestamp 95 | */ 96 | void cleanup_measurements(double timestamp); 97 | 98 | /** 99 | * @brief This function will delete all feature measurements that are at the specified timestamp 100 | */ 101 | void cleanup_measurements_exact(double timestamp); 102 | 103 | /** 104 | * @brief Returns the size of the feature database 105 | */ 106 | size_t size() { 107 | std::lock_guard lck(mtx); 108 | return features_idlookup.size(); 109 | } 110 | 111 | /** 112 | * @brief Returns the internal data (should not normally be used) 113 | */ 114 | std::unordered_map> get_internal_data() { 115 | std::lock_guard lck(mtx); 116 | return features_idlookup; 117 | } 118 | 119 | /** 120 | * @brief Gets the oldest time in the database 121 | */ 122 | double get_oldest_timestamp(); 123 | 124 | /** 125 | * @brief Will update the passed database with this database's latest feature information. 126 | */ 127 | void append_new_measurements(const std::shared_ptr &database); 128 | 129 | protected: 130 | /// Mutex lock for our map 131 | std::mutex mtx; 132 | 133 | /// Our lookup array that allow use to query based on ID 134 | std::unordered_map> features_idlookup; 135 | }; 136 | } // namespace night_voyager 137 | #endif -------------------------------------------------------------------------------- /include/feature_tracker/FeatureHelper.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef FEATURE_HELPER_H 12 | #define FEATURE_HELPER_H 13 | 14 | #include "feature_tracker/Feature.h" 15 | #include "feature_tracker/FeatureDatabase.h" 16 | 17 | namespace night_voyager { 18 | class FeatureHelper { 19 | 20 | public: 21 | /** 22 | * @brief This functions will compute the disparity between common features in the two frames. 23 | * 24 | * First we find all features in the first frame. 25 | * Then we loop through each and find the uv of it in the next requested frame. 26 | * Features are skipped if no tracked feature is found (it was lost). 27 | * NOTE: this is on the RAW coordinates of the feature not the normalized ones. 28 | * NOTE: This computes the disparity over all cameras! 29 | * 30 | * @param db Feature database pointer 31 | * @param time0 First camera frame timestamp 32 | * @param time1 Second camera frame timestamp 33 | * @param disp_mean Average raw disparity 34 | * @param disp_var Variance of the disparities 35 | * @param total_feats Total number of common features 36 | */ 37 | static void compute_disparity(std::shared_ptr db, double time0, double time1, double &disp_mean, double &disp_var, 38 | int &total_feats) { 39 | 40 | // Get features seen from the first image 41 | std::vector> feats0 = db->features_containing(time0, false, true); 42 | 43 | // Compute the disparity 44 | std::vector disparities; 45 | for (auto &feat : feats0) { 46 | 47 | // Get the two uvs for both times 48 | auto it0 = std::find(feat->timestamps.begin(), feat->timestamps.end(), time0); 49 | auto it1 = std::find(feat->timestamps.begin(), feat->timestamps.end(), time1); 50 | if (it0 == feat->timestamps.end() || it1 == feat->timestamps.end()) 51 | continue; 52 | auto idx0 = std::distance(feat->timestamps.begin(), it0); 53 | auto idx1 = std::distance(feat->timestamps.begin(), it1); 54 | 55 | // Now lets calculate the disparity 56 | Eigen::Vector2f uv0 = feat->uvs.at(idx0).block(0, 0, 2, 1); 57 | Eigen::Vector2f uv1 = feat->uvs.at(idx1).block(0, 0, 2, 1); 58 | disparities.push_back((uv1 - uv0).norm()); 59 | } 60 | 61 | // If no disparities, just return 62 | if (disparities.size() < 2) { 63 | disp_mean = -1; 64 | disp_var = -1; 65 | total_feats = 0; 66 | } 67 | 68 | // Compute mean and standard deviation in respect to it 69 | disp_mean = 0; 70 | for (double disp_i : disparities) { 71 | disp_mean += disp_i; 72 | } 73 | disp_mean /= (double)disparities.size(); 74 | disp_var = 0; 75 | for (double &disp_i : disparities) { 76 | disp_var += std::pow(disp_i - disp_mean, 2); 77 | } 78 | disp_var = std::sqrt(disp_var / (double)(disparities.size() - 1)); 79 | total_feats = (int)disparities.size(); 80 | } 81 | 82 | /** 83 | * @brief This functions will compute the disparity over all features we have 84 | * 85 | * NOTE: this is on the RAW coordinates of the feature not the normalized ones. 86 | * NOTE: This computes the disparity over all cameras! 87 | * 88 | * @param db Feature database pointer 89 | * @param disp_mean Average raw disparity 90 | * @param disp_var Variance of the disparities 91 | * @param total_feats Total number of common features 92 | * @param newest_time Only compute disparity for ones older (-1 to disable) 93 | * @param oldest_time Only compute disparity for ones newer (-1 to disable) 94 | */ 95 | static void compute_disparity(std::shared_ptr db, double &disp_mean, double &disp_var, int &total_feats, double newest_time = -1, 96 | double oldest_time = -1) { 97 | 98 | // Compute the disparity 99 | std::vector disparities; 100 | for (auto &feat : db->get_internal_data()) { 101 | 102 | // Skip if only one observation 103 | if (feat.second->timestamps.size() < 2) 104 | continue; 105 | 106 | // Now lets calculate the disparity (assumes time array is monotonic) 107 | bool found0 = false; 108 | bool found1 = false; 109 | Eigen::Vector2f uv0 = Eigen::Vector2f::Zero(); 110 | Eigen::Vector2f uv1 = Eigen::Vector2f::Zero(); 111 | for (size_t idx = 0; idx < feat.second->timestamps.size(); idx++) { 112 | double time = feat.second->timestamps.at(idx); 113 | if ((oldest_time == -1 || time > oldest_time) && !found0) { 114 | uv0 = feat.second->uvs.at(idx).block(0, 0, 2, 1); 115 | found0 = true; 116 | continue; 117 | } 118 | if ((newest_time == -1 || time < newest_time) && found0) { 119 | uv1 = feat.second->uvs.at(idx).block(0, 0, 2, 1); 120 | found1 = true; 121 | continue; 122 | } 123 | } 124 | 125 | // If we found both an old and a new time, then we are good! 126 | if (!found0 || !found1) 127 | continue; 128 | disparities.push_back((uv1 - uv0).norm()); 129 | } 130 | 131 | // If no disparities, just return 132 | if (disparities.size() < 2) { 133 | disp_mean = -1; 134 | disp_var = -1; 135 | total_feats = 0; 136 | } 137 | 138 | // Compute mean and standard deviation in respect to it 139 | disp_mean = 0; 140 | for (double disp_i : disparities) { 141 | disp_mean += disp_i; 142 | } 143 | disp_mean /= (double)disparities.size(); 144 | disp_var = 0; 145 | for (double &disp_i : disparities) { 146 | disp_var += std::pow(disp_i - disp_mean, 2); 147 | } 148 | disp_var = std::sqrt(disp_var / (double)(disparities.size() - 1)); 149 | total_feats = (int)disparities.size(); 150 | } 151 | 152 | private: 153 | // Cannot construct this class 154 | FeatureHelper() {} 155 | }; 156 | } // namespace night_voyager 157 | #endif -------------------------------------------------------------------------------- /include/feature_tracker/FeatureInitializer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef OPEN_VINS_FEATUREINITIALIZER_H 12 | #define OPEN_VINS_FEATUREINITIALIZER_H 13 | 14 | #include 15 | 16 | #include "core/NightVoyagerOptions.h" 17 | 18 | namespace night_voyager { 19 | class Feature; 20 | 21 | /** 22 | * @brief Class that triangulates feature 23 | * 24 | * This class has the functions needed to triangulate and then refine a given 3D feature. 25 | * As in the standard MSCKF, we know the clones of the camera from propagation and past updates. 26 | * Thus, we just need to triangulate a feature in 3D with the known poses and then refine it. 27 | * One should first call the single_triangulation() function afterwhich single_gaussnewton() allows for refinement. 28 | * Please see the @ref update-featinit page for detailed derivations. 29 | */ 30 | class FeatureInitializer { 31 | 32 | public: 33 | /** 34 | * @brief Structure which stores pose estimates for use in triangulation 35 | * 36 | * - R_GtoC - rotation from global to camera 37 | * - p_CinG - position of camera in global frame 38 | */ 39 | struct ClonePose { 40 | 41 | /// Rotation 42 | Eigen::Matrix _Rot; 43 | 44 | /// Position 45 | Eigen::Matrix _pos; 46 | 47 | /// Constructs pose from rotation and position 48 | ClonePose(const Eigen::Matrix &R, const Eigen::Matrix &p) { 49 | _Rot = R; 50 | _pos = p; 51 | } 52 | 53 | /// Default constructor 54 | ClonePose() { 55 | _Rot = Eigen::Matrix::Identity(); 56 | _pos = Eigen::Matrix::Zero(); 57 | } 58 | 59 | /// Accessor for rotation 60 | const Eigen::Matrix &Rot() { return _Rot; } 61 | 62 | /// Accessor for position 63 | const Eigen::Matrix &pos() { return _pos; } 64 | }; 65 | 66 | /** 67 | * @brief Default constructor 68 | * @param options Options for the initializer 69 | */ 70 | FeatureInitializer(FeatureInitializerOptions &options) : _options(options) {} 71 | 72 | /** 73 | * @brief Uses a linear triangulation to get initial estimate for the feature 74 | * 75 | * The derivations for this method can be found in the @ref featinit-linear documentation page. 76 | * 77 | * @param feat Pointer to feature 78 | * @param clonesCAM Map of timestamp to camera pose estimate (rotation from global to camera, position of camera 79 | * in global frame) 80 | * @return Returns false if it fails to triangulate (based on the thresholds) 81 | */ 82 | bool single_triangulation(std::shared_ptr feat, std::unordered_map &clonesCAM); 83 | 84 | /** 85 | * @brief Uses a linear triangulation to get initial estimate for the feature, treating the anchor observation as a true bearing. 86 | * 87 | * The derivations for this method can be found in the @ref featinit-linear-1d documentation page. 88 | * This function should be used if you want speed, or know your anchor bearing is reasonably accurate. 89 | * 90 | * @param feat Pointer to feature 91 | * @param clonesCAM Map of timestamp to camera pose estimate (rotation from global to camera, position of camera 92 | * in global frame) 93 | * @return Returns false if it fails to triangulate (based on the thresholds) 94 | */ 95 | bool single_triangulation_1d(std::shared_ptr feat, std::unordered_map &clonesCAM); 96 | 97 | /** 98 | * @brief Uses a nonlinear triangulation to refine initial linear estimate of the feature 99 | * @param feat Pointer to feature 100 | * @param clonesCAM Map of timestamp to camera pose estimate (rotation from global to camera, position of camera 101 | * in global frame) 102 | * @return Returns false if it fails to be optimize (based on the thresholds) 103 | */ 104 | bool single_gaussnewton(std::shared_ptr feat, std::unordered_map &clonesCAM); 105 | 106 | /** 107 | * @brief Gets the current configuration of the feature initializer 108 | * @return Const feature initializer config 109 | */ 110 | const FeatureInitializerOptions config() { return _options; } 111 | 112 | protected: 113 | /// Contains options for the initializer process 114 | FeatureInitializerOptions _options; 115 | 116 | /** 117 | * @brief Helper function for the gauss newton method that computes error of the given estimate 118 | * @param clonesCAM Map of timestamp to camera pose estimate 119 | * @param feat Pointer to the feature 120 | * @param alpha x/z in anchor 121 | * @param beta y/z in anchor 122 | * @param rho 1/z inverse depth 123 | */ 124 | double compute_error(std::unordered_map &clonesCAM, std::shared_ptr feat, double alpha, double beta, double rho); 125 | }; 126 | } // namespace night_voyager 127 | 128 | #endif -------------------------------------------------------------------------------- /include/feature_tracker/TrackKLT.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef TRACKKLT_H 12 | #define TRACKKLT_H 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include "core/NightVoyagerOptions.h" 27 | #include "feature_tracker/FeatureDatabase.h" 28 | 29 | namespace night_voyager { 30 | class CamBase; 31 | 32 | class TrackKLT { 33 | public: 34 | explicit TrackKLT(const NightVoyagerOptions &options, std::shared_ptr &camera_intrinsic) 35 | : num_features(options.init_options.init_max_features), camera_calib(camera_intrinsic), histogram_method(options.histogram_method), 36 | threshold(options.fast_threshold), grid_x(options.grid_x), grid_y(options.grid_y), database(std::make_shared()), 37 | min_px_dist(options.min_px_dist) { 38 | currid = 1; 39 | // cout << "num_features: " << num_features; 40 | } 41 | 42 | void feed_new_camera(const CameraData &message); 43 | 44 | shared_ptr get_feature_database() { return database; } 45 | 46 | /// Getter method for number of active features 47 | int get_num_features() { return num_features; } 48 | 49 | /// Setter method for number of active features 50 | void set_num_features(int _num_features) { num_features = _num_features; } 51 | 52 | void display_history(cv::Mat &img_out, int r1, int g1, int b1, int r2, int g2, int b2, std::vector highlighted, std::string overlay); 53 | 54 | protected: 55 | void feed_monocular(const CameraData &message); 56 | 57 | void perform_detection_monocular(const std::vector &img0pyr, std::vector &pts0, std::vector &ids0); 58 | 59 | void perform_matching(const std::vector &img0pyr, const std::vector &img1pyr, std::vector &pts0, 60 | std::vector &pts1, std::vector &mask_out); 61 | 62 | // Number of features we should try to track frame to frame 63 | int num_features; 64 | 65 | std::shared_ptr camera_calib; 66 | 67 | /// What histogram equalization method we should pre-process images with? 68 | HistogramMethod histogram_method; 69 | 70 | // Parameters for our FAST grid detector 71 | int threshold; 72 | int grid_x; 73 | int grid_y; 74 | 75 | // Database with all our current features 76 | std::shared_ptr database; 77 | 78 | // Minimum pixel distance to be "far away enough" to be a different extracted feature 79 | int min_px_dist; 80 | int pyr_levels = 5; 81 | cv::Size win_size = cv::Size(15, 15); 82 | 83 | // Last set of image pyramids 84 | vector img_pyramid_last; 85 | cv::Mat img_curr; 86 | vector img_pyramid_curr; 87 | 88 | /// Mutexs for our last set of image storage (img_last, pts_last, and ids_last) 89 | std::mutex mtx_feed; 90 | 91 | /// Mutex for editing the *_last variables 92 | std::mutex mtx_last_vars; 93 | 94 | /// Last set of images (use map so all trackers render in the same order) 95 | cv::Mat img_last; 96 | 97 | /// Last set of tracked points 98 | std::vector pts_last; 99 | 100 | /// Set of IDs of each current feature in the database 101 | std::vector ids_last; 102 | 103 | /// Master ID for this tracker (atomic to allow for multi-threading) 104 | std::atomic currid; 105 | 106 | // Timing variables (most children use these...) 107 | boost::posix_time::ptime rT1, rT2, rT3, rT4, rT5, rT6, rT7; 108 | }; 109 | } // namespace night_voyager 110 | #endif -------------------------------------------------------------------------------- /include/initializer/InertialInitializer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef INERTIAL_INITIALIZER_H 12 | #define INERTIAL_INITIALIZER_H 13 | 14 | #include "core/CommonLib.h" 15 | #include "core/NightVoyagerOptions.h" 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | namespace night_voyager { 22 | class IMU; 23 | class Pose; 24 | class Type; 25 | class State; 26 | class FeatureDatabase; 27 | class StaticInitializer; 28 | class PcdManager; 29 | class PriorPoseManager; 30 | 31 | class InertialInitializer { 32 | public: 33 | struct Tuple { 34 | /// Indices of streetlight observations 35 | int o1, o2, o3; 36 | }; 37 | 38 | struct TupleScore { 39 | /// Indices of streetlight clusters 40 | int l1, l2, l3; 41 | 42 | /// Score of this combination 43 | double score = -1; 44 | 45 | /// 2D pose estimated from this combination, p_CinMAP, theta_CtoMAP 46 | Eigen::Vector3d pose; 47 | 48 | /// Tuple streetlight matches 49 | std::vector matches; 50 | }; 51 | 52 | struct PoseScore { 53 | double score = -10000; 54 | double subscore = -10000; 55 | 56 | Eigen::Matrix3d R; 57 | Eigen::Vector3d p; 58 | std::vector matches; 59 | int region; 60 | }; 61 | 62 | /** 63 | * @brief Default constructor 64 | * @param options Parameters loaded from either ROS or CMDLINE 65 | * @param db Feature tracker database with all features in it 66 | */ 67 | explicit InertialInitializer(NightVoyagerOptions &options, std::shared_ptr db, std::shared_ptr pcd, 68 | std::shared_ptr prpose, std::shared_ptr &camera_intrinsic); 69 | 70 | /** 71 | * @brief Feed function for inertial data 72 | * @param message Contains our timestamp and inertial information 73 | * @param oldest_time Time that we can discard measurements before 74 | */ 75 | void feed_imu(const ImuData &message, double oldest_time = -1); 76 | 77 | /** 78 | * @brief Feed function for Box data 79 | * @param message Contains our timestamp and box information 80 | * @param oldest_time Time that we can discard measurements before 81 | */ 82 | void feed_box(const BoxData &message, double oldest_time = -1); 83 | 84 | /** 85 | * @brief Try to get the initialized system 86 | * 87 | * 88 | * @m_class{m-note m-warning} 89 | * 90 | * @par Processing Cost 91 | * This is a serial process that can take on orders of seconds to complete. 92 | * If you are a real-time application then you will likely want to call this from 93 | * a async thread which allows for this to process in the background. 94 | * The features used are cloned from the feature database thus should be thread-safe 95 | * to continue to append new feature tracks to the database. 96 | * 97 | * @param[out] timestamp Timestamp we have initialized the state at 98 | * @param[out] covariance Calculated covariance of the returned state 99 | * @param[out] order Order of the covariance matrix 100 | * @param[out] t_imu Our imu type (need to have correct ids) 101 | * @return True if we have successfully initialized our system 102 | */ 103 | void initialize_viwo(double ×tamp, Eigen::MatrixXd &covariance_viwo, std::vector> &order_viwo, 104 | std::shared_ptr t_imu); 105 | 106 | void initialize_map(const PackData &pack_queue, const Eigen::Matrix4d &T_ItoC, const Eigen::Matrix4d &T_ItoG, 107 | const Eigen::MatrixXd &covariance_viwo_pose, Eigen::MatrixXd &T_MAPtoLOC, Eigen::MatrixXd &covariance_map); 108 | 109 | void process_box(const CameraData &img, BoxData &box); 110 | 111 | void display_streetlights(cv::Mat &img_out, string overlay); 112 | 113 | bool viwo_initialized() { return initialized_viwo; } 114 | 115 | bool map_initialized() { return initialized_map; } 116 | 117 | protected: 118 | /// Initialization parameters 119 | InertialInitializerOptions _options; 120 | 121 | /// Feature tracker database with all features in it 122 | std::shared_ptr _db; 123 | 124 | /// PcdManager with all map points 125 | std::shared_ptr _pcd; 126 | 127 | /// PriorposeManager with downsampled poses 128 | std::shared_ptr _prpose; 129 | 130 | bool initialized_viwo, initialized_map; 131 | 132 | std::shared_ptr camera_calib; 133 | 134 | /// Our history of IMU messages (time, angular, linear) 135 | std::shared_ptr> imu_data; 136 | 137 | /// Our history of box messages 138 | std::vector box_data; 139 | std::mutex box_data_mtx; 140 | 141 | /// Static initialization helper class 142 | std::shared_ptr init_static; 143 | 144 | /// Used to store the selected box measruement (static initializer) 145 | BoxData selected_box_data; 146 | BoxData unselected_box_data; 147 | 148 | std::vector region_samples; 149 | std::vector> stidx_in_regions; 150 | std::vector> combs_in_region; 151 | }; 152 | } // namespace night_voyager 153 | 154 | #endif -------------------------------------------------------------------------------- /include/initializer/StaticInitializer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | /* 12 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 13 | * Copyright (C) 2025 Night-Voyager Contributors 14 | * 15 | * For technical issues and support, please contact Tianxiao Gao at 16 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 17 | * 18 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 19 | * which is included as part of this source code package. 20 | */ 21 | #ifndef STATICINITIALIZER_H 22 | #define STATICINITIALIZER_H 23 | 24 | #include "core/CommonLib.h" 25 | #include "core/NightVoyagerOptions.h" 26 | 27 | namespace night_voyager { 28 | class IMU; 29 | class Type; 30 | class FeatureDatabase; 31 | 32 | class StaticInitializer { 33 | public: 34 | /** 35 | * @brief Default constructor 36 | * @param options Parameters loaded from either ROS or CMDLINE 37 | * @param db Feature tracker database with all features in it 38 | * @param imu_data_ Shared pointer to our IMU vector of historical information 39 | */ 40 | explicit StaticInitializer(InertialInitializerOptions &options, std::shared_ptr db, 41 | std::shared_ptr> imu_data_) 42 | : _options(options), _db(db), imu_data(imu_data_) {} 43 | 44 | /** 45 | * @brief Try to get the initialized system using just the imu 46 | * 47 | * This will check if we have had a large enough jump in our acceleration. 48 | * If we have then we will use the period of time before this jump to initialize the state. 49 | * This assumes that our imu is sitting still and is not moving (so this would fail if we are experiencing constant acceleration). 50 | * 51 | * In the case that we do not wait for a jump (i.e. `wait_for_jerk` is false), then the system will try to initialize as soon as possible. 52 | * This is only recommended if you have zero velocity update enabled to handle the stationary cases. 53 | * To initialize in this case, we need to have the average angular variance be below the set threshold (i.e. we need to be stationary). 54 | * 55 | * @param[out] timestamp Timestamp we have initialized the state at 56 | * @param[out] covariance Calculated covariance of the returned state 57 | * @param[out] order Order of the covariance matrix 58 | * @param[out] t_imu Our imu type element 59 | * @return True if we have successfully initialized our system 60 | */ 61 | bool initialize(double ×tamp, Eigen::MatrixXd &covariance, std::vector> &order, std::shared_ptr t_imu); 62 | 63 | private: 64 | /// Initialization parameters 65 | InertialInitializerOptions _options; 66 | 67 | /// Feature tracker database with all features in it 68 | std::shared_ptr _db; 69 | 70 | /// Our history of IMU messages (time, angular, linear) 71 | std::shared_ptr> imu_data; 72 | }; 73 | } // namespace night_voyager 74 | #endif -------------------------------------------------------------------------------- /include/msckf_iekf/UpdaterMSCKF.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef UPDATER_MSCKF_H 12 | #define UPDATER_MSCKF_H 13 | 14 | #include "core/NightVoyagerOptions.h" 15 | #include 16 | #include 17 | 18 | namespace night_voyager { 19 | class State; 20 | class Feature; 21 | class FeatureInitializer; 22 | 23 | class UpdaterMSCKF { 24 | 25 | public: 26 | /** 27 | * @brief Default constructor for our MSCKF updater 28 | * 29 | * Our updater has a feature initializer which we use to initialize features as needed. 30 | * Also the options allow for one to tune the different parameters for update. 31 | */ 32 | UpdaterMSCKF(NightVoyagerOptions &options); 33 | 34 | /** 35 | * @brief Given tracked features, this will try to use them to update the state. 36 | * 37 | * @param state State of the filter 38 | * @param feature_vec Features that can be used for update 39 | */ 40 | void update(std::shared_ptr state, std::vector> &feature_vec); 41 | 42 | protected: 43 | /// Options used during update 44 | UpdaterOptions _options; 45 | 46 | /// Feature initializer class object 47 | std::shared_ptr initializer_feat; 48 | 49 | /// Chi squared 95th percentile table (lookup would be size of residual) 50 | std::map chi_squared_table; 51 | }; 52 | } // namespace night_voyager 53 | #endif -------------------------------------------------------------------------------- /include/msckf_iekf/UpdaterOdom.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef UPDATER_ODOM_H 12 | #define UPDATER_ODOM_H 13 | 14 | #include "core/CommonLib.h" 15 | #include "core/NightVoyagerOptions.h" 16 | 17 | namespace night_voyager { 18 | 19 | class State; 20 | 21 | class UpdaterOdom { 22 | 23 | public: 24 | UpdaterOdom(NightVoyagerOptions &options); 25 | 26 | void update(std::shared_ptr state, const OdomData &odom); 27 | 28 | protected: 29 | /// Options used during update 30 | UpdaterOdomOptions _options; 31 | 32 | /// Chi squared 95th percentile table (lookup would be size of residual) 33 | std::map chi_squared_table; 34 | }; 35 | 36 | } // namespace night_voyager 37 | 38 | #endif -------------------------------------------------------------------------------- /include/msckf_iekf/UpdaterPlane.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef UPDATER_PLANE_H 12 | #define UPDATER_PLANE_H 13 | 14 | #include "core/NightVoyagerOptions.h" 15 | 16 | namespace night_voyager { 17 | 18 | class State; 19 | class PriorPoseManager; 20 | 21 | class UpdaterPlane { 22 | 23 | public: 24 | /** 25 | * @brief Default constructor for our Plane updater 26 | * 27 | * Our updater has options allow for one to tune the different parameters for update. 28 | * 29 | */ 30 | UpdaterPlane(NightVoyagerOptions &options, std::shared_ptr prpose); 31 | 32 | /** 33 | * @brief This will try to use the plane constraints to update the state. 34 | * @param state State of the filter 35 | */ 36 | void update(std::shared_ptr state); 37 | 38 | /** 39 | * @brief This will try to use the prior pose to update the state 40 | * @param state State of the filter 41 | */ 42 | void update_with_prior(std::shared_ptr state); 43 | 44 | /** 45 | * @brief This will try to use the prior pose to update the state and return the near prio poses 46 | * @param state State of the filter 47 | */ 48 | std::map update_with_prior_tracking_recover(std::shared_ptr state); 49 | 50 | void add_near_pose(const std::map &near_indices_to_store); 51 | 52 | protected: 53 | /// Options used during update for slam features 54 | UpdaterPlaneOptions _options_plane; 55 | 56 | /// Chi squared 95th percentile table (lookup would be size of residual) 57 | std::map chi_squared_table; 58 | 59 | /// Prior pose manager 60 | std::shared_ptr _prpose; 61 | 62 | /// Select clone poses for reduce computation cost 63 | int step = 5; 64 | 65 | Eigen::Matrix3d _Roi, _Rio; 66 | }; 67 | 68 | } // namespace night_voyager 69 | 70 | #endif -------------------------------------------------------------------------------- /include/msckf_iekf/UpdaterSLAM.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef UPDATER_SLAM_H 12 | #define UPDATER_SLAM_H 13 | 14 | #include "core/NightVoyagerOptions.h" 15 | 16 | namespace night_voyager { 17 | 18 | class State; 19 | class Feature; 20 | class Landmark; 21 | class FeatureInitializer; 22 | 23 | class UpdaterSLAM { 24 | 25 | public: 26 | /** 27 | * @brief Default constructor for our SLAM updater 28 | * 29 | * Our updater has a feature initializer which we use to initialize features as needed. 30 | * Also the options allow for one to tune the different parameters for update. 31 | * 32 | */ 33 | UpdaterSLAM(NightVoyagerOptions &options); 34 | 35 | /** 36 | * @brief Given tracked SLAM features, this will try to use them to update the state. 37 | * @param state State of the filter 38 | * @param feature_vec Features that can be used for update 39 | */ 40 | void update(std::shared_ptr state, std::vector> &feature_vec); 41 | 42 | /** 43 | * @brief Given max track features, this will try to use them to initialize them in the state. 44 | * @param state State of the filter 45 | * @param feature_vec Features that can be used for update 46 | */ 47 | void delayed_init(std::shared_ptr state, std::vector> &feature_vec); 48 | 49 | protected: 50 | /// Options used during update for slam features 51 | UpdaterOptions _options_slam; 52 | 53 | /// Feature initializer class object 54 | std::shared_ptr initializer_feat; 55 | 56 | /// Chi squared 95th percentile table (lookup would be size of residual) 57 | std::map chi_squared_table; 58 | }; 59 | 60 | } // namespace night_voyager 61 | 62 | #endif -------------------------------------------------------------------------------- /include/msckf_iekf/UpdaterZeroVelocity.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef UPDATER_ZEROVELOCITY_H 12 | #define UPDATER_ZEROVELOCITY_H 13 | 14 | #include "core/CommonLib.h" 15 | #include "core/NightVoyagerOptions.h" 16 | #include "utils/Transform.h" 17 | #include 18 | #include 19 | #include 20 | 21 | namespace night_voyager { 22 | 23 | class Feature; 24 | class FeatureDatabase; 25 | class State; 26 | class Propagator; 27 | class Landmark; 28 | class NoiseManager; 29 | 30 | class UpdaterZeroVelocity { 31 | public: 32 | /** 33 | * @brief Default constructor for our zero velocity detector and updater. 34 | * @param options Updater options (chi2 multiplier) 35 | * @param noises imu noise characteristics (continuous time) 36 | * @param db Feature tracker database with all features in it 37 | * @param prop Propagator class object which can predict the state forward in time 38 | * @param gravity_mag Global gravity magnitude of the system (normally 9.81) 39 | * @param zupt_max_velocity Max velocity we should consider to do a update with 40 | * @param zupt_noise_multiplier Multiplier of our IMU noise matrix (default should be 1.0) 41 | * @param zupt_max_disparity Max disparity we should consider to do a update with 42 | */ 43 | UpdaterZeroVelocity(const NightVoyagerOptions &options, std::shared_ptr db, std::shared_ptr prop); 44 | 45 | /** 46 | * @brief Feed function for inertial data 47 | * @param message Contains our timestamp and inertial information 48 | * @param oldest_time Time that we can discard measurements before 49 | */ 50 | void feed_imu(const ImuData &message, double oldest_time = -1) { 51 | 52 | // Append it to our vector 53 | imu_data.emplace_back(message); 54 | 55 | // Sort our imu data (handles any out of order measurements) 56 | // std::sort(imu_data.begin(), imu_data.end(), [](const IMUDATA i, const IMUDATA j) { 57 | // return i.timestamp < j.timestamp; 58 | //}); 59 | 60 | // Clean old measurements 61 | // std::cout << "ZVUPT: imu_data.size() " << imu_data.size() << std::endl; 62 | clean_old_imu_measurements(oldest_time - 0.10); 63 | } 64 | 65 | /** 66 | * @brief This will remove any IMU measurements that are older then the given measurement time 67 | * @param oldest_time Time that we can discard measurements before (in IMU clock) 68 | */ 69 | void clean_old_imu_measurements(double oldest_time) { 70 | if (oldest_time < 0) 71 | return; 72 | auto it0 = imu_data.begin(); 73 | while (it0 != imu_data.end()) { 74 | if (it0->timestamp < oldest_time) { 75 | it0 = imu_data.erase(it0); 76 | } else { 77 | it0++; 78 | } 79 | } 80 | } 81 | 82 | /** 83 | * @brief Will first detect if the system is zero velocity, then will update. 84 | * @param state State of the filter 85 | * @param timestamp Next camera or odom timestamp we want to see if we should propagate to. 86 | * @return True if the system is currently at zero velocity 87 | */ 88 | bool try_update(std::shared_ptr state, const PackData &message); 89 | 90 | protected: 91 | UpdaterOptions _options; 92 | 93 | /// Container for the imu noise values 94 | NoiseManager _noises; 95 | 96 | /// Feature tracker database with all features in it 97 | std::shared_ptr _db; 98 | 99 | /// Our propagator! 100 | std::shared_ptr _prop; 101 | 102 | /// Max velocity (m/s) that we should consider a zupt with(for odom) 103 | double _zupt_max_velocity_odom = 0.05; 104 | 105 | /// Max velocity (m/s) that we should consider a zupt with 106 | double _zupt_max_velocity = 1.0; 107 | 108 | /// Multiplier of our IMU noise matrix (default should be 1.0) 109 | double _zupt_noise_multiplier = 1.0; 110 | 111 | /// Max disparity (pixels) that we should consider a zupt with 112 | double _zupt_max_disparity = 1.0; 113 | 114 | /// Gravity vector 115 | Eigen::Vector3d _gravity; 116 | 117 | /// Chi squared 95th percentile table (lookup would be size of residual) 118 | std::map chi_squared_table; 119 | 120 | /// Our history of IMU messages (time, angular, linear) 121 | std::vector imu_data; 122 | 123 | /// Last timestamp we did zero velocity update with 124 | double last_zupt_state_timestamp = 0.0; 125 | 126 | /// Number of times we have called update 127 | int last_zupt_count = 0; 128 | }; 129 | } // namespace night_voyager 130 | #endif -------------------------------------------------------------------------------- /include/msckf_iekf/VILManager.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef VILMANAGER_H 12 | #define VILMANAGER_H 13 | 14 | #include "core/CommonLib.h" 15 | #include "core/NightVoyagerOptions.h" 16 | #include "msckf_iekf/Propagator.h" 17 | #include 18 | #include 19 | #include 20 | 21 | namespace night_voyager { 22 | 23 | class TrackKLT; 24 | class PcdManager; 25 | class PriorPoseManager; 26 | class UpdaterMSCKF; 27 | class UpdaterSLAM; 28 | class UpdaterOdom; 29 | class UpdaterMAP; 30 | class UpdaterZeroVelocity; 31 | class UpdaterPlane; 32 | class Propagator; 33 | class State; 34 | class InertialInitializer; 35 | class TrackingRecover; 36 | 37 | class VILManager { 38 | public: 39 | VILManager(NightVoyagerOptions &options, std::shared_ptr pcd, std::shared_ptr prpose); 40 | 41 | void feed_measurement_all(const PackData &message); 42 | 43 | void feed_measurement_box(const BoxData &message); 44 | 45 | bool try_to_initialize_imu(const PackData &message); 46 | 47 | bool try_to_initialize_map(const PackData &message); 48 | 49 | void init_thread_imu(); 50 | 51 | void init_thread_map(const PackData &message, const Eigen::Matrix4d &T_ItoC, const Eigen::Matrix4d &T_ItoG, 52 | const Eigen::MatrixXd &covariance_viwo_pose); 53 | 54 | void postprocess_after_initviwo(); 55 | 56 | void do_feature_propagate_update(const PackData &message, const ImuData &last_imu); 57 | 58 | void do_feature_propagate_update_tracking_recover(const PackData &message, const ImuData &last_imu); 59 | 60 | /// Get a nice visualization image of what tracks we have 61 | bool get_historical_viz_image(cv::Mat &img_history); 62 | 63 | /// Get a nice visualization image of what matched streetlight detections we have 64 | bool get_streetlight_viz_image_init(cv::Mat &img_streetlight1, cv::Mat &img_streetlight2); 65 | 66 | bool get_streetlight_viz_image_tracking_recover(cv::Mat &img_streetlight1, cv::Mat &img_streetlight2); 67 | 68 | bool get_other_viz_image_tracking_recover(cv::Mat &img_streetlight); 69 | 70 | /// Get a nice visualization image of what streetlight detections we have 71 | bool get_streetlight_viz_image_not_init(cv::Mat &img_streetlight); 72 | 73 | bool get_streetlight_detection_viz_image(cv::Mat &img_streetlight); 74 | 75 | /// Accessor to get the current state 76 | std::shared_ptr get_state() { return state; } 77 | 78 | /// Accessor to get the tracking recover state 79 | std::shared_ptr get_state_tracking_recover(); 80 | 81 | std::vector> get_all_states_tracking_recover(); 82 | 83 | /// Accessor to get the current pcdmanager 84 | std::shared_ptr get_pcd_manager() { return _pcd; } 85 | 86 | /// Accessor to get the current prior pose manager 87 | std::shared_ptr get_prpose_manager() { return _prpose; } 88 | 89 | /// If we are initialized or not (IMU) 90 | bool initialized_imu() { return is_initialized_imu; } // && timelastupdate != -1; 91 | 92 | /// If we are initialized or not 93 | bool initialized_map() { return is_initialized_map; } 94 | 95 | /// Returns 3d SLAM features in the global frame 96 | std::vector> get_features_SLAM(); 97 | 98 | /// Returns 3d features used in the last update in global frame 99 | std::vector> get_good_features_MSCKF() { return good_features_MSCKF; } 100 | 101 | bool is_tracking_lost(); 102 | 103 | std::map get_rots_global_in_tracking_lost(); 104 | 105 | std::map get_poss_global_in_tracking_lost(); 106 | 107 | std::map get_rots_loc_in_tracking_lost(); 108 | 109 | std::map get_poss_loc_in_tracking_lost(); 110 | 111 | bool no_streetlight; 112 | 113 | double time_no_map; 114 | 115 | protected: 116 | NightVoyagerOptions options; 117 | 118 | /// Our master state object :D 119 | std::shared_ptr state; 120 | 121 | std::shared_ptr tracking_recover; 122 | 123 | std::shared_ptr _pcd; 124 | std::shared_ptr _prpose; 125 | 126 | shared_ptr propagator; 127 | 128 | // Our sparse feature tracker (klt) 129 | shared_ptr trackFEATS; 130 | 131 | /// State initializer 132 | std::shared_ptr initializer; 133 | 134 | /// Our MSCKF feature updater 135 | std::shared_ptr updaterMSCKF; 136 | 137 | /// Our SLAM feature updater 138 | std::shared_ptr updaterSLAM; 139 | 140 | /// Our Odom updater 141 | std::shared_ptr updaterOdom; 142 | 143 | /// Our streetlight feature updater 144 | std::shared_ptr updaterMAP; 145 | 146 | /// Our plane updater 147 | std::shared_ptr updaterPlane; 148 | 149 | /// Our zero velocity tracker 150 | std::shared_ptr updaterZUPT; 151 | 152 | bool is_initialized_map = false, is_initialized_imu = false; 153 | 154 | atomic thread_init_imu_running, thread_init_imu_success, thread_init_map_running, thread_init_map_success; 155 | 156 | // double timelastupdate = -1; 157 | 158 | // If we did a zero velocity update 159 | bool did_zupt_update = false; 160 | bool has_moved_since_zupt = false; 161 | 162 | /// This is the queue of measurement times that have come in since we starting doing initialization 163 | /// After we initialize, we will want to prop & update to the latest timestamp quickly 164 | std::vector camera_queue_init; 165 | std::vector odom_queue_init; 166 | std::vector pack_imu_queue_init; 167 | std::mutex camera_queue_init_mtx; 168 | std::mutex odom_queue_init_mtx; 169 | std::mutex pack_imu_queue_init_mtx; 170 | 171 | boost::posix_time::ptime rT1, rT2, rT3, rT4, rT5, rT6, rT7, rT8, rT9, rT10, rT11, rT12; 172 | 173 | // Startup time of the filter 174 | double startup_time = -1; 175 | 176 | // Good features that where used in the last update (used in visualization) 177 | std::vector> good_features_MSCKF; 178 | }; 179 | } // namespace night_voyager 180 | #endif -------------------------------------------------------------------------------- /include/streetlight_matcher/BIMatcher.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef BIMATCHER_H 12 | #define BIMATCHER_H 13 | 14 | #include "core/CommonLib.h" 15 | #include "core/NightVoyagerOptions.h" 16 | #include 17 | #include 18 | #include 19 | 20 | namespace night_voyager { 21 | 22 | class PcdManager; 23 | 24 | class BIMatcher { 25 | public: 26 | BIMatcher(NightVoyagerOptions &options, std::shared_ptr camera_intrinsic) 27 | : update_z_th(options.match_options.update_z_th), extend(options.match_options.extend), bi_filter(options.match_options.bi_filter), 28 | grey_th(options.match_options.grey_th), grey_th_low(options.match_options.grey_th_low), ang_th(options.match_options.ang_th_bi), 29 | large_off(options.match_options.large_off), large_extend(options.match_options.large_extend), 30 | remain_match(options.match_options.remain_match), cam(camera_intrinsic) {} 31 | 32 | vector match(const CameraData &img, std::shared_ptr pcd, const vector &matches, Eigen::Matrix3d &R_MAPtoC, 33 | Eigen::Vector3d &p_MAPinC); 34 | 35 | vector search_streetlight(std::shared_ptr pcd, const vector &matches, Eigen::Matrix3d &R_MAPtoC, 36 | Eigen::Vector3d &p_MAPinC, double min_dist, double max_dist); 37 | 38 | vector binary_segment(const CameraData &img, const vector &matches, bool is_low = false); 39 | 40 | vector search_matches(const vector &pre_boxes, const Eigen::Matrix3d &R_MAPtoC, const Eigen::Vector3d &p_MAPinC, 41 | const vector &STs, std::shared_ptr pcd, int off, bool use_large_extend = false); 42 | 43 | void match_filter(std::vector &matches, bool remain_match); 44 | 45 | std::vector get_boxes() { return bi_boxes; } 46 | 47 | protected: 48 | /// Threshold for searching streetlights 49 | double update_z_th; 50 | 51 | /// For expanding the projected center 52 | int extend; 53 | 54 | bool bi_filter; 55 | 56 | /// For searching more boxes using binary method 57 | int grey_th; 58 | 59 | int grey_th_low; 60 | 61 | double ang_th; 62 | 63 | int large_off; 64 | 65 | int large_extend; 66 | 67 | bool remain_match; 68 | 69 | std::shared_ptr cam; 70 | 71 | vector all_STs; 72 | 73 | std::vector bi_boxes; 74 | }; 75 | 76 | } // namespace night_voyager 77 | 78 | #endif -------------------------------------------------------------------------------- /include/streetlight_matcher/DLMatcher.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef DLMATCHER_H 12 | #define DLMATCHER_H 13 | 14 | #include "core/CommonLib.h" 15 | #include "core/NightVoyagerOptions.h" 16 | 17 | namespace night_voyager { 18 | 19 | class STMatch; 20 | class PcdManager; 21 | 22 | class DLMatcher { 23 | public: 24 | DLMatcher(NightVoyagerOptions &options, std::shared_ptr camera_intrinsic) 25 | : z_th(options.match_options.z_th), dl_filter(options.match_options.dl_filter), alpha(options.match_options.alpha), 26 | ang_th(options.match_options.ang_th_dl), cam(camera_intrinsic) {} 27 | 28 | vector match(const BoxData &boxes, std::shared_ptr pcd, Eigen::Matrix3d &R_MAPtoC, Eigen::Vector3d &p_MAPinC, 29 | Eigen::MatrixXd &pose_cov); 30 | 31 | vector match_tracking_recover(const BoxData &boxes, std::shared_ptr pcd, Eigen::Matrix3d &R_MAPtoC, 32 | Eigen::Vector3d &p_MAPinC, double reloc_z_th, int reloc_extend); 33 | 34 | vector match_box(const BoxData &boxes, std::shared_ptr pcd, vector &matches, Eigen::Matrix3d &R_MAPtoC, 35 | Eigen::Vector3d &p_MAPinC); 36 | 37 | double md_distance(const Eigen::Vector2f &box_center, const Eigen::Vector2d &proj_center, const Eigen::Matrix2d &cov_box_center); 38 | 39 | double ang_distance(const Eigen::Vector2f &box_center, const Eigen::Vector3d &streetlight_center, const Eigen::Matrix3d &cov_streetlight_center, 40 | const Eigen::Matrix2d &cov_box_center); 41 | 42 | vector search_streetlight(std::shared_ptr pcd, const Eigen::Matrix3d &R_MAPtoC, const Eigen::Vector3d &p_MAPinC); 43 | 44 | vector search_streetlight_tracking_recover(std::shared_ptr pcd, const Eigen::Matrix3d &R_MAPtoC, 45 | const Eigen::Vector3d &p_MAPinC, double reloc_z_th); 46 | 47 | void match_filter(std::vector &matches); 48 | 49 | std::vector get_boxes() { return dl_boxes; } 50 | 51 | protected: 52 | /// Threshold for searching streetlights 53 | double z_th; 54 | 55 | bool dl_filter; 56 | 57 | /// Weight for the matching score 58 | double alpha; 59 | 60 | double ang_th; 61 | 62 | std::shared_ptr cam; 63 | 64 | std::vector all_STs; 65 | 66 | std::vector dl_boxes; 67 | }; 68 | } // namespace night_voyager 69 | 70 | #endif -------------------------------------------------------------------------------- /include/streetlight_matcher/HungaryEstimator.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef HUNGARY_ESTIMATOR_H 12 | #define HUNGARY_ESTIMATOR_H 13 | 14 | #include 15 | #include 16 | 17 | using namespace std; 18 | 19 | namespace night_voyager { 20 | class HungaryEstimator { 21 | public: 22 | HungaryEstimator(const Eigen::MatrixXd &costMatrix) : n(costMatrix.rows()), mat(costMatrix), matRcd(costMatrix) { 23 | assign.resize(costMatrix.rows()); 24 | for (int i = 0; i < n; i++) 25 | assign[i] = -1; 26 | totalCost = 0; 27 | } 28 | void rowSub(); //Row-wise reduction 29 | 30 | void colSub(); //Column-wise reduction 31 | 32 | bool isOptimal(); //Determine whether the optimal allocation solution is found through trial assignment 33 | 34 | void matTrans(); //Matrix transformation to make the cost matrix have enough 0 elements 35 | 36 | vector solve(); 37 | 38 | private: 39 | int n; //Number of elements 40 | vector assign; //Assignment result 41 | Eigen::MatrixXd mat; //Cost Matrix 42 | Eigen::MatrixXd matRcd; //Cost Matrix 43 | double totalCost; 44 | }; 45 | } // namespace night_voyager 46 | 47 | #endif -------------------------------------------------------------------------------- /include/streetlight_matcher/StreetlightFeature.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef STREETLIGHT_FEATURE_H 12 | #define STREETLIGHT_FEATURE_H 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | namespace night_voyager { 20 | 21 | class StreetlightFeature { 22 | public: 23 | /// Unique ID of this feature 24 | size_t featid; 25 | 26 | /// If this feature should be deleted 27 | bool to_delete; 28 | 29 | /// boxes that this feature has been detected from 30 | std::vector boxes; 31 | 32 | /// UV coordinates of box centers that this feature has been seen from 33 | std::vector uvs; 34 | 35 | /// UV normalized coordinates of box centers that this feature has been seen from 36 | std::vector uvs_norm; 37 | 38 | /// Timestamps of each UV measurement 39 | std::vector timestamps; 40 | 41 | /** 42 | * @brief Remove measurements that are older then the specified timestamp. 43 | * 44 | * Given a valid timestamp, this will remove all measurements that have occured earlier then this. 45 | * 46 | * @param timestamp Timestamps that our measurements must occur after 47 | */ 48 | void clean_older_measurements(double timestamp) { 49 | // Assert that we have all the parts of a measurement 50 | assert(timestamps.size() == uvs.size()); 51 | assert(timestamps.size() == uvs_norm.size()); 52 | assert(timestamps.size() == boxes.size()); 53 | 54 | // Our iterators 55 | auto it1 = timestamps.begin(); 56 | auto it2 = uvs.begin(); 57 | auto it3 = uvs_norm.begin(); 58 | auto it4 = boxes.begin(); 59 | 60 | // Loop through measurement times, remove ones that are older then the specified one 61 | while (it1 != timestamps.end()) { 62 | if (*it1 <= timestamp) { 63 | it1 = timestamps.erase(it1); 64 | it2 = uvs.erase(it2); 65 | it3 = uvs_norm.erase(it3); 66 | it4 = boxes.erase(it4); 67 | } else { 68 | ++it1; 69 | ++it2; 70 | ++it3; 71 | ++it4; 72 | } 73 | } 74 | } 75 | 76 | /** 77 | * @brief Remove measurements that do not occur at passed timestamps. 78 | * 79 | * Given a series of valid timestamps, this will remove all measurements that have not occurred at these times. 80 | * This would normally be used to ensure that the measurements that we have occur at our clone times. 81 | * 82 | * @param valid_times Vector of timestamps that our measurements must occur at 83 | */ 84 | void clean_old_measurements(const std::vector &valid_times) { 85 | 86 | // Assert that we have all the parts of a measurement 87 | // std::cout << "timestamps: " << timestamps.size() << " " << "uvs: " << uvs.size() << std::endl; 88 | assert(timestamps.size() == uvs.size()); 89 | assert(timestamps.size() == uvs_norm.size()); 90 | assert(timestamps.size() == boxes.size()); 91 | 92 | // Our iterators 93 | auto it1 = timestamps.begin(); 94 | auto it2 = uvs.begin(); 95 | auto it3 = uvs_norm.begin(); 96 | auto it4 = boxes.begin(); 97 | 98 | // Loop through measurement times, remove ones that are not in our timestamps 99 | while (it1 != timestamps.end()) { 100 | if (std::find(valid_times.begin(), valid_times.end(), *it1) == valid_times.end()) { 101 | it1 = timestamps.erase(it1); 102 | it2 = uvs.erase(it2); 103 | it3 = uvs_norm.erase(it3); 104 | it4 = boxes.erase(it4); 105 | } else { 106 | ++it1; 107 | ++it2; 108 | ++it3; 109 | ++it4; 110 | } 111 | } 112 | } 113 | }; 114 | 115 | } // namespace night_voyager 116 | #endif -------------------------------------------------------------------------------- /include/streetlight_matcher/StreetlightFeatureDatabase.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | /* 12 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 13 | * Copyright (C) 2025 Night-Voyager Contributors 14 | * 15 | * For technical issues and support, please contact Tianxiao Gao at 16 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 17 | * 18 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 19 | * which is included as part of this source code package. 20 | */ 21 | #ifndef STREETLIGHT_FEATUREDATABASE_H 22 | #define STREETLIGHT_FEATUREDATABASE_H 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace night_voyager { 32 | 33 | class StreetlightFeature; 34 | 35 | class StreetlightFeatureDatabase { 36 | 37 | public: 38 | /** 39 | * @brief Default constructor 40 | */ 41 | StreetlightFeatureDatabase(){} 42 | 43 | StreetlightFeatureDatabase(const StreetlightFeatureDatabase &other); 44 | 45 | /** 46 | * @brief Update a feature object 47 | * @param id ID of the feature we will update 48 | * @param timestamp time that this measurement occured at 49 | * @param box box measurement of streetlight 50 | * @param u_n undistorted/normalized u coordinate of box center 51 | * @param v_n undistorted/normalized v coordinate of box center 52 | * 53 | * This will update a given feature based on the passed ID it has. 54 | * It will create a new feature, if it is an ID that we have not seen before. 55 | */ 56 | void update_feature(size_t id, double timestamp, const cv::Rect &rect, const Eigen::Vector2f ¢er, const Eigen::Vector2f ¢er_n, 57 | const Eigen::Vector2f &noise = Eigen::Vector2f::Zero()); 58 | 59 | void update_feature(std::shared_ptr feature); 60 | 61 | /** 62 | * @brief Check if there exists streetlight features 63 | */ 64 | bool database_empty() { return features_idlookup.empty(); } 65 | 66 | /** 67 | * @brief Remove measurements that do not occur at passed timestamps. 68 | * 69 | * Given a series of valid timestamps, this will remove all measurements that have not occurred at these times. 70 | * This would normally be used to ensure that the measurements that we have occur at our clone times. 71 | * 72 | * @param valid_times Vector of timestamps that our measurements must occur at 73 | */ 74 | void clean_old_measurements(const std::vector &valid_times); 75 | 76 | /**StreetlightFeatureDatabase 77 | * @brief Return number of measurements 78 | */ 79 | size_t count_measurements(); 80 | 81 | /** 82 | * @brief Return the feautures_idlookup 83 | */ 84 | std::unordered_map> &get_features_idlookup() { 85 | std::lock_guard lck(mtx); 86 | return features_idlookup; 87 | } 88 | 89 | protected: 90 | /// Mutex lock for our map 91 | std::mutex mtx; 92 | 93 | /// Our lookup array that allow use to query based on ID 94 | std::unordered_map> features_idlookup; 95 | }; 96 | 97 | } // namespace night_voyager 98 | 99 | #endif -------------------------------------------------------------------------------- /include/streetlight_matcher/StreetlightMatcher.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef STREETLIGHT_MATCHER_H 12 | #define STREETLIGHT_MATCHER_H 13 | 14 | #include "core/NightVoyagerOptions.h" 15 | #include "streetlight_matcher/BIMatcher.h" 16 | #include "streetlight_matcher/DLMatcher.h" 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace night_voyager { 23 | 24 | class PcdManager; 25 | 26 | class StreetlightMatcher { 27 | public: 28 | StreetlightMatcher(NightVoyagerOptions &options, std::shared_ptr camera_intrinsic) { 29 | dlmatcher = std::make_shared(options, camera_intrinsic); 30 | bimatcher = std::make_shared(options, camera_intrinsic); 31 | } 32 | 33 | std::vector run_dl_match(const BoxData &boxes, std::shared_ptr pcd, Eigen::Matrix3d &R_MAPtoC, Eigen::Vector3d &p_MAPinC, 34 | Eigen::MatrixXd &pose_cov) { 35 | return dlmatcher->match(boxes, pcd, R_MAPtoC, p_MAPinC, pose_cov); 36 | } 37 | 38 | std::vector run_dl_match_box(const BoxData &boxes, std::shared_ptr pcd, vector &matches, Eigen::Matrix3d &R_MAPtoC, 39 | Eigen::Vector3d &p_MAPinC) { 40 | return dlmatcher->match_box(boxes, pcd, matches, R_MAPtoC, p_MAPinC); 41 | } 42 | 43 | std::vector run_dl_match_tracking_recover(const BoxData &boxes, std::shared_ptr pcd, Eigen::Matrix3d &R_MAPtoC, 44 | Eigen::Vector3d &p_MAPinC, double reloc_z_th, int reloc_extend) { 45 | return dlmatcher->match_tracking_recover(boxes, pcd, R_MAPtoC, p_MAPinC, reloc_z_th, reloc_extend); 46 | } 47 | 48 | std::vector run_bi_match(const CameraData &img, std::shared_ptr pcd, vector &matches, Eigen::Matrix3d &R_MAPtoC, 49 | Eigen::Vector3d &p_MAPinC) { 50 | return bimatcher->match(img, pcd, matches, R_MAPtoC, p_MAPinC); 51 | } 52 | 53 | std::vector get_dl_boxes() { return dlmatcher->get_boxes(); } 54 | 55 | std::vector get_bi_boxes() { return bimatcher->get_boxes(); } 56 | 57 | protected: 58 | std::shared_ptr dlmatcher; 59 | std::shared_ptr bimatcher; 60 | }; 61 | 62 | } // namespace night_voyager 63 | 64 | #endif -------------------------------------------------------------------------------- /include/third_party/p3p_solver.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef P3PSOLVER_H 12 | #define P3PSOLVER_H 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | namespace night_voyager { 19 | 20 | class P3PSolver { 21 | public: 22 | int p3p_ding(const std::vector &x, const std::vector &X, std::vector &output_R, 23 | std::vector &output_T); 24 | 25 | private: 26 | double cubic_cardano_solution(const double beta, const double G, const double k2); 27 | 28 | double cubic_trigonometric_solution(const double alpha, const double beta, const double k2); 29 | 30 | std::array compute_pq(const double s, const double a, const double b, const double m12, const double m13, const double m23); 31 | 32 | std::pair> compute_line_conic_intersection(Eigen::Vector3d &l, const double b, const double m13, const double m23); 33 | 34 | void compute_pose(const std::vector &x, const std::vector &X, const double a12, const double a13, 35 | const double a23, const double m12, const double m13, const double m23, const double x_root, const double y_root, 36 | std::vector &output_R, std::vector &output_T); 37 | 38 | // Performs a few newton steps on the equations 39 | inline void refine_lambda(double &lambda1, double &lambda2, double &lambda3, const double a12, const double a13, const double a23, 40 | const double b12, const double b13, const double b23) { 41 | 42 | for (int iter = 0; iter < 5; ++iter) { 43 | double r1 = (lambda1 * lambda1 - 2.0 * lambda1 * lambda2 * b12 + lambda2 * lambda2 - a12); 44 | double r2 = (lambda1 * lambda1 - 2.0 * lambda1 * lambda3 * b13 + lambda3 * lambda3 - a13); 45 | double r3 = (lambda2 * lambda2 - 2.0 * lambda2 * lambda3 * b23 + lambda3 * lambda3 - a23); 46 | if (std::abs(r1) + std::abs(r2) + std::abs(r3) < 1e-10) 47 | return; 48 | double x11 = lambda1 - lambda2 * b12; 49 | double x12 = lambda2 - lambda1 * b12; 50 | double x21 = lambda1 - lambda3 * b13; 51 | double x23 = lambda3 - lambda1 * b13; 52 | double x32 = lambda2 - lambda3 * b23; 53 | double x33 = lambda3 - lambda2 * b23; 54 | double detJ = 0.5 / (x11 * x23 * x32 + x12 * x21 * x33); // half minus inverse determinant 55 | // This uses the closed form of the inverse for the jacobean. 56 | // Due to the zero elements this actually becomes quite nice. 57 | lambda1 += (-x23 * x32 * r1 - x12 * x33 * r2 + x12 * x23 * r3) * detJ; 58 | lambda2 += (-x21 * x33 * r1 + x11 * x33 * r2 - x11 * x23 * r3) * detJ; 59 | lambda3 += (x21 * x32 * r1 - x11 * x32 * r2 - x12 * x21 * r3) * detJ; 60 | } 61 | } 62 | }; 63 | 64 | } // namespace night_voyager 65 | 66 | #endif -------------------------------------------------------------------------------- /include/tracking_recover/TrackingRecover.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef TRACKINGRECOVER_H 12 | #define TRACKINGRECOVER_H 13 | 14 | #include "core/NightVoyagerOptions.h" 15 | #include "utils/Print.h" 16 | #include 17 | #include 18 | #include 19 | 20 | namespace night_voyager { 21 | 22 | struct PoseScore { 23 | float score = -10000; 24 | Eigen::Matrix3d R; 25 | Eigen::Vector3d p; 26 | std::vector matches; 27 | }; 28 | 29 | class State; 30 | class StreetlightFeatureDatabase; 31 | class PriorPoseManager; 32 | class PcdManager; 33 | 34 | class TrackingRecover { 35 | public: 36 | TrackingRecover(const NightVoyagerOptions &options, std::shared_ptr pcd, std::shared_ptr camera_intrinsic, 37 | std::shared_ptr prpose) 38 | : options(options.tracking_recover_options), _pcd(pcd), _cam(camera_intrinsic), _prpose(prpose), nomatch_dist(-1.0), nomatch_ang(-1.0) { 39 | tracking_status = TrackingStatus::TRACKINGSUCCESS; 40 | } 41 | 42 | void trans_after_nomatch(const Eigen::Matrix3d &rot, const Eigen::Vector3d &pos); 43 | 44 | bool traverse_all_situations(std::shared_ptr state, const CameraData &img, BoxData &st_boxes, 45 | std::shared_ptr st_database, bool msckf = false); 46 | 47 | void change_tracking_status(TrackingStatus tracking_status_) { 48 | tracking_status = tracking_status_; 49 | if (tracking_status == TrackingStatus::TRACKINGSUCCESS) { 50 | PRINT_INFO(YELLOW "[change-tracking-state]: Tracking state is changed to TRACKINGSUCCESS!\n"); 51 | } else if (tracking_status == TrackingStatus::TRACKINGUNSTABLESUCCESS) { 52 | PRINT_INFO(YELLOW "[change-tracking-state]: Tracking state is changed to TRACKINGUNSTABLESUCCESS!\n"); 53 | } else if (tracking_status == TrackingStatus::TRACKINGMAYLOST) { 54 | PRINT_INFO(YELLOW "[change-tracking-state]: Tracking state is changed to TRACKINGMAYLOST\n"); 55 | } else if (tracking_status == TrackingStatus::TRACKINGLOST) { 56 | PRINT_INFO(YELLOW "[change-tracking-state]: No matches lasts for %f meters, %f radians. Tracking state is changed to TRACKINGLOST\n", 57 | nomatch_dist, nomatch_ang); 58 | } 59 | nomatch_dist = -1.0; 60 | nomatch_ang = -1.0; 61 | } 62 | 63 | bool is_tracking_lost() { return tracking_status == TrackingStatus::TRACKINGLOST; } 64 | 65 | bool check_transform_th() { return (nomatch_dist > options.dist_th && nomatch_ang > options.ang_th); } 66 | 67 | void clear_dist() { 68 | nomatch_dist = -1.0; 69 | nomatch_ang = -1.0; 70 | } 71 | 72 | TrackingStatus check_tracking_status() { return tracking_status; } 73 | 74 | void copy_state_stdatabase(std::shared_ptr state, int states_num, std::shared_ptr st_database); 75 | 76 | bool check_if_select(const BoxData &cur_box); 77 | 78 | std::vector select_best_state(const CameraData &img, const BoxData &cur_box); 79 | 80 | void update_select_state(std::shared_ptr state, std::shared_ptr st_database, 81 | const std::vector &matches, bool msckf = false); 82 | 83 | BoxData process_box(const CameraData &img, const BoxData &cur_box); 84 | 85 | std::vector select_streetlights(const Eigen::Matrix3d &R_MAPtoC, const Eigen::Vector3d &p_MAPinC); 86 | 87 | void generateCombinations(std::vector &combination, int start, int n, int index, int r, std::vector> &allCombinations); 88 | 89 | void generatePermutations(std::vector &permutation, std::vector &chosen, int n, int r, std::vector> &allPermutations); 90 | 91 | void store_poses(); 92 | 93 | std::vector> states; 94 | 95 | /// Stores Streetlight features 96 | std::vector> st_databases; 97 | 98 | /// Current matches for each state 99 | std::vector> cur_stmatches; 100 | 101 | std::vector> rots_loc; 102 | 103 | std::vector> poss_loc; 104 | 105 | std::vector> rots_global; 106 | 107 | std::vector> poss_global; 108 | 109 | private: 110 | TrackingRecoverOptions options; 111 | 112 | /// Pointcloud class object 113 | std::shared_ptr _pcd; 114 | 115 | std::shared_ptr _cam; 116 | 117 | std::shared_ptr _prpose; 118 | 119 | Eigen::Matrix3d nomatch_rot; 120 | 121 | Eigen::Vector3d nomatch_pos; 122 | 123 | float nomatch_dist; 124 | 125 | float nomatch_ang; 126 | 127 | Eigen::Matrix3d last_R_MAPtoC; 128 | 129 | Eigen::Vector3d last_P_MAPinC; 130 | 131 | std::vector>> matches_per_obs; 132 | 133 | TrackingStatus tracking_status; 134 | 135 | int level; 136 | }; 137 | } // namespace night_voyager 138 | #endif -------------------------------------------------------------------------------- /include/utils/Print.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef NIGHT_VOYAGER_PRINT_H 12 | #define NIGHT_VOYAGER_PRINT_H 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace night_voyager { 21 | 22 | /** 23 | * @brief Printer for open_vins that allows for various levels of printing to be done 24 | * 25 | * To set the global verbosity level one can do the following: 26 | * @code{.cpp} 27 | * night_voyager::Printer::setPrintLevel("WARNING"); 28 | * night_voyager::Printer::setPrintLevel(night_voyager::Printer::PrintLevel::WARNING); 29 | * @endcode 30 | */ 31 | class Printer { 32 | public: 33 | /** 34 | * @brief The different print levels possible 35 | * 36 | * - PrintLevel::ALL : All PRINT_XXXX will output to the console 37 | * - PrintLevel::DEBUG : "DEBUG", "INFO", "WARNING" and "ERROR" will be printed. "ALL" will be silenced 38 | * - PrintLevel::INFO : "INFO", "WARNING" and "ERROR" will be printed. "ALL" and "DEBUG" will be silenced 39 | * - PrintLevel::WARNING : "WARNING" and "ERROR" will be printed. "ALL", "DEBUG" and "INFO" will be silenced 40 | * - PrintLevel::ERROR : Only "ERROR" will be printed. All the rest are silenced 41 | * - PrintLevel::SILENT : All PRINT_XXXX will be silenced. 42 | */ 43 | enum PrintLevel { ALL = 0, DEBUG = 1, INFO = 2, WARNING = 3, ERROR = 4, SILENT = 5 }; 44 | 45 | /** 46 | * @brief Set the print level to use for all future printing to stdout. 47 | * @param level The debug level to use 48 | */ 49 | static void setPrintLevel(const std::string &level); 50 | 51 | /** 52 | * @brief Set the print level to use for all future printing to stdout. 53 | * @param level The debug level to use 54 | */ 55 | static void setPrintLevel(PrintLevel level); 56 | 57 | /** 58 | * @brief The print function that prints to stdout. 59 | * @param level the print level for this print call 60 | * @param location the location the print was made from 61 | * @param line the line the print was made from 62 | * @param format The printf format 63 | */ 64 | static void debugPrint(PrintLevel level, const char location[], const char line[], const char *format, ...); 65 | 66 | /// The current print level 67 | static PrintLevel current_print_level; 68 | 69 | private: 70 | /// The max length for the file path. This is to avoid very long file paths from 71 | static constexpr uint32_t MAX_FILE_PATH_LEGTH = 30; 72 | }; 73 | 74 | } /* namespace night_voyager */ 75 | 76 | /* 77 | * Converts anything to a string 78 | */ 79 | #define STRINGIFY(x) #x 80 | #define TOSTRING(x) STRINGIFY(x) 81 | 82 | /* 83 | * The different Types of print levels 84 | */ 85 | #define PRINT_ALL(x...) night_voyager::Printer::debugPrint(night_voyager::Printer::PrintLevel::ALL, __FILE__, TOSTRING(__LINE__), x); 86 | #define PRINT_DEBUG(x...) night_voyager::Printer::debugPrint(night_voyager::Printer::PrintLevel::DEBUG, __FILE__, TOSTRING(__LINE__), x); 87 | #define PRINT_INFO(x...) night_voyager::Printer::debugPrint(night_voyager::Printer::PrintLevel::INFO, __FILE__, TOSTRING(__LINE__), x); 88 | #define PRINT_WARNING(x...) night_voyager::Printer::debugPrint(night_voyager::Printer::PrintLevel::WARNING, __FILE__, TOSTRING(__LINE__), x); 89 | #define PRINT_ERROR(x...) night_voyager::Printer::debugPrint(night_voyager::Printer::PrintLevel::ERROR, __FILE__, TOSTRING(__LINE__), x); 90 | 91 | #define RESET "\033[0m" 92 | #define BLACK "\033[30m" /* Black */ 93 | #define RED "\033[31m" /* Red */ 94 | #define GREEN "\033[32m" /* Green */ 95 | #define YELLOW "\033[33m" /* Yellow */ 96 | #define BLUE "\033[34m" /* Blue */ 97 | #define MAGENTA "\033[35m" /* Magenta */ 98 | #define CYAN "\033[36m" /* Cyan */ 99 | #define WHITE "\033[37m" /* White */ 100 | #define REDPURPLE "\033[95m" /* Red Purple */ 101 | #define BOLDBLACK "\033[1m\033[30m" /* Bold Black */ 102 | #define BOLDRED "\033[1m\033[31m" /* Bold Red */ 103 | #define BOLDGREEN "\033[1m\033[32m" /* Bold Green */ 104 | #define BOLDYELLOW "\033[1m\033[33m" /* Bold Yellow */ 105 | #define BOLDBLUE "\033[1m\033[34m" /* Bold Blue */ 106 | #define BOLDMAGENTA "\033[1m\033[35m" /* Bold Magenta */ 107 | #define BOLDCYAN "\033[1m\033[36m" /* Bold Cyan */ 108 | #define BOLDWHITE "\033[1m\033[37m" /* Bold White */ 109 | #define BOLDREDPURPLE "\033[1m\033[95m" /* Bold Red Purple */ 110 | 111 | #endif /* NIGHT_VOYAGER_PRINT_H */ 112 | -------------------------------------------------------------------------------- /include/visualizer/CameraPoseVisualization.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef CAMERA_POSE_VISUALIZATION_H 12 | #define CAMERA_POSE_VISUALIZATION_H 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | namespace night_voyager { 22 | 23 | class CameraPoseVisualization { 24 | public: 25 | CameraPoseVisualization() : m_scale(2), m_line_width(0.4) { 26 | m_image_boundary_color.r = 1; 27 | m_image_boundary_color.g = 1; 28 | m_image_boundary_color.b = 1; 29 | m_image_boundary_color.a = 1; 30 | m_optical_center_connector_color.r = 1; 31 | m_optical_center_connector_color.g = 1; 32 | m_optical_center_connector_color.b = 1; 33 | m_optical_center_connector_color.a = 1; 34 | } 35 | 36 | CameraPoseVisualization(float r, float g, float b, float a) : m_scale(1), m_line_width(0.2) { 37 | m_image_boundary_color.r = r; 38 | m_image_boundary_color.g = g; 39 | m_image_boundary_color.b = b; 40 | m_image_boundary_color.a = a; 41 | m_optical_center_connector_color.r = r; 42 | m_optical_center_connector_color.g = g; 43 | m_optical_center_connector_color.b = b; 44 | m_optical_center_connector_color.a = a; 45 | } 46 | 47 | void Eigen2Point(const Eigen::Vector3d &v, geometry_msgs::Point &p); 48 | 49 | // void setImageBoundaryColor(float r, float g, float b, float a=1.0); 50 | // void setOpticalCenterConnectorColor(float r, float g, float b, float a=1.0); 51 | void setScale(double s) { m_scale = s; } 52 | 53 | void setLineWidth(double width) { m_line_width = width; } 54 | 55 | void setRGBA(float r, float g, float b, float a) { 56 | m_image_boundary_color.r = r; 57 | m_image_boundary_color.g = g; 58 | m_image_boundary_color.b = b; 59 | m_image_boundary_color.a = a; 60 | m_optical_center_connector_color.r = r; 61 | m_optical_center_connector_color.g = g; 62 | m_optical_center_connector_color.b = b; 63 | m_optical_center_connector_color.a = a; 64 | } 65 | 66 | void add_pose(const Eigen::Vector3d &p, const Eigen::Quaterniond &q); 67 | 68 | void reset() { m_markers.clear(); } 69 | 70 | void publish_by(ros::Publisher &pub, const double &stamp); 71 | 72 | private: 73 | std::vector m_markers; 74 | std_msgs::ColorRGBA m_image_boundary_color; 75 | std_msgs::ColorRGBA m_optical_center_connector_color; 76 | double m_scale; 77 | double m_line_width; 78 | 79 | static const Eigen::Vector3d imlt; 80 | static const Eigen::Vector3d imlb; 81 | static const Eigen::Vector3d imrt; 82 | static const Eigen::Vector3d imrb; 83 | static const Eigen::Vector3d oc; 84 | static const Eigen::Vector3d lt0; 85 | static const Eigen::Vector3d lt1; 86 | static const Eigen::Vector3d lt2; 87 | }; 88 | } // namespace night_voyager 89 | 90 | #endif -------------------------------------------------------------------------------- /include/visualizer/Visualizer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #ifndef VISUALIZER_H 12 | #define VISUALIZER_H 13 | 14 | #include "core/CommonLib.h" 15 | #include "core/NightVoyagerOptions.h" 16 | #include "visualizer/CameraPoseVisualization.h" 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | using namespace std; 37 | 38 | namespace night_voyager { 39 | class VILManager; 40 | class CameraPoseVisualization; 41 | 42 | class Visualizer { 43 | public: 44 | Visualizer(ros::NodeHandle &nh, const NightVoyagerOptions &options, shared_ptr sys); 45 | 46 | void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg); 47 | 48 | void img_cbk(const sensor_msgs::CompressedImage::ConstPtr &msg); 49 | 50 | void box_cbk(const night_voyager::BoundingBoxes::ConstPtr &msg); 51 | 52 | void odom_cbk(const nav_msgs::Odometry::ConstPtr &msg); 53 | 54 | void run(); 55 | 56 | void visualize(const PackData &message, double time_cost); 57 | 58 | void publish_images(const PackData &message); 59 | 60 | void publish_detection(const PackData &message); 61 | 62 | void publish_images_tracking_recover(const PackData &message); 63 | 64 | void publish_other_images_tracking_recover(const PackData &message); 65 | 66 | void publish_state(); 67 | 68 | void publish_hz(double time_cost); 69 | 70 | void publish_state_tracking_recover(); 71 | 72 | void publish_features(); 73 | 74 | void publish_odometry(); 75 | 76 | void publish_cam_path(); 77 | 78 | void publish_features_tracking_recover(); 79 | 80 | void publish_prior_poses(); 81 | 82 | void publish_initialization_regions(); 83 | 84 | void publish_near_prior_poses(); 85 | 86 | void publish_global_pointcloud(); 87 | 88 | void publish_matched_pointcloud(); 89 | 90 | void publish_all_possible_paths(); 91 | 92 | void readPoses(const string &path, deque &poses_stamp) { 93 | ifstream txtFile; 94 | txtFile.open(path.c_str()); 95 | while (!txtFile.eof()) { 96 | string line; 97 | getline(txtFile, line); 98 | if (!line.empty()) { 99 | stringstream ss; 100 | ss << line; 101 | TumPose pose; 102 | ss >> pose.timestamp; 103 | ss >> pose.position.x(); 104 | ss >> pose.position.y(); 105 | ss >> pose.position.z(); 106 | ss >> pose.orientation.x(); 107 | ss >> pose.orientation.y(); 108 | ss >> pose.orientation.z(); 109 | ss >> pose.orientation.w(); 110 | 111 | poses_stamp.push_back(pose); 112 | } 113 | } 114 | } 115 | 116 | void save_total_state_to_file(); 117 | 118 | void save_total_state_to_file_tracking_recover(); 119 | 120 | sensor_msgs::PointCloud2 get_ros_pointcloud_inMAP(const std::vector> &feats); 121 | 122 | sensor_msgs::PointCloud2 get_ros_pointcloud_inMAP_tracking_recover(const std::vector> &feats); 123 | 124 | double time_before_initialized = 0, time_after_initialized = 0; 125 | int n_before_initialized = 1, n_after_initialized = 1; 126 | 127 | private: 128 | shared_ptr app; 129 | mutex camera_queue_mtx; 130 | mutex odom_queue_mtx; 131 | mutex box_queue_mtx; 132 | // Thread atomics 133 | atomic thread_update_running; 134 | 135 | // For path viz 136 | unsigned int poses_seq_imu = 0, poses_seq_gt = 0; 137 | unsigned int num_hz = 0; 138 | std::vector poses_imu, poses_gt; 139 | 140 | // deque imu_buffer; 141 | deque imu_buffer; 142 | deque img_buffer; 143 | deque odom_buffer; 144 | deque box_buffer; 145 | 146 | ros::Subscriber sub_img; 147 | ros::Subscriber sub_imu; 148 | ros::Subscriber sub_box; 149 | ros::Subscriber sub_odom; 150 | 151 | image_transport::Publisher it_pub_tracks, it_pub_loop_img_depth, it_pub_loop_img_depth_color, it_pub_streetlight_matches, it_pub_detection, it_pub_projection, 152 | it_pub_projection_tracking_recover, it_pub_hz; 153 | ros::Publisher pub_poseimu, pub_pathimu, pub_paths_tracking_recover; 154 | ros::Publisher pub_points_msckf, pub_points_slam, pub_points_streetlight; 155 | ros::Publisher pub_prior_map, pub_prior_poss, pub_prior_quats, pub_init_regions, pub_near_prior_cloud, pub_view_region_bound, pub_view_region_fill, pub_odometer; 156 | ros::Publisher pub_night_voyager_cam, pub_gt_cam, pub_gt_path; 157 | 158 | bool save_total_state, save_time_consume; 159 | ofstream of_state_est, of_state_std, of_state_tum_loc, of_state_tum_global, of_state_loc_part, of_state_rel_part, of_state_global_part; 160 | 161 | ofstream of_state_est_tracking_recover, of_state_std_tracking_recover, of_state_tum_loc_tracking_recover, of_state_tum_global_tracking_recover; 162 | 163 | bool last_tracking_lost; 164 | 165 | double last_timestamp_imu; 166 | double last_timestamp_img; 167 | double last_timestamp_odom; 168 | double last_timestamp_box; 169 | 170 | double accel_norm; 171 | 172 | double last_visualization_timestamp, last_visualization_timestamp_image; 173 | 174 | visualization_msgs::MarkerArray marker_array; 175 | 176 | deque gt_poses; 177 | 178 | CameraPoseVisualization night_voyager_cam, gt_cam; 179 | }; 180 | } // namespace night_voyager 181 | #endif -------------------------------------------------------------------------------- /launch/night_voyager.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /msg/BoundingBox.msg: -------------------------------------------------------------------------------- 1 | float64 probability 2 | float64 xmin 3 | float64 ymin 4 | float64 xmax 5 | float64 ymax 6 | int16 box_num 7 | # string Class 8 | -------------------------------------------------------------------------------- /msg/BoundingBoxes.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | # Header image_header 3 | BoundingBox[] bounding_boxes 4 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | night_voyager 4 | 0.0.0 5 | The night_voyager package 6 | 7 | 8 | 9 | 10 | gtx 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | cv_bridge 53 | geometry_msgs 54 | nav_msgs 55 | pcl_ros 56 | roscpp 57 | rospy 58 | sensor_msgs 59 | tf 60 | std_msgs 61 | visualization_msgs 62 | image_transport 63 | message_generation 64 | cv_bridge 65 | geometry_msgs 66 | nav_msgs 67 | pcl_ros 68 | roscpp 69 | rospy 70 | tf 71 | sensor_msgs 72 | std_msgs 73 | visualization_msgs 74 | image_transport 75 | cv_bridge 76 | geometry_msgs 77 | nav_msgs 78 | pcl_ros 79 | roscpp 80 | rospy 81 | tf 82 | sensor_msgs 83 | std_msgs 84 | visualization_msgs 85 | image_transport 86 | message_runtime 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | -------------------------------------------------------------------------------- /src/feature_tracker/Feature.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #include "feature_tracker/Feature.h" 12 | 13 | namespace night_voyager { 14 | void Feature::clean_old_measurements(const std::vector &valid_times) { 15 | 16 | // Assert that we have all the parts of a measurement 17 | assert(timestamps.size() == uvs.size()); 18 | assert(timestamps.size() == uvs_norm.size()); 19 | 20 | // Our iterators 21 | auto it1 = timestamps.begin(); 22 | auto it2 = uvs.begin(); 23 | auto it3 = uvs_norm.begin(); 24 | 25 | // Loop through measurement times, remove ones that are not in our timestamps 26 | while (it1 != timestamps.end()) { 27 | if (std::find(valid_times.begin(), valid_times.end(), *it1) == valid_times.end()) { 28 | it1 = timestamps.erase(it1); 29 | it2 = uvs.erase(it2); 30 | it3 = uvs_norm.erase(it3); 31 | } else { 32 | ++it1; 33 | ++it2; 34 | ++it3; 35 | } 36 | } 37 | } 38 | 39 | void Feature::clean_invalid_measurements(const std::vector &invalid_times) { 40 | 41 | // Assert that we have all the parts of a measurement 42 | assert(timestamps.size() == uvs.size()); 43 | assert(timestamps.size() == uvs_norm.size()); 44 | 45 | // Our iterators 46 | auto it1 = timestamps.begin(); 47 | auto it2 = uvs.begin(); 48 | auto it3 = uvs_norm.begin(); 49 | 50 | // Loop through measurement times, remove ones that are in our timestamps 51 | while (it1 != timestamps.end()) { 52 | if (std::find(invalid_times.begin(), invalid_times.end(), *it1) != invalid_times.end()) { 53 | it1 = timestamps.erase(it1); 54 | it2 = uvs.erase(it2); 55 | it3 = uvs_norm.erase(it3); 56 | } else { 57 | ++it1; 58 | ++it2; 59 | ++it3; 60 | } 61 | } 62 | } 63 | 64 | void Feature::clean_older_measurements(double timestamp) { 65 | 66 | // Assert that we have all the parts of a measurement 67 | assert(timestamps.size() == uvs.size()); 68 | assert(timestamps.size() == uvs_norm.size()); 69 | 70 | // Our iterators 71 | auto it1 = timestamps.begin(); 72 | auto it2 = uvs.begin(); 73 | auto it3 = uvs_norm.begin(); 74 | 75 | // Loop through measurement times, remove ones that are older then the specified one 76 | while (it1 != timestamps.end()) { 77 | if (*it1 <= timestamp) { 78 | it1 = timestamps.erase(it1); 79 | it2 = uvs.erase(it2); 80 | it3 = uvs_norm.erase(it3); 81 | } else { 82 | ++it1; 83 | ++it2; 84 | ++it3; 85 | } 86 | } 87 | } 88 | } // namespace night_voyager -------------------------------------------------------------------------------- /src/initializer/StaticInitializer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #include "initializer/StaticInitializer.h" 12 | #include "core/IMU.h" 13 | #include "initializer/InitializerHelper.h" 14 | #include "utils/Print.h" 15 | 16 | namespace night_voyager { 17 | bool StaticInitializer::initialize(double ×tamp, Eigen::MatrixXd &covariance, std::vector> &order, 18 | std::shared_ptr t_imu) { 19 | // Return if we don't have any measurements 20 | if (imu_data->size() < 2) { 21 | return false; 22 | } 23 | 24 | // Newest and oldest imu timestamp 25 | // double newesttime = imu_data->at(imu_data->size() - 1).timestamp; 26 | double newesttime = imu_data->at(imu_data->size() - 1).timestamp; 27 | double oldesttime = imu_data->at(0).timestamp; 28 | 29 | // Return if we don't have enough for two windows 30 | if (newesttime - oldesttime < _options.init_window_time) { 31 | PRINT_INFO(YELLOW "[init-s]: unable to select window of IMU readings, not enough readings\n" RESET); 32 | return false; 33 | } 34 | 35 | Eigen::Vector3d a_avg = Eigen::Vector3d::Zero(); 36 | Eigen::Vector3d w_avg = Eigen::Vector3d::Zero(); 37 | for (const ImuData &data : *imu_data) { 38 | a_avg += data.am; 39 | w_avg += data.wm; 40 | } 41 | a_avg = a_avg / imu_data->size(); 42 | w_avg = w_avg / imu_data->size(); 43 | 44 | double a_var = 0; 45 | for (const ImuData &data : *imu_data) { 46 | a_var += (data.am - a_avg).dot(data.am - a_avg); 47 | } 48 | a_var = std::sqrt(a_var / ((int)imu_data->size() - 1)); 49 | 50 | // If it is above the threshold and we are not waiting for a jerk 51 | // Then we are not stationary (i.e. moving) so we should wait till we are 52 | if (a_var > _options.init_imu_thresh) { 53 | PRINT_INFO(YELLOW "[init-s]: to much IMU excitation, above threshold %.3f > %.3f\n" RESET, a_var, _options.init_imu_thresh); 54 | return false; 55 | } 56 | 57 | // Get rotation with z axis aligned with -g (z_in_G=0,0,1) 58 | Eigen::Vector3d z_axis = a_avg / a_avg.norm(); 59 | // InitializerHelper::gram_schmidt(z_axis, Ro); 60 | // Create an x_axis 61 | Eigen::Vector3d e_1(1, 0, 0); 62 | 63 | // Make x_axis perpendicular to z 64 | // as z_axis and e_1 are norm vector, so , z_axis.transpose()*e_1 is the length of e1 project into z_axis 65 | // then it is multiplied by z_axis (which is the direction of z_axis), so that e1, z_axis*z_axis.transpose()*e_1 and 66 | // e1-z_axis*z_axis.transpose()*e_1 compose a right-angled triangle. so x_axis is perpendicular to z 67 | Eigen::Vector3d x_axis = e_1 - z_axis * z_axis.transpose() * e_1; 68 | x_axis = x_axis / x_axis.norm(); 69 | 70 | // Get y from the cross product of these two 71 | Eigen::Vector3d y_axis = skew(z_axis) * x_axis; 72 | y_axis = y_axis / y_axis.norm(); 73 | 74 | // From these axes get rotation 75 | // Pose_IMU=R_GtoI*Pose_G. 76 | // we assume the Pose_G is Identity, so Pose_IMU(i.e. Ro) is R_GtoI. 77 | Eigen::Matrix Ro; 78 | Ro.block(0, 0, 3, 1) = x_axis; 79 | Ro.block(0, 1, 3, 1) = y_axis; 80 | Ro.block(0, 2, 3, 1) = z_axis; 81 | 82 | // Set our biases equal to our noise (subtract our gravity from accelerometer bias) 83 | Eigen::Vector3d gravity_inG; 84 | gravity_inG << 0.0, 0.0, _options.gravity_mag; 85 | Eigen::Vector3d bg = w_avg; 86 | Eigen::Vector3d ba = a_avg - Ro * gravity_inG; 87 | 88 | // Set our state variables 89 | timestamp = newesttime; 90 | Eigen::MatrixXd imu_state = Eigen::MatrixXd::Identity(5, 7); 91 | imu_state.block(0, 0, 3, 3) = Ro.transpose(); 92 | imu_state.block(0, 5, 3, 1) = bg; 93 | imu_state.block(0, 6, 3, 1) = ba; 94 | assert(t_imu != nullptr); 95 | t_imu->set_value(imu_state); 96 | 97 | // Create base covariance and its covariance ordering 98 | order.clear(); 99 | order.push_back(t_imu); 100 | covariance = std::pow(0.02, 2) * Eigen::MatrixXd::Identity(t_imu->size(), t_imu->size()); 101 | covariance.block(0, 0, 3, 3) = std::pow(0.05, 2) * Eigen::Matrix3d::Identity(); // q0.05 102 | covariance.block(3, 3, 3, 3) = std::pow(0.005, 2) * Eigen::Matrix3d::Identity(); // p0.005 103 | covariance.block(6, 6, 3, 3) = std::pow(0.05, 2) * Eigen::Matrix3d::Identity(); // v0.05 (static) 104 | 105 | return true; 106 | } 107 | } // namespace night_voyager -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #include "core/NightVoyagerOptions.h" 12 | #include "msckf_iekf/VILManager.h" 13 | #include "prior_pose/PriorPoseManager.h" 14 | #include "streetlight_matcher/PcdManager.h" 15 | #include "utils/Print.h" 16 | #include "visualizer/Visualizer.h" 17 | #include 18 | #include 19 | #include 20 | 21 | using namespace night_voyager; 22 | 23 | int main(int argc, char **argv) { 24 | ros::init(argc, argv, "Night_Voyager"); 25 | ros::NodeHandle nh; 26 | 27 | NightVoyagerOptions options; 28 | options.root_dir = ROOT_DIR; 29 | options.param_load(nh); 30 | // Verbosity 31 | std::string verbosity = "INFO"; 32 | Printer::setPrintLevel(verbosity); 33 | 34 | auto pcd_manager = std::make_shared(options); 35 | auto prior_pose_manager = std::make_shared(options); 36 | auto sys = std::make_shared(options, pcd_manager, prior_pose_manager); 37 | auto viz = std::make_shared(nh, options, sys); 38 | 39 | ros::AsyncSpinner spinner(0); 40 | spinner.start(); 41 | 42 | while (ros::ok()) { 43 | viz->run(); 44 | } 45 | ros::waitForShutdown(); 46 | ros::shutdown(); 47 | 48 | return EXIT_SUCCESS; 49 | } 50 | -------------------------------------------------------------------------------- /src/msckf_iekf/UpdaterOdom.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #include "msckf_iekf/UpdaterOdom.h" 12 | #include "msckf_iekf/State.h" 13 | #include "msckf_iekf/StateHelper.h" 14 | #include "msckf_iekf/UpdaterHelper.h" 15 | #include 16 | 17 | namespace night_voyager { 18 | 19 | UpdaterOdom::UpdaterOdom(NightVoyagerOptions &options) { 20 | 21 | _options = options.odom_options; 22 | 23 | // Initialize the chi squared test table with confidence level 0.95 24 | // https://github.com/KumarRobotics/msckf_vio/blob/050c50defa5a7fd9a04c1eed5687b405f02919b5/src/msckf_vio.cpp#L215-L221 25 | for (int i = 1; i < 500; i++) { 26 | boost::math::chi_squared chi_squared_dist(i); 27 | chi_squared_table[i] = boost::math::quantile(chi_squared_dist, 0.95); 28 | } 29 | } 30 | 31 | void UpdaterOdom::update(std::shared_ptr state, const OdomData &odom) { 32 | Eigen::MatrixXd H_x; 33 | Eigen::VectorXd res; 34 | Eigen::MatrixXd R; 35 | std::vector> Hx_order; 36 | 37 | // cout << "odom_vel: " << odom.vm.transpose() << endl; 38 | 39 | // Get the Jacobian for this odom 40 | if (state->_kf == KFCLASS::MSCKF) 41 | UpdaterHelper::get_odom_jacobian_full_msckf(state, odom, res, H_x, Hx_order, R); 42 | else 43 | UpdaterHelper::get_odom_jacobian_full(state, odom, res, H_x, Hx_order, R); 44 | 45 | R = _options.odom_cov_eig; 46 | 47 | // Eigen::MatrixXd P_marg = StateHelper::get_marginal_covariance(state, Hx_order); 48 | // Eigen::MatrixXd S = H_x*P_marg*H_x.transpose() + R; //HPH^T+R 49 | // double chi2 = res.dot(S.llt().solve(res)); // r.dot(S^-1*r) S is the measurement covariance, S^-1 is information matrix 50 | // // the result is the Mahalanobis distance 51 | 52 | // // Get our threshold (we precompute up to 500 but handle the case that it is more) 53 | // double chi2_check; 54 | 55 | // if (res.rows() < 500) { 56 | // chi2_check = chi_squared_table[res.rows()]; 57 | // } 58 | // else { 59 | // boost::math::chi_squared chi_squared_dist(res.rows()); 60 | // chi2_check = boost::math::quantile(chi_squared_dist, 0.95); 61 | // PRINT_WARNING(YELLOW "chi2_check over the residual limit - %d\n" RESET, (int)res.rows()); 62 | // } 63 | 64 | // // Check if we should delete or not 65 | // if (chi2 > _options.chi2_multipler * chi2_check) { 66 | // PRINT_DEBUG("Odom measurement, chi2 = %f > %f, PASS this update\n", chi2, _options.chi2_multipler*chi2_check); 67 | // std::stringstream ss; 68 | // ss << "res = " << std::endl << res.transpose() << std::endl; 69 | // PRINT_DEBUG(ss.str().c_str()); 70 | // } 71 | 72 | StateHelper::EKFUpdate(state, Hx_order, H_x, res, R); 73 | } 74 | 75 | } // namespace night_voyager -------------------------------------------------------------------------------- /src/streetlight_matcher/HungaryEstimator.cpp: -------------------------------------------------------------------------------- 1 | #include "streetlight_matcher/HungaryEstimator.h" 2 | 3 | namespace night_voyager { 4 | void HungaryEstimator::rowSub() { 5 | Eigen::VectorXd minEmt; 6 | minEmt = mat.rowwise().minCoeff(); 7 | 8 | for (int j = 0; j < n; j++) { 9 | mat.col(j) -= minEmt; 10 | } 11 | } 12 | 13 | void HungaryEstimator::colSub() { 14 | Eigen::VectorXd minEmt; 15 | minEmt = mat.colwise().minCoeff(); 16 | 17 | for (int i = 0; i < n; i++) { 18 | mat.row(i) -= minEmt; 19 | } 20 | } 21 | 22 | bool HungaryEstimator::isOptimal() { 23 | vector tAssign(n, -1); 24 | vector nZero(n, 0); 25 | vector rowIsUsed(n, false); 26 | vector colIsUsed(n, false); 27 | 28 | int nline = 0; 29 | while (nline < n) { 30 | for (int i = 0; i < n; i++) 31 | nZero[i] = 0; 32 | for (int i = 0; i < n; i++) { 33 | if (rowIsUsed[i]) 34 | continue; 35 | for (int j = 0; j < n; j++) 36 | if (!colIsUsed[j] && mat(i, j) == 0) 37 | ++nZero[i]; 38 | } 39 | 40 | int minZeros = n; 41 | int rowId = -1; 42 | for (int i = 0; i < n; i++) { 43 | if (!rowIsUsed[i] && nZero[i] < minZeros && nZero[i] > 0) { 44 | minZeros = nZero[i]; 45 | rowId = i; 46 | } 47 | } 48 | 49 | if (rowId == -1) 50 | break; 51 | for (int j = 0; j < n; j++) { 52 | if (mat(rowId, j) == 0 && !colIsUsed[j]) { 53 | rowIsUsed[rowId] = 1; 54 | colIsUsed[j] = 1; 55 | tAssign[rowId] = j; 56 | break; 57 | } 58 | } 59 | ++nline; 60 | } 61 | 62 | for (int i = 0; i < n; i++) { 63 | assign[i] = tAssign[i]; 64 | } 65 | for (int i = 0; i < n; i++) { 66 | if (assign[i] == -1) 67 | return false; 68 | } 69 | return true; 70 | } 71 | 72 | void HungaryEstimator::matTrans() { 73 | vector rowTip(n, false); 74 | vector colTip(n, false); 75 | vector rowLine(n, false); 76 | vector colLine(n, false); 77 | 78 | //打勾 79 | for (int i = 0; i < n; i++) 80 | if (assign[i] == -1) 81 | rowTip[i] = 1; 82 | 83 | while (1) { 84 | int preTip = 0; 85 | for (int i = 0; i < n; i++) 86 | preTip += rowTip[i] + colTip[i]; 87 | for (int i = 0; i < n; i++) { 88 | if (rowTip[i]) { 89 | for (int j = 0; j < n; j++) { 90 | if (mat(i, j) == 0) 91 | colTip[j] = 1; 92 | } 93 | } 94 | } 95 | for (int j = 0; j < n; j++) { 96 | if (colTip[j]) { 97 | for (int i = 0; i < n; i++) { 98 | if (assign[i] == j) 99 | rowTip[i] = 1; 100 | } 101 | } 102 | } 103 | int curTip = 0; 104 | for (int i = 0; i < n; i++) 105 | curTip += rowTip[i] + colTip[i]; 106 | if (preTip == curTip) 107 | break; 108 | } 109 | 110 | //画线 111 | for (int i = 0; i < n; i++) { 112 | if (!rowTip[i]) 113 | rowLine[i] = 1; 114 | if (colTip[i]) 115 | colLine[i] = 1; 116 | } 117 | 118 | //找最小元素 119 | double minEmt = 1000.0; 120 | for (int i = 0; i < n; i++) 121 | for (int j = 0; j < n; j++) 122 | if (!rowLine[i] && !colLine[j] && mat(i, j) < minEmt) 123 | minEmt = mat(i, j); 124 | 125 | //变换 126 | for (int i = 0; i < n; i++) { 127 | if (rowTip[i]) { 128 | for (int j = 0; j < n; j++) 129 | mat(i, j) -= minEmt; 130 | } 131 | } 132 | for (int j = 0; j < n; j++) { 133 | if (colTip[j]) { 134 | for (int i = 0; i < n; i++) 135 | mat(i, j) += minEmt; 136 | } 137 | } 138 | } 139 | 140 | vector HungaryEstimator::solve() { 141 | rowSub(); 142 | colSub(); 143 | 144 | // cout << mat << endl; 145 | 146 | while (!isOptimal()) { 147 | matTrans(); 148 | } 149 | 150 | for (int i = 0; i < n; i++) 151 | totalCost += matRcd(i, assign[i]); 152 | 153 | // cout << "total_cost: " << totalCost << endl; 154 | 155 | return assign; 156 | } 157 | } // namespace night_voyager -------------------------------------------------------------------------------- /src/streetlight_matcher/StreetlightFeatureDatabase.cpp: -------------------------------------------------------------------------------- 1 | #include "streetlight_matcher/StreetlightFeatureDatabase.h" 2 | #include "streetlight_matcher/StreetlightFeature.h" 3 | 4 | namespace night_voyager { 5 | 6 | StreetlightFeatureDatabase::StreetlightFeatureDatabase(const StreetlightFeatureDatabase &other) { 7 | for (const auto &pair : other.features_idlookup) { 8 | features_idlookup[pair.first] = std::make_shared(*pair.second); 9 | } 10 | } 11 | 12 | void StreetlightFeatureDatabase::update_feature(size_t id, double timestamp, const cv::Rect &rect, const Eigen::Vector2f ¢er, const Eigen::Vector2f ¢er_n, 13 | const Eigen::Vector2f &noise) { 14 | std::lock_guard lck(mtx); 15 | if (features_idlookup.find(id) != features_idlookup.end()) { 16 | 17 | std::shared_ptr feat = features_idlookup.at(id); 18 | assert(feat->featid == id); 19 | feat->boxes.push_back(rect); 20 | feat->uvs.push_back(center); 21 | feat->uvs_norm.push_back(center_n); 22 | feat->timestamps.push_back(timestamp); 23 | return; 24 | } 25 | 26 | std::shared_ptr feat = std::make_shared(); 27 | feat->featid = id; 28 | feat->boxes.push_back(rect); 29 | feat->uvs.push_back(center); 30 | feat->uvs_norm.push_back(center_n); 31 | feat->timestamps.push_back(timestamp); 32 | 33 | features_idlookup[id] = feat; 34 | } 35 | 36 | void StreetlightFeatureDatabase::update_feature(std::shared_ptr feature) { 37 | 38 | assert(feature->boxes.size() == feature->timestamps.size()); 39 | assert(feature->boxes.size() == feature->uvs.size()); 40 | assert(feature->boxes.size() == feature->uvs_norm.size()); 41 | 42 | std::lock_guard lck(mtx); 43 | if (features_idlookup.find(feature->featid) != features_idlookup.end()) { 44 | 45 | std::shared_ptr feat = features_idlookup.at(feature->featid); 46 | assert(feat->featid == feature->featid); 47 | feat->boxes.insert(feat->boxes.end(), feature->boxes.begin(), feature->boxes.end()); 48 | feat->uvs.insert(feat->uvs.end(), feature->uvs.begin(), feature->uvs.end()); 49 | feat->uvs_norm.insert(feat->uvs_norm.end(), feature->uvs_norm.begin(), feature->uvs_norm.end()); 50 | feat->timestamps.insert(feat->timestamps.end(), feature->timestamps.begin(), feature->timestamps.end()); 51 | return; 52 | } 53 | 54 | std::shared_ptr feat = std::make_shared(); 55 | feat->featid = feature->featid; 56 | feat->boxes = feature->boxes; 57 | feat->uvs = feature->uvs; 58 | feat->uvs_norm = feature->uvs_norm; 59 | feat->timestamps = feature->timestamps; 60 | 61 | features_idlookup[feat->featid] = feat; 62 | } 63 | 64 | void StreetlightFeatureDatabase::clean_old_measurements(const std::vector &valid_times) { 65 | 66 | std::lock_guard lck(mtx); 67 | auto it0 = features_idlookup.begin(); 68 | while (it0 != features_idlookup.end()) { 69 | 70 | it0->second->clean_old_measurements(valid_times); 71 | 72 | // Count how many measurements 73 | int ct_meas = it0->second->timestamps.size(); 74 | 75 | // Remove if we don't have enough 76 | if (ct_meas < 1) { 77 | it0->second->to_delete = true; 78 | it0 = features_idlookup.erase(it0); 79 | } else { 80 | it0++; 81 | } 82 | } 83 | } 84 | 85 | size_t StreetlightFeatureDatabase::count_measurements() { 86 | 87 | std::lock_guard lck(mtx); 88 | size_t max_mea_size = 0; 89 | auto iter_feature = features_idlookup.begin(); 90 | while (iter_feature != features_idlookup.end()) { 91 | max_mea_size += 2 * iter_feature->second->timestamps.size(); 92 | iter_feature++; 93 | } 94 | return max_mea_size; 95 | } 96 | 97 | } // namespace night_voyager -------------------------------------------------------------------------------- /src/utils/Print.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #include "utils/Print.h" 12 | 13 | namespace night_voyager { 14 | 15 | // Need to define the static variable for everything to work 16 | Printer::PrintLevel Printer::current_print_level = PrintLevel::INFO; 17 | 18 | void Printer::setPrintLevel(const std::string &level) { 19 | if (level == "ALL") 20 | setPrintLevel(PrintLevel::ALL); 21 | else if (level == "DEBUG") 22 | setPrintLevel(PrintLevel::DEBUG); 23 | else if (level == "INFO") 24 | setPrintLevel(PrintLevel::INFO); 25 | else if (level == "WARNING") 26 | setPrintLevel(PrintLevel::WARNING); 27 | else if (level == "ERROR") 28 | setPrintLevel(PrintLevel::ERROR); 29 | else if (level == "SILENT") 30 | setPrintLevel(PrintLevel::SILENT); 31 | else { 32 | std::cout << "Invalid print level requested: " << level << std::endl; 33 | std::cout << "Valid levels are: ALL, DEBUG, INFO, WARNING, ERROR, SILENT" << std::endl; 34 | std::exit(EXIT_FAILURE); 35 | } 36 | } 37 | 38 | void Printer::setPrintLevel(PrintLevel level) { 39 | Printer::current_print_level = level; 40 | std::cout << "Setting printing level to: "; 41 | switch (current_print_level) { 42 | case PrintLevel::ALL: 43 | std::cout << "ALL"; 44 | break; 45 | case PrintLevel::DEBUG: 46 | std::cout << "DEBUG"; 47 | break; 48 | case PrintLevel::INFO: 49 | std::cout << "INFO"; 50 | break; 51 | case PrintLevel::WARNING: 52 | std::cout << "WARNING"; 53 | break; 54 | case PrintLevel::ERROR: 55 | std::cout << "ERROR"; 56 | break; 57 | case PrintLevel::SILENT: 58 | std::cout << "SILENT"; 59 | break; 60 | default: 61 | std::cout << std::endl; 62 | std::cout << "Invalid print level requested: " << level << std::endl; 63 | std::cout << "Valid levels are: ALL, DEBUG, INFO, WARNING, ERROR, SILENT" << std::endl; 64 | std::exit(EXIT_FAILURE); 65 | } 66 | std::cout << std::endl; 67 | } 68 | 69 | void Printer::debugPrint(PrintLevel level, const char location[], const char line[], const char *format, ...) { 70 | // Only print for the current debug level 71 | if (static_cast(level) < static_cast(Printer::current_print_level)) { 72 | return; 73 | } 74 | 75 | // Print the location info first for our debug output 76 | // Truncate the filename to the max size for the filepath 77 | if (static_cast(Printer::current_print_level) <= static_cast(Printer::PrintLevel::DEBUG)) { 78 | std::string path(location); 79 | std::string base_filename = path.substr(path.find_last_of("/\\") + 1); 80 | if (base_filename.size() > MAX_FILE_PATH_LEGTH) { 81 | printf("%s", base_filename.substr(base_filename.size() - MAX_FILE_PATH_LEGTH, base_filename.size()).c_str()); 82 | } else { 83 | printf("%s", base_filename.c_str()); 84 | } 85 | printf(":%s ", line); 86 | } 87 | 88 | // Print the rest of the args 89 | va_list args; 90 | va_start(args, format); 91 | vprintf(format, args); 92 | va_end(args); 93 | } 94 | 95 | } // namespace night_voyager -------------------------------------------------------------------------------- /src/visualizer/CameraPoseVisualization.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Night-Voyager: Consistent and Efficient Nocturnal Vision-Aided State Estimation in Object Maps 3 | * Copyright (C) 2025 Night-Voyager Contributors 4 | * 5 | * For technical issues and support, please contact Tianxiao Gao at 6 | * or Mingle Zhao at . For commercial use, please contact Prof. Hui Kong at . 7 | * 8 | * This file is subject to the terms and conditions outlined in the 'LICENSE' file, 9 | * which is included as part of this source code package. 10 | */ 11 | #include "visualizer/CameraPoseVisualization.h" 12 | 13 | namespace night_voyager { 14 | 15 | const Eigen::Vector3d CameraPoseVisualization::imlt = Eigen::Vector3d(-1.0, -0.5, 1.0); 16 | const Eigen::Vector3d CameraPoseVisualization::imrt = Eigen::Vector3d(1.0, -0.5, 1.0); 17 | const Eigen::Vector3d CameraPoseVisualization::imlb = Eigen::Vector3d(-1.0, 0.5, 1.0); 18 | const Eigen::Vector3d CameraPoseVisualization::imrb = Eigen::Vector3d(1.0, 0.5, 1.0); 19 | const Eigen::Vector3d CameraPoseVisualization::lt0 = Eigen::Vector3d(-0.7, -0.5, 1.0); 20 | const Eigen::Vector3d CameraPoseVisualization::lt1 = Eigen::Vector3d(-0.7, -0.2, 1.0); 21 | const Eigen::Vector3d CameraPoseVisualization::lt2 = Eigen::Vector3d(-1.0, -0.2, 1.0); 22 | const Eigen::Vector3d CameraPoseVisualization::oc = Eigen::Vector3d(0.0, 0.0, 0.0); 23 | 24 | void CameraPoseVisualization::Eigen2Point(const Eigen::Vector3d &v, geometry_msgs::Point &p) { 25 | p.x = v.x(); 26 | p.y = v.y(); 27 | p.z = v.z(); 28 | } 29 | 30 | void CameraPoseVisualization::add_pose(const Eigen::Vector3d &p, const Eigen::Quaterniond &q) { 31 | visualization_msgs::Marker marker; 32 | 33 | marker.ns = ""; 34 | marker.id = m_markers.size() + 1; 35 | marker.type = visualization_msgs::Marker::LINE_STRIP; 36 | marker.action = visualization_msgs::Marker::ADD; 37 | marker.scale.x = m_line_width; 38 | 39 | marker.pose.position.x = 0.0; 40 | marker.pose.position.y = 0.0; 41 | marker.pose.position.z = 0.0; 42 | marker.pose.orientation.w = 1.0; 43 | marker.pose.orientation.x = 0.0; 44 | marker.pose.orientation.y = 0.0; 45 | marker.pose.orientation.z = 0.0; 46 | 47 | geometry_msgs::Point pt_lt, pt_lb, pt_rt, pt_rb, pt_oc, pt_lt0, pt_lt1, pt_lt2; 48 | 49 | Eigen2Point(q * (m_scale * imlt) + p, pt_lt); 50 | Eigen2Point(q * (m_scale * imlb) + p, pt_lb); 51 | Eigen2Point(q * (m_scale * imrt) + p, pt_rt); 52 | Eigen2Point(q * (m_scale * imrb) + p, pt_rb); 53 | Eigen2Point(q * (m_scale * lt0) + p, pt_lt0); 54 | Eigen2Point(q * (m_scale * lt1) + p, pt_lt1); 55 | Eigen2Point(q * (m_scale * lt2) + p, pt_lt2); 56 | Eigen2Point(q * (m_scale * oc) + p, pt_oc); 57 | 58 | // image boundaries 59 | marker.points.push_back(pt_lt); 60 | marker.points.push_back(pt_lb); 61 | marker.colors.push_back(m_image_boundary_color); 62 | marker.colors.push_back(m_image_boundary_color); 63 | 64 | marker.points.push_back(pt_lb); 65 | marker.points.push_back(pt_rb); 66 | marker.colors.push_back(m_image_boundary_color); 67 | marker.colors.push_back(m_image_boundary_color); 68 | 69 | marker.points.push_back(pt_rb); 70 | marker.points.push_back(pt_rt); 71 | marker.colors.push_back(m_image_boundary_color); 72 | marker.colors.push_back(m_image_boundary_color); 73 | 74 | marker.points.push_back(pt_rt); 75 | marker.points.push_back(pt_lt); 76 | marker.colors.push_back(m_image_boundary_color); 77 | marker.colors.push_back(m_image_boundary_color); 78 | 79 | // top-left indicator 80 | marker.points.push_back(pt_lt0); 81 | marker.points.push_back(pt_lt1); 82 | marker.colors.push_back(m_image_boundary_color); 83 | marker.colors.push_back(m_image_boundary_color); 84 | 85 | marker.points.push_back(pt_lt1); 86 | marker.points.push_back(pt_lt2); 87 | marker.colors.push_back(m_image_boundary_color); 88 | marker.colors.push_back(m_image_boundary_color); 89 | 90 | // optical center connector 91 | marker.points.push_back(pt_lt); 92 | marker.points.push_back(pt_oc); 93 | marker.colors.push_back(m_optical_center_connector_color); 94 | marker.colors.push_back(m_optical_center_connector_color); 95 | 96 | marker.points.push_back(pt_lb); 97 | marker.points.push_back(pt_oc); 98 | marker.colors.push_back(m_optical_center_connector_color); 99 | marker.colors.push_back(m_optical_center_connector_color); 100 | 101 | marker.points.push_back(pt_rt); 102 | marker.points.push_back(pt_oc); 103 | marker.colors.push_back(m_optical_center_connector_color); 104 | marker.colors.push_back(m_optical_center_connector_color); 105 | 106 | marker.points.push_back(pt_rb); 107 | marker.points.push_back(pt_oc); 108 | marker.colors.push_back(m_optical_center_connector_color); 109 | marker.colors.push_back(m_optical_center_connector_color); 110 | 111 | m_markers.push_back(marker); 112 | } 113 | 114 | void CameraPoseVisualization::publish_by(ros::Publisher &pub, const double &stamp) { 115 | visualization_msgs::MarkerArray markerArray_msg; 116 | for (auto &marker : m_markers) { 117 | marker.header.frame_id = "global"; 118 | marker.header.stamp = ros::Time().fromSec(stamp); 119 | markerArray_msg.markers.push_back(marker); 120 | } 121 | 122 | pub.publish(markerArray_msg); 123 | } 124 | } // namespace night_voyager -------------------------------------------------------------------------------- /supp/Supplementary_Material.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRL/Night-Voyager/1e14aa726ebccc78438d2fc46551dbd494579942/supp/Supplementary_Material.pdf -------------------------------------------------------------------------------- /thirdparty/FindTBB.cmake: -------------------------------------------------------------------------------- 1 | find_path(TBB_INCLUDE_DIR tbb/tbb.h) 2 | find_library(TBB_LIBRARY tbb) 3 | 4 | include(FindPackageHandleStandardArgs) 5 | find_package_handle_standard_args(TBB DEFAULT_MSG TBB_LIBRARY TBB_INCLUDE_DIR) 6 | 7 | if(TBB_FOUND) 8 | set(TBB_LIBRARIES ${TBB_LIBRARY}) 9 | set(TBB_INCLUDE_DIRS ${TBB_INCLUDE_DIR}) 10 | endif() --------------------------------------------------------------------------------