diff --git a/.gitignore b/.gitignore index 6ae5d2a6..cf09f08b 100644 --- a/.gitignore +++ b/.gitignore @@ -47,5 +47,4 @@ borrar/* */ExecMean.txt -!Dockerfile -docker/docker-compose.yml \ No newline at end of file +!Dockerfile \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 8365d21e..580a25d2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,6 +46,8 @@ find_package(Eigen3 3.1.0 REQUIRED) find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(segmenter_ros REQUIRED) +find_package(keyframe_depth_estimator REQUIRED) +find_package(keyframe_depth_validator REQUIRED) find_package(image_transport REQUIRED) find_package(message_filters REQUIRED) find_package(rviz_visual_tools REQUIRED) @@ -66,6 +68,10 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/VSGraphsRoomData.msg" "msg/VSGraphsAllWallsData.msg" "msg/VSGraphsAllDetectdetRooms.msg" + "msg/MapResetEvent.msg" + "msg/MapReadyEvent.msg" + "msg/MapRescaleEvent.msg" + "msg/ImuBiasEstimate.msg" DEPENDENCIES std_msgs geometry_msgs sensor_msgs ) @@ -83,6 +89,8 @@ include_directories( ${Pangolin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${segmenter_ros_INCLUDE_DIRS} + ${keyframe_depth_estimator_INCLUDE_DIRS} + ${keyframe_depth_validator_INCLUDE_DIRS} ) include_directories(${OPENGL_INCLUDE_DIRS}) @@ -210,6 +218,8 @@ ament_target_dependencies(${PROJECT_NAME}_lib image_transport rviz_visual_tools situational_graphs_msgs + keyframe_depth_estimator + keyframe_depth_validator ) # Common source files @@ -228,6 +238,8 @@ set(COMMON_DEPENDENCIES sensor_msgs geometry_msgs segmenter_ros + keyframe_depth_estimator + keyframe_depth_validator message_filters situational_graphs_msgs ) @@ -254,6 +266,7 @@ endfunction() # ROS2 nodes add_ros2_node(ros_rgbd src/ros_rgbd.cc) add_ros2_node(ros_rgbd_inertial src/ros_rgbd_inertial.cc) +add_ros2_node(ros_mono_inertial src/ros_mono_inertial.cc) # Install other resources install( @@ -267,4 +280,4 @@ install(TARGETS ${PROJECT_NAME}_lib ) # Ament package configuration -ament_package() \ No newline at end of file +ament_package() diff --git a/README.md b/README.md index 55789407..2cd3f31b 100644 --- a/README.md +++ b/README.md @@ -43,6 +43,7 @@ Once you have installed the required dependencies and configured the parameters, > 🛎️ Note: The current version of vS-Graphs supports **ROS2 Jazzy** and is primarily tested on Ubuntu 24.04.2 LTS. +> 🛎️ Note: mono-inertial vS-Graphs is currently under development, having a stable version at commit ea5a79819e5f82a9a420cc4cc6b6709197dbf991 ## 🐋 Docker For a fully reproducible, environment-independent setup, see the [Docker](/docker) section. @@ -73,3 +74,13 @@ To evaluate vS-Graphs against other visual SLAM frameworks, read the [evaluation ## 🔑 License This project is licensed under the GPL-3.0 license - see the [LICENSE](/LICENSE) for more details. + + +## Usage + +```bash +ros2 launch vs_graphs mono-imu.launch.py \ + launch_keyframe_depth_estimator:=false \ + keyframe_depth_validator_use_offline_metric_depth:=true \ + keyframe_depth_validator_offline_sync_tolerance_ms:=20.0 +``` \ No newline at end of file diff --git a/config/Monocular-Inertial/EuRoC.yaml b/config/Monocular-Inertial/EuRoC.yaml index 47284534..4ca8b32b 100644 --- a/config/Monocular-Inertial/EuRoC.yaml +++ b/config/Monocular-Inertial/EuRoC.yaml @@ -42,6 +42,7 @@ IMU.T_b_c1: !!opencv-matrix 0.0, 0.0, 0.0, 1.0] # IMU noise +IMU.Threshold: 0.5 # Default: 0.5 IMU.NoiseGyro: 1.7e-4 #1.6968e-04 IMU.NoiseAcc: 2.0000e-3 #2.0e-3 IMU.GyroWalk: 1.9393e-05 @@ -81,4 +82,3 @@ Viewer.ViewpointX: 0.0 Viewer.ViewpointY: -1.7 Viewer.ViewpointZ: 0.8 Viewer.ViewpointF: -500.0 - diff --git a/config/Monocular-Inertial/LuxVSD_RS_D435i_640.yaml b/config/Monocular-Inertial/LuxVSD_RS_D435i_640.yaml index 72394556..1549c315 100755 --- a/config/Monocular-Inertial/LuxVSD_RS_D435i_640.yaml +++ b/config/Monocular-Inertial/LuxVSD_RS_D435i_640.yaml @@ -43,6 +43,7 @@ IMU.T_b_c1: !!opencv-matrix IMU.InsertKFsWhenLost: 0 # IMU noise (Use those from VINS-mono) +IMU.Threshold: 0.5 # Default: 0.5 IMU.NoiseGyro: 1e-3 # 2.44e-4 #1e-3 # rad/s^0.5 IMU.NoiseAcc: 1e-2 # 1.47e-3 #1e-2 # m/s^1.5 IMU.GyroWalk: 1e-6 # rad/s^1.5 diff --git a/config/Monocular-Inertial/RealSense_D435i.yaml b/config/Monocular-Inertial/RealSense_D435i.yaml index 7b67ddd0..ed46c4f0 100755 --- a/config/Monocular-Inertial/RealSense_D435i.yaml +++ b/config/Monocular-Inertial/RealSense_D435i.yaml @@ -43,6 +43,7 @@ IMU.T_b_c1: !!opencv-matrix IMU.InsertKFsWhenLost: 0 # IMU noise (Use those from VINS-mono) +IMU.Threshold: 0.5 # Default: 0.5 IMU.NoiseGyro: 1e-3 # 2.44e-4 #1e-3 # rad/s^0.5 IMU.NoiseAcc: 1e-2 # 1.47e-3 #1e-2 # m/s^1.5 IMU.GyroWalk: 1e-6 # rad/s^1.5 diff --git a/config/Monocular-Inertial/RealSense_T265.yaml b/config/Monocular-Inertial/RealSense_T265.yaml index e9043c66..089d3fdb 100755 --- a/config/Monocular-Inertial/RealSense_T265.yaml +++ b/config/Monocular-Inertial/RealSense_T265.yaml @@ -43,6 +43,7 @@ IMU.T_b_c1: !!opencv-matrix IMU.InsertKFsWhenLost: 0 # IMU noise (Use those from VINS-mono) +IMU.Threshold: 0.5 # Default: 0.5 IMU.NoiseGyro: 0.000005148030141 # rad/s^0.5 IMU.NoiseAcc: 0.000066952452471 # m/s^1.5 IMU.GyroWalk: 0.000000499999999 # rad/s^1.5 diff --git a/config/Monocular-Inertial/TUM-VI.yaml b/config/Monocular-Inertial/TUM-VI.yaml index b882f33f..ae825d49 100755 --- a/config/Monocular-Inertial/TUM-VI.yaml +++ b/config/Monocular-Inertial/TUM-VI.yaml @@ -41,6 +41,7 @@ IMU.T_b_c1: !!opencv-matrix 0.0, 0.0, 0.0, 1.0] # IMU noise (Use those from VINS-mono) +IMU.Threshold: 0.5 # Default: 0.5 IMU.NoiseGyro: 0.00016 # rad/s^0.5 IMU.NoiseAcc: 0.0028 # m/s^1.5 IMU.GyroWalk: 0.000022 # rad/s^1.5 diff --git a/config/Monocular-Inertial/TUM-VI_far.yaml b/config/Monocular-Inertial/TUM-VI_far.yaml index 8ad355aa..dcfe82f8 100755 --- a/config/Monocular-Inertial/TUM-VI_far.yaml +++ b/config/Monocular-Inertial/TUM-VI_far.yaml @@ -42,6 +42,7 @@ IMU.T_b_c1: !!opencv-matrix # IMU noise (Use those from VINS-mono) +IMU.Threshold: 0.5 # Default: 0.5 IMU.NoiseGyro: 0.00016 # 0.004 (VINS) # 0.00016 (TUM) # 0.00016 # rad/s^0.5 IMU.NoiseAcc: 0.0028 # 0.04 (VINS) # 0.0028 (TUM) # 0.0028 # m/s^1.5 IMU.GyroWalk: 0.000022 # 0.000022 (VINS and TUM) rad/s^1.5 diff --git a/config/Monocular-Inertial/UniLu_RealSense_D435i_640.yaml b/config/Monocular-Inertial/UniLu_RealSense_D435i_640.yaml index ba056d39..a65e4931 100755 --- a/config/Monocular-Inertial/UniLu_RealSense_D435i_640.yaml +++ b/config/Monocular-Inertial/UniLu_RealSense_D435i_640.yaml @@ -43,11 +43,12 @@ IMU.T_b_c1: !!opencv-matrix IMU.InsertKFsWhenLost: 0 # IMU noise (Use those from VINS-mono) +IMU.Threshold: 0.5 # Default: 0.5 IMU.NoiseGyro: 1e-3 # 2.44e-4 #1e-3 # rad/s^0.5 IMU.NoiseAcc: 1e-2 # 1.47e-3 #1e-2 # m/s^1.5 IMU.GyroWalk: 1e-6 # rad/s^1.5 IMU.AccWalk: 1e-4 # m/s^2.5 -IMU.Frequency: 200.0 +IMU.Frequency: 400.0 #-------------------------------------------------------------------------------------------- # ORB Parameters diff --git a/config/Monocular-Inertial/UniLu_RealSense_D435i_640_g2step4rerun.yaml b/config/Monocular-Inertial/UniLu_RealSense_D435i_640_g2step4rerun.yaml new file mode 100644 index 00000000..79f29ee1 --- /dev/null +++ b/config/Monocular-Inertial/UniLu_RealSense_D435i_640_g2step4rerun.yaml @@ -0,0 +1,90 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. Adjust them! (RealSense D435 camera - Serial Number: 044322070965) +#-------------------------------------------------------------------------------------------- +File.version: "1.0" + +Camera.type: "PinHole" + +# Rectified Camera calibration (OpenCV) +Camera1.fx: 605.856689453125 +Camera1.fy: 606.26611328125 +Camera1.cx: 325.021026611328 +Camera1.cy: 244.760711669922 + +# Distortion parameters +Camera1.k1: 0.0 +Camera1.k2: 0.0 +Camera1.p1: 0.0 +Camera1.p2: 0.0 + +# Camera resolution +Camera.width: 640 +Camera.height: 480 + +# Camera frames per second +Camera.fps: 30 + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 1 + +# Transformation from body-frame (imu) to left camera +IMU.T_b_c1: !!opencv-matrix + rows: 4 + cols: 4 + dt: f + data: [1,0,0,-0.005, + 0,1,0,-0.005, + 0,0,1,0.0117, + 0.0, 0.0, 0.0, 1.0] + +# Do not insert KFs when recently lost +IMU.InsertKFsWhenLost: 0 + +# G2 Step 4 rerun (patched backend): enable IMU-aided relocalization during +# RECENTLY_LOST, ported from BinoSLAM. Off by default upstream in this fork; +# turned on here only, so this file is a rerun-only copy of +# UniLu_RealSense_D435i_640.yaml, not a shared-config edit. +IMU.ImuAidedReloc: 1 + +# IMU noise (Use those from VINS-mono) +IMU.Threshold: 0.5 # Default: 0.5 +IMU.NoiseGyro: 1e-3 # 2.44e-4 #1e-3 # rad/s^0.5 +IMU.NoiseAcc: 1e-2 # 1.47e-3 #1e-2 # m/s^1.5 +IMU.GyroWalk: 1e-6 # rad/s^1.5 +IMU.AccWalk: 1e-4 # m/s^2.5 +IMU.Frequency: 400.0 + +#-------------------------------------------------------------------------------------------- +# ORB Parameters +#-------------------------------------------------------------------------------------------- +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 1250 + +# ORB Extractor: Scale factor between levels in the scale pyramid +ORBextractor.scaleFactor: 1.2 + +# ORB Extractor: Number of levels in the scale pyramid +ORBextractor.nLevels: 8 + +# ORB Extractor: Fast threshold +# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. +# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST +# You can lower these values if your images have low contrast +ORBextractor.iniThFAST: 20 +ORBextractor.minThFAST: 7 + +#-------------------------------------------------------------------------------------------- +# Viewer Parameters +#-------------------------------------------------------------------------------------------- +Viewer.KeyFrameSize: 0.05 +Viewer.KeyFrameLineWidth: 1.0 +Viewer.GraphLineWidth: 0.9 +Viewer.PointSize: 2.0 +Viewer.CameraSize: 0.08 +Viewer.CameraLineWidth: 3.0 +Viewer.ViewpointX: 0.0 +Viewer.ViewpointY: -0.7 +Viewer.ViewpointZ: -3.5 +Viewer.ViewpointF: 500.0 diff --git a/config/Monocular-Inertial/uHumans2_TESSE.yaml b/config/Monocular-Inertial/uHumans2_TESSE.yaml new file mode 100644 index 00000000..eb4bf44c --- /dev/null +++ b/config/Monocular-Inertial/uHumans2_TESSE.yaml @@ -0,0 +1,91 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. TESSE simulator (uHumans2 dataset) - left_cam +# Extracted directly from the recorded bag: +# /tesse/left_cam/camera_info -> intrinsics (720x480, fx=fy=415.692, cx=360, cy=240, zero distortion) +# /tf_static (base_link_gt -> left_cam) -> IMU.T_b_c1 (t=[0,0.05,0], q=[0.5,-0.5,0.5,-0.5]) +#-------------------------------------------------------------------------------------------- +File.version: "1.0" + +Camera.type: "PinHole" + +# Rectified Camera calibration (OpenCV) +Camera1.fx: 415.69219381653056 +Camera1.fy: 415.69219381653056 +Camera1.cx: 360.0 +Camera1.cy: 240.0 + +# Distortion parameters (TESSE renders undistorted images) +Camera1.k1: 0.0 +Camera1.k2: 0.0 +Camera1.p1: 0.0 +Camera1.p2: 0.0 + +# Camera resolution +Camera.width: 720 +Camera.height: 480 + +# Camera frames per second (measured from consecutive /tesse/left_cam header stamps, median dt=0.1s) +Camera.fps: 10 + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 1 + +# Transformation from body-frame (imu, "base_link_gt") to left camera ("left_cam") +# Derived from the bag's own /tf_static: t=[0, 0.05, 0], q=[x=0.5, y=-0.5, z=0.5, w=-0.5] +IMU.T_b_c1: !!opencv-matrix + rows: 4 + cols: 4 + dt: f + data: [0.0, 0.0, 1.0, 0.0, + -1.0, 0.0, 0.0, 0.05, + 0.0, -1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0] + +# Do not insert KFs when recently lost +IMU.InsertKFsWhenLost: 0 + +# IMU noise - TESSE's actual simulated noise model is not recorded in the bag; +# these are generic starting values (same family as VINS-mono defaults used in the +# other configs here). Tune IMU.NoiseGyro/NoiseAcc/GyroWalk/AccWalk against the +# /tesse/imu/clean/imu vs /tesse/imu/noisy/imu topics if initialization remains unstable. +IMU.Threshold: 0.5 +IMU.NoiseGyro: 1e-3 +IMU.NoiseAcc: 1e-2 +IMU.GyroWalk: 1e-6 +IMU.AccWalk: 1e-4 +IMU.Frequency: 200.0 + +#-------------------------------------------------------------------------------------------- +# ORB Parameters +#-------------------------------------------------------------------------------------------- +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 1250 + +# ORB Extractor: Scale factor between levels in the scale pyramid +ORBextractor.scaleFactor: 1.2 + +# ORB Extractor: Number of levels in the scale pyramid +ORBextractor.nLevels: 8 + +# ORB Extractor: Fast threshold +# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. +# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST +# You can lower these values if your images have low contrast +ORBextractor.iniThFAST: 20 +ORBextractor.minThFAST: 7 + +#-------------------------------------------------------------------------------------------- +# Viewer Parameters +#-------------------------------------------------------------------------------------------- +Viewer.KeyFrameSize: 0.05 +Viewer.KeyFrameLineWidth: 1.0 +Viewer.GraphLineWidth: 0.9 +Viewer.PointSize: 2.0 +Viewer.CameraSize: 0.08 +Viewer.CameraLineWidth: 3.0 +Viewer.ViewpointX: 0.0 +Viewer.ViewpointY: -0.7 +Viewer.ViewpointZ: -3.5 +Viewer.ViewpointF: 500.0 diff --git a/config/Visualization/vsgraphs_rgbd.rviz b/config/Visualization/vsgraphs_rgbd.rviz index 56edc5b1..c440f029 100644 --- a/config/Visualization/vsgraphs_rgbd.rviz +++ b/config/Visualization/vsgraphs_rgbd.rviz @@ -1,12 +1,18 @@ Panels: - Class: rviz_common/Displays - Help Height: 157 + Help Height: 70 Name: Displays Property Tree Widget: Expanded: + - /Global Options1 - /IMU1/Shape1 + - /All Points1 + - /Segmented PC1 + - /Keyframe Depth Debug1 + - /Keyframe Depth Validator Debug1 + - /Object Motion Debug Image1 Splitter Ratio: 0.679411768913269 - Tree Height: 621 + Tree Height: 760 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -24,7 +30,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: Tracked Points + SyncSource: All Points Visualization Manager: Class: "" Displays: @@ -91,7 +97,7 @@ Visualization Manager: Value: /vs_graphs/tracking_image Value: true - Class: rviz_default_plugins/Image - Enabled: true + Enabled: false Max Value: 1 Median window: 5 Min Value: 0 @@ -103,7 +109,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /camera/color/image_segment_vis - Value: true + Value: false - Alpha: 0.699999988079071 Buffer Length: 1 Class: rviz_default_plugins/Path @@ -195,7 +201,7 @@ Visualization Manager: Reliability Policy: Reliable Value: /vs_graphs/body_odom Value: true - - Alpha: 0.3499999940395355 + - Alpha: 0.800000011920929 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 @@ -217,7 +223,7 @@ Visualization Manager: Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.019999999552965164 + Size (m): 0.029999999329447746 Style: Spheres Topic: Depth: 5 @@ -262,7 +268,7 @@ Visualization Manager: Use rainbow: true Value: true - Class: rviz_default_plugins/MarkerArray - Enabled: true + Enabled: false Name: Fiducial Markers Namespaces: {} @@ -272,7 +278,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /vs_graphs/fiducial_markers - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -348,10 +354,10 @@ Visualization Manager: Axis: X Channel Name: intensity Class: rviz_default_plugins/PointCloud2 - Color: 255; 170; 0 + Color: 255; 255; 0 Color Transformer: FlatColor Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: true Max Color: 255; 255; 255 Max Intensity: 4096 @@ -362,7 +368,7 @@ Visualization Manager: Selectable: true Size (Pixels): 3 Size (m): 0.019999999552965164 - Style: Flat Squares + Style: Boxes Topic: Depth: 5 Durability Policy: Volatile @@ -371,9 +377,9 @@ Visualization Manager: Value: /vs_graphs/segmented_point_clouds Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Class: rviz_default_plugins/MarkerArray - Enabled: false + Enabled: true Name: Plane Labels Namespaces: {} @@ -383,7 +389,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /vs_graphs/plane_labels - Value: false + Value: true - Class: rviz_default_plugins/MarkerArray Enabled: false Name: Voxblox Skeleton @@ -433,13 +439,7 @@ Visualization Manager: Enabled: true Name: Structural Elements Namespaces: - "": true - floorLabels: true - floorRoomEdges: true - floors: true - room: true - roomLabel: true - roomWallLine: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -480,9 +480,345 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Depth Estimate + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/depth_da3/image_rect + Value: false + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Instance Masks + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/color/image_instance_masks + Value: false + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Keyframe Depth Debug + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /keyframe_depth/debug_image + Value: false + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Keyframe Depth Validator Debug + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /keyframe_depth_validator/debug_image + Value: false + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Tuning Debug Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /keyframe_depth_validator/tuning_debug_image + Value: false + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Dynamic Keypoint Lifter Debug + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /dynamic_keypoint_3d_lifter/debug_image + Value: false + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Dynamic Keypoint Tracker Debug + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /dynamic_keypoint_tracker/debug_image + Value: false + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Object Track Manager Debug + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /object_track_manager/debug_image + Value: true + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Dynamic Keypoint Interpolator Debug + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /dynamic_keypoint_interpolator/debug_image + Value: false + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Object Motion Debug Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /object_motion/debug_image + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: "Pipeline A: depth model" + Namespaces: + track_121: true + track_127: true + track_129: true + track_131: true + track_132: true + track_135: true + track_136: true + track_137: true + track_139: true + track_140: true + track_142: true + track_143: true + track_145: true + track_146: true + track_147: true + track_148: true + track_149: true + track_150: true + track_151: true + track_155: true + track_158: true + track_159: true + track_165: true + track_166: true + track_167: true + track_168: true + track_174: true + track_175: true + track_177: true + track_183: true + track_184: true + track_185: true + track_189: true + track_193: true + track_195: true + track_198: true + track_199: true + track_201: true + track_202: true + track_203: true + track_205: true + track_206: true + track_209: true + track_213: true + track_214: true + track_215: true + track_219: true + track_223: true + track_224: true + track_225: true + track_227: true + track_228: true + track_229: true + track_231: true + track_233: true + track_234: true + track_237: true + track_238: true + track_241: true + track_245: true + track_246: true + track_248: true + track_249: true + track_251: true + track_255: true + track_256: true + track_257: true + track_261: true + track_262: true + track_273: true + track_288: true + track_289: true + track_290: true + track_292: true + track_293: true + track_294: true + track_295: true + track_298: true + track_302: true + track_303: true + track_304: true + track_305: true + track_307: true + track_309: true + track_310: true + track_316: true + track_318: true + track_323: true + track_326: true + track_337: true + track_338: true + track_340: true + track_341: true + track_345: true + track_349: true + track_351: true + track_353: true + track_354: true + track_358: true + track_361: true + track_362: true + track_364: true + track_365: true + track_367: true + track_368: true + track_369: true + track_370: true + track_373: true + track_374: true + track_376: true + track_377: true + track_378: true + track_381: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /object_motion/depth_model/markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: "Joint window: object pose+landmarks (D12)" + Namespaces: + track_132: true + track_135: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /object_motion/joint/markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: "Pipeline B: BA+triangulation (retired)" + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /object_motion/ba/markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Ground Truth (live, bright green) + Namespaces: + gt_object_0: true + gt_object_1: true + gt_object_10: true + gt_object_11: true + gt_object_12: true + gt_object_13: true + gt_object_14: true + gt_object_15: true + gt_object_16: true + gt_object_17: true + gt_object_18: true + gt_object_19: true + gt_object_2: true + gt_object_20: true + gt_object_21: true + gt_object_22: true + gt_object_23: true + gt_object_24: true + gt_object_25: true + gt_object_26: true + gt_object_27: true + gt_object_28: true + gt_object_29: true + gt_object_3: true + gt_object_30: true + gt_object_31: true + gt_object_32: true + gt_object_33: true + gt_object_34: true + gt_object_35: true + gt_object_4: true + gt_object_5: true + gt_object_6: true + gt_object_7: true + gt_object_8: true + gt_object_9: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /ground_truth/markers + Value: true Enabled: true Global Options: - Background Color: 255; 255; 255 + Background Color: 122; 122; 122 Fixed Frame: map Frame Rate: 30 Name: root @@ -526,33 +862,51 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 12 + Distance: 8.140125274658203 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.9686291813850403 - Y: -0.3703233003616333 - Z: -0.6169102191925049 + X: 1.705122709274292 + Y: 30.160234451293945 + Z: 7.017193794250488 Focal Shape Fixed Size: false Focal Shape Size: 0.0010000000474974513 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.919999897480011 + Pitch: 0.5100010633468628 Target Frame: camera Value: Orbit (rviz_default_plugins) - Yaw: 4.145030975341797 + Yaw: 2.04367995262146 Saved: ~ Window Geometry: + Depth Estimate: + collapsed: false Displays: collapsed: false - Height: 991 + Dynamic Keypoint Interpolator Debug: + collapsed: false + Dynamic Keypoint Lifter Debug: + collapsed: false + Dynamic Keypoint Tracker Debug: + collapsed: false + Height: 1043 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000015600000345fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000345000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000002d2000000c70000000000000000000000010000012900000345fc0200000008fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb000000140052006100770020004600720061006d00650073000000003b000000c20000001600fffffffb0000000a0049006d00610067006500000000e5000000a50000000000000000fb0000001c0054007200610063006b006500640020004600720061006d00650073010000003b000000ed0000001600fffffffb0000000a0049006d006100670065010000010d000000d40000000000000000fb00000026005300650067006d0065006e0074006500640020004b00650079004600720061006d00650073010000012e000000e10000001600fffffffb0000000a0056006900650077007301000002150000016b000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004f50000034500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Instance Masks: + collapsed: false + Keyframe Depth Debug: + collapsed: false + Keyframe Depth Validator Debug: + collapsed: false + Object Motion Debug Image: + collapsed: false + Object Track Manager Debug: + collapsed: false + QMainWindow State: 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 Raw Frames: collapsed: false Segmented KeyFrames: @@ -565,6 +919,8 @@ Window Geometry: collapsed: false Tracked Frames: collapsed: false + Tuning Debug Image: + collapsed: false Views: collapsed: false Width: 1920 diff --git a/core/include/Frame.h b/core/include/Frame.h index 5832cb72..dc96dc66 100644 --- a/core/include/Frame.h +++ b/core/include/Frame.h @@ -92,7 +92,8 @@ namespace ORB_SLAM3 Frame(const cv::Mat &imColor, const cv::Mat &imGray, const double &timeStamp, ORBextractor *extractor, ORBVocabulary *voc, GeometricCamera *pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, Frame *pPrevF = static_cast(NULL), const IMU::Calib &ImuCalib = IMU::Calib(), - const std::vector markers = std::vector{}); + const std::vector markers = std::vector{}, + const cv::Mat &mask = cv::Mat()); /** * @brief Extract ORB features from the given grayscale image @@ -102,7 +103,7 @@ namespace ORB_SLAM3 * @param x0 The x-coordinate of the top-left corner of the ROI * @param x1 The x-coordinate of the bottom-right corner of the ROI */ - void ExtractORB(int flag, const cv::Mat &imageGray, const int x0, const int x1); + void ExtractORB(int flag, const cv::Mat &imageGray, const int x0, const int x1, const cv::Mat &mask = cv::Mat()); // Compute Bag of Words representation. void ComputeBoW(); @@ -412,6 +413,8 @@ namespace ORB_SLAM3 } Sophus::SE3 T_test; + + void FilterKeysAndDescriptorsByMask(std::vector& keys, cv::Mat& descriptors, const cv::Mat& mask); }; } // namespace ORB_SLAM diff --git a/core/include/KeyFrame.h b/core/include/KeyFrame.h index 819806d9..bb8b459b 100644 --- a/core/include/KeyFrame.h +++ b/core/include/KeyFrame.h @@ -447,6 +447,15 @@ namespace ORB_SLAM3 // For Semantic Segmentation cv::Mat mImage; bool isPublished; + pcl::PointCloud::Ptr mAuxPointCloud; + bool mbHasAuxPointCloud = false; + double mAuxDepthTimestamp = 0.0; + std::string mAuxDepthFrameId; + void SetAuxPointCloudFromDepth(const cv::Mat &auxDepth, double auxDepthTimestamp, + const std::string &auxDepthFrameId, float auxDepthMin, + float auxDepthMax, int auxDepthStride, + const std::string &auxDepthScaleMode); + pcl::PointCloud::Ptr getAuxPointCloud(); // The following variables need to be accessed trough a mutex to be thread safe. protected: diff --git a/core/include/LocalMapping.h b/core/include/LocalMapping.h index cb25462e..95fdb131 100644 --- a/core/include/LocalMapping.h +++ b/core/include/LocalMapping.h @@ -31,6 +31,7 @@ #include "Settings.h" #include "Types/SystemParams.h" +#include #include namespace ORB_SLAM3 @@ -45,11 +46,14 @@ namespace ORB_SLAM3 { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + using KeyFrameCreatedCallback = std::function; + LocalMapping(System *pSys, Atlas *pAtlas, const float bMonocular, bool bInertial, const string &_strSeqName = std::string()); void SetLoopCloser(LoopClosing *pLoopCloser); void SetTracker(Tracking *pTracker); + void SetKeyFrameCreatedCallback(KeyFrameCreatedCallback callback); // Main function void Run(); @@ -166,6 +170,7 @@ namespace ORB_SLAM3 std::list mlNewKeyFrames; KeyFrame *mpCurrentKeyFrame; + KeyFrameCreatedCallback mKeyFrameCreatedCallback; std::list mlpRecentAddedMapPoints; diff --git a/core/include/Map.h b/core/include/Map.h index 95f84a17..f7d59e03 100644 --- a/core/include/Map.h +++ b/core/include/Map.h @@ -31,6 +31,7 @@ #include #include +#include #include #include #include @@ -167,6 +168,12 @@ namespace ORB_SLAM3 void ApplyScaledRotation(const Sophus::SE3f &T, const float s, const bool bScaledVel = false); + // Last similarity correction (T, s) applied by ApplyScaledRotation, and a version counter + // that increments each time -- lets external consumers that keep their own world-frame + // state (e.g. object_motion_estimator) detect a rescale and apply the same correction. + void GetLastScaleCorrection(Sophus::SE3f &T, float &s); + uint64_t GetScaleCorrectionVersion(); + bool IsInertial(); void SetIniertialBA1(); void SetIniertialBA2(); @@ -257,6 +264,11 @@ namespace ORB_SLAM3 bool mbIMU_BA1; bool mbIMU_BA2; + // Last similarity correction applied by ApplyScaledRotation -- see GetLastScaleCorrection + Sophus::SE3f mLastScaleCorrectionT; + float mLastScaleCorrectionS = 1.0f; + uint64_t mnScaleCorrectionVersion = 0; + // Mutex std::mutex mMutexMap; }; diff --git a/core/include/Settings.h b/core/include/Settings.h index 754766a4..ed84a5c6 100644 --- a/core/include/Settings.h +++ b/core/include/Settings.h @@ -99,6 +99,7 @@ namespace ORB_SLAM3 float noiseGyro() { return noiseGyro_; } Sophus::SE3f Tbc() { return Tbc_; } bool insertKFsWhenLost() { return insertKFsWhenLost_; } + bool imuAidedReloc() { return imuAidedReloc_; } float depthMapFactor() { return depthMapFactor_; } @@ -205,6 +206,7 @@ namespace ORB_SLAM3 float imuThreshold_; Sophus::SE3f Tbc_; bool insertKFsWhenLost_; + bool imuAidedReloc_; /* * RGBD stuff diff --git a/core/include/System.h b/core/include/System.h index 437e5221..d87f3476 100644 --- a/core/include/System.h +++ b/core/include/System.h @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -88,6 +89,7 @@ namespace ORB_SLAM3 class Viewer; class FrameDrawer; class MapDrawer; + class KeyFrame; class Atlas; class Tracking; class LocalMapping; @@ -167,7 +169,9 @@ namespace ORB_SLAM3 */ Sophus::SE3f TrackMonocular(const cv::Mat &im, const double ×tamp, const vector &vImuMeas = vector(), string filename = "", - const vector markers = vector{}); + const vector markers = vector{}, const cv::Mat &mask = cv::Mat()); + + void SetKeyFrameCreatedCallback(std::function callback); // This stops local mapping thread (map building) and performs only camera tracking. void ActivateLocalizationMode(); @@ -248,6 +252,7 @@ namespace ORB_SLAM3 Sophus::SE3f GetCamTwc(); Sophus::SE3f GetImuTwb(); Eigen::Vector3f GetImuVwb(); + IMU::Bias GetImuBias(); bool isImuPreintegrated(); // For debugging @@ -255,6 +260,12 @@ namespace ORB_SLAM3 bool isLost(); bool isFinished(); + // Number of keyframes pending in LocalMapping's queue. Lets a caller feeding + // frames faster than real time (e.g. offline bag replay) apply backpressure so + // LocalMapping's IMU init/BA gets real wall-clock time to keep up, instead of + // racing far ahead of what it has actually processed. + int GetLocalMappingQueueSize(); + void ChangeDataset(); float GetImageScale(); @@ -270,6 +281,10 @@ namespace ORB_SLAM3 * @param tuple the address of the tuple of segmented image and pointcloud */ void addSegmentedImage(std::tuple *tuple); + void AttachAuxDepthToKeyFrame(uint64_t keyFrameId, const cv::Mat &auxDepth, + double auxDepthTimestamp, const std::string &auxDepthFrameId, + float auxDepthMin, float auxDepthMax, int auxDepthStride, + const std::string &auxDepthScaleMode); /** * @brief Get the skeleton cluster coming from the current map @@ -297,6 +312,12 @@ namespace ORB_SLAM3 bool SaveAtlas(int type); bool LoadAtlas(int type); + // Snapshot mCurrentFrame's tracked map points/keypoints into mTrackedMapPoints/ + // mTrackedKeyPointsUn, excluding entries the tracker's own pose optimization has already + // flagged as outliers (mvbOutlier) this frame -- consumers (e.g. publishStaticMapPoints) + // must not receive point/pixel associations ORB-SLAM3 itself has already rejected. + void UpdateTrackedPointsFromCurrentFrame(); + string CalculateCheckSum(string filename, int type); // Input sensor diff --git a/core/include/Tracking.h b/core/include/Tracking.h index 2d0c6be3..fc4bc174 100644 --- a/core/include/Tracking.h +++ b/core/include/Tracking.h @@ -95,7 +95,7 @@ namespace ORB_SLAM3 const std::vector doors, const std::vector rooms); Sophus::SE3f GrabImageMonocular(const cv::Mat &im, const double ×tamp, string filename, const std::vector markers, const std::vector doors, - const std::vector rooms); + const std::vector rooms, const cv::Mat &mask = cv::Mat()); void GrabImuData(const IMU::Point &imuMeasurement); @@ -124,6 +124,7 @@ namespace ORB_SLAM3 Sophus::SE3f GetCamTwc(); Sophus::SE3f GetImuTwb(); Eigen::Vector3f GetImuVwb(); + IMU::Bias GetImuBias(); bool isImuPreintegrated(); void CreateMapInAtlas(); @@ -270,6 +271,14 @@ namespace ORB_SLAM3 bool Relocalization(); + // IMU-aided relocalization: attempted while RECENTLY_LOST (inertial sensors) to + // recover visual tracking via the IMU-predicted pose instead of only coasting on + // PredictStateIMU() until time_recently_lost expires. + bool mbImuAidedReloc = false; + unsigned int mnLastImuRelocFrameId = 0; + bool RelocalizationWithIMU(); + bool VerifyRelocAgainstIMU(const Sophus::SE3f &Tcw_reloc); + void UpdateLocalMap(); void UpdateLocalPoints(); void UpdateLocalKeyFrames(); diff --git a/core/include/Types/SystemParams.h b/core/include/Types/SystemParams.h index 0bceab7e..45362af4 100644 --- a/core/include/Types/SystemParams.h +++ b/core/include/Types/SystemParams.h @@ -160,7 +160,8 @@ namespace ORB_SLAM3 { GEOMETRIC = 0, FREE_SPACE = 1, - GNN = 2 + GNN = 2, + GNN_NEW = 3 }; Method method = FREE_SPACE; @@ -185,4 +186,4 @@ namespace ORB_SLAM3 }; } -#endif \ No newline at end of file +#endif diff --git a/core/src/Atlas.cc b/core/src/Atlas.cc index 19b02454..59f23491 100644 --- a/core/src/Atlas.cc +++ b/core/src/Atlas.cc @@ -117,45 +117,72 @@ namespace ORB_SLAM3 pMapMP->AddMapPoint(pMP); } + namespace + { + // Non-inertial sensors have no separate scale-initialization phase: if a keyframe exists + // to hang an entity off at all, tracking was already OK and scale was always metric. + // Inertial sensors need scale/gravity to have actually converged (GetIniertialBA2 -- + // isImuInitialized() alone is only the earliest, rough bias-only stage, see Tracking.cc's + // own use of this same pairing) before any entity position can be trusted. + bool IsMapReadyForEntities(Map *pMap) + { + return !pMap->IsInertial() || (pMap->isImuInitialized() && pMap->GetIniertialBA2()); + } + } + void Atlas::AddMapMarker(Marker *marker) { Map *pMapMP = marker->getMap(); + if (!IsMapReadyForEntities(pMapMP)) + return; pMapMP->AddMapMarker(marker); } void Atlas::AddMapPlane(ORB_SLAM3::Plane *plane) { ORB_SLAM3::Map *pMapMP = plane->GetMap(); + if (!IsMapReadyForEntities(pMapMP)) + return; pMapMP->AddMapPlane(plane); } void Atlas::AddRoomWallPlane(ORB_SLAM3::Plane *pPlane) { ORB_SLAM3::Map *pMapMP = pPlane->GetMap(); + if (!IsMapReadyForEntities(pMapMP)) + return; pMapMP->AddRoomWallPlane(pPlane); } void Atlas::AddMapDoor(Door *door) { Map *pMapMP = door->getMap(); + if (!IsMapReadyForEntities(pMapMP)) + return; pMapMP->AddMapDoor(door); } void Atlas::AddDetectedMapRoom(Room *room) { Map *pMapMP = room->getMap(); + if (!IsMapReadyForEntities(pMapMP)) + return; pMapMP->AddDetectedMapRoom(room); } void Atlas::AddCandidateMapRoom(Room *room) { Map *pMapMP = room->getMap(); + if (!IsMapReadyForEntities(pMapMP)) + return; pMapMP->AddCandidateMapRoom(room); } void Atlas::AddMapFloor(Floor *floor) { Map *pMapMP = floor->getMap(); + if (!IsMapReadyForEntities(pMapMP)) + return; pMapMP->AddMapFloor(floor); } diff --git a/core/src/Frame.cc b/core/src/Frame.cc index b42a364f..86548dd9 100644 --- a/core/src/Frame.cc +++ b/core/src/Frame.cc @@ -46,7 +46,7 @@ namespace ORB_SLAM3 // For stereo fisheye matching cv::BFMatcher Frame::BFmatcher = cv::BFMatcher(cv::NORM_HAMMING); - Frame::Frame() : mpcpi(NULL), mpImuPreintegrated(NULL), mpPrevFrame(NULL), mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbIsSet(false), mbImuPreintegrated(false), mbHasPose(false), mbHasVelocity(false) + Frame::Frame() : mpcpi(NULL), mpImuPreintegrated(NULL), mpPrevFrame(NULL), mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbIsSet(false), mbImuPreintegrated(false), mbHasPose(false), mbHasVelocity(false), mpMutexImu(new std::mutex()) { #ifdef REGISTER_TIMES mTimeStereoMatch = 0; @@ -109,7 +109,7 @@ namespace ORB_SLAM3 const std::vector markers) : mpcpi(NULL), mpORBvocabulary(voc), mpORBextractorLeft(extractorLeft), mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()), mK_(Converter::toMatrix3f(K)), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), mImuCalib(ImuCalib), mpImuPreintegrated(NULL), mpPrevFrame(pPrevF), mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbIsSet(false), mbImuPreintegrated(false), - mpCamera(pCamera), mpCamera2(nullptr), mbHasPose(false), mbHasVelocity(false) + mpCamera(pCamera), mpCamera2(nullptr), mbHasPose(false), mbHasVelocity(false), mpMutexImu(new std::mutex()) { // Setting the color image for Semantic Segmentation colorImg = imColor.clone(); @@ -130,8 +130,8 @@ namespace ORB_SLAM3 #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now(); #endif - thread threadLeft(&Frame::ExtractORB, this, 0, imLeft, 0, 0); - thread threadRight(&Frame::ExtractORB, this, 1, imRight, 0, 0); + thread threadLeft(&Frame::ExtractORB, this, 0, imLeft, 0, 0, cv::Mat()); + thread threadRight(&Frame::ExtractORB, this, 1, imRight, 0, 0, cv::Mat()); threadLeft.join(); threadRight.join(); #ifdef REGISTER_TIMES @@ -197,8 +197,6 @@ namespace ORB_SLAM3 mVw.setZero(); } - mpMutexImu = new std::mutex(); - // Set no stereo fisheye information Nleft = -1; Nright = -1; @@ -218,7 +216,7 @@ namespace ORB_SLAM3 Frame *pPrevF, const IMU::Calib &ImuCalib, const std::vector markers) : mpcpi(NULL), mpORBvocabulary(voc), mpORBextractorLeft(extractorLeft), mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()), mK_(Converter::toMatrix3f(K)), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), mImuCalib(ImuCalib), mpImuPreintegrated(NULL), mpPrevFrame(pPrevF), mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbImuPreintegrated(false), mpCamera(pCamera), mpCamera2(pCamera2), - mbHasPose(false), mbHasVelocity(false) + mbHasPose(false), mbHasVelocity(false), mpMutexImu(new std::mutex()) { imgLeft = imLeft.clone(); @@ -243,8 +241,8 @@ namespace ORB_SLAM3 #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now(); #endif - thread threadLeft(&Frame::ExtractORB, this, 0, imLeft, static_cast(mpCamera)->mvLappingArea[0], static_cast(mpCamera)->mvLappingArea[1]); - thread threadRight(&Frame::ExtractORB, this, 1, imRight, static_cast(mpCamera2)->mvLappingArea[0], static_cast(mpCamera2)->mvLappingArea[1]); + thread threadLeft(&Frame::ExtractORB, this, 0, imLeft, static_cast(mpCamera)->mvLappingArea[0], static_cast(mpCamera)->mvLappingArea[1], cv::Mat()); + thread threadRight(&Frame::ExtractORB, this, 1, imRight, static_cast(mpCamera2)->mvLappingArea[0], static_cast(mpCamera2)->mvLappingArea[1], cv::Mat()); threadLeft.join(); threadRight.join(); #ifdef REGISTER_TIMES @@ -309,8 +307,6 @@ namespace ORB_SLAM3 AssignFeaturesToGrid(); - mpMutexImu = new std::mutex(); - UndistortKeyPoints(); } @@ -324,7 +320,7 @@ namespace ORB_SLAM3 mTimeStamp(timeStamp), mK(K.clone()), mK_(Converter::toMatrix3f(K)), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), mImuCalib(ImuCalib), mpImuPreintegrated(NULL), mpPrevFrame(pPrevF), mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbIsSet(false), mbImuPreintegrated(false), - mpCamera(pCamera), mpCamera2(nullptr), mbHasPose(false), mbHasVelocity(false) + mpCamera(pCamera), mpCamera2(nullptr), mbHasPose(false), mbHasVelocity(false), mpMutexImu(new std::mutex()) { // Setting the color image for Semantic Segmentation colorImg = imColor.clone(); @@ -400,8 +396,6 @@ namespace ORB_SLAM3 else mVw.setZero(); - mpMutexImu = new std::mutex(); - // Set no stereo fisheye information Nleft = -1; Nright = -1; @@ -417,11 +411,11 @@ namespace ORB_SLAM3 // Monocular Frames Processing Frame::Frame(const cv::Mat &imColor, const cv::Mat &imGray, const double &timeStamp, ORBextractor *extractor, ORBVocabulary *voc, GeometricCamera *pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, Frame *pPrevF, - const IMU::Calib &ImuCalib, const std::vector markers) + const IMU::Calib &ImuCalib, const std::vector markers, const cv::Mat &mask) : mpcpi(NULL), mpORBvocabulary(voc), mpORBextractorLeft(extractor), mpORBextractorRight(static_cast(NULL)), mTimeStamp(timeStamp), mK(static_cast(pCamera)->toK()), mK_(static_cast(pCamera)->toK_()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), mImuCalib(ImuCalib), mpImuPreintegrated(NULL), mpPrevFrame(pPrevF), mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbIsSet(false), mbImuPreintegrated(false), mpCamera(pCamera), - mpCamera2(nullptr), mbHasPose(false), mbHasVelocity(false) + mpCamera2(nullptr), mbHasPose(false), mbHasVelocity(false), mpMutexImu(new std::mutex()) { // Setting the color image for Semantic Segmentation colorImg = imColor.clone(); @@ -442,7 +436,7 @@ namespace ORB_SLAM3 #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now(); #endif - ExtractORB(0, imGray, 0, 1000); + ExtractORB(0, imGray, 0, 1000, mask); #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now(); @@ -514,8 +508,6 @@ namespace ORB_SLAM3 { mVw.setZero(); } - - mpMutexImu = new std::mutex(); } void Frame::AssignFeaturesToGrid() @@ -552,14 +544,19 @@ namespace ORB_SLAM3 } } - void Frame::ExtractORB(int flag, const cv::Mat &imageGray, const int x0, const int x1) + void Frame::ExtractORB(int flag, const cv::Mat &imageGray, const int x0, const int x1, const cv::Mat &mask) { vector vLapping = {x0, x1}; // Compute ORB based on the flag (0: left, 1: right) if (flag == 0) - monoLeft = (*mpORBextractorLeft)(imageGray, cv::Mat(), mvKeys, mDescriptors, vLapping); + { + monoLeft = (*mpORBextractorLeft)(imageGray, mask, mvKeys, mDescriptors, vLapping); + FilterKeysAndDescriptorsByMask(mvKeys, mDescriptors, mask); + } else + { monoRight = (*mpORBextractorRight)(imageGray, cv::Mat(), mvKeysRight, mDescriptorsRight, vLapping); + } } bool Frame::isSet() const @@ -1312,4 +1309,40 @@ namespace ORB_SLAM3 return mRwc * mvStereo3Dpoints[i] + mOw; } + void Frame::FilterKeysAndDescriptorsByMask( + std::vector& keys, + cv::Mat& descriptors, + const cv::Mat& mask) + { + if (mask.empty()) + return; + + CV_Assert(mask.type() == CV_8UC1); + + std::vector keptKeys; + cv::Mat keptDescriptors; + + keptKeys.reserve(keys.size()); + + for (int i = 0; i < static_cast(keys.size()); ++i) + { + const int x = cvRound(keys[i].pt.x); + const int y = cvRound(keys[i].pt.y); + + if (x < 0 || x >= mask.cols || y < 0 || y >= mask.rows) + continue; + + if (mask.at(y, x) == 0) + continue; + + keptKeys.push_back(keys[i]); + + if (!descriptors.empty()) + keptDescriptors.push_back(descriptors.row(i)); + } + + keys.swap(keptKeys); + descriptors = keptDescriptors.clone(); + } + } // namespace ORB_SLAM diff --git a/core/src/GeometricSegmentation.cc b/core/src/GeometricSegmentation.cc index 63235805..03d423ad 100644 --- a/core/src/GeometricSegmentation.cc +++ b/core/src/GeometricSegmentation.cc @@ -130,6 +130,9 @@ namespace ORB_SLAM3 // the depth from points using Machine Learning to get a better plane estimate. pointcloud = getCloudFromSparsePoints(pKF->getCurrentFrameMapPoints()); // mCurrentFrame.mvpMapPoints + if (pointcloud == nullptr) + return extractedPlanes; + // Convert the pointcloud to one with PointXYZRGBA for consistency pcl::PointCloud::Ptr cloudRGBA(new pcl::PointCloud); pcl::copyPointCloud(*pointcloud, *cloudRGBA); @@ -183,4 +186,4 @@ namespace ORB_SLAM3 return Eigen::Vector4d(normal.x(), normal.y(), normal.z(), D); } -} \ No newline at end of file +} diff --git a/core/src/KeyFrame.cc b/core/src/KeyFrame.cc index 829f76f7..3dfdca8c 100644 --- a/core/src/KeyFrame.cc +++ b/core/src/KeyFrame.cc @@ -23,6 +23,9 @@ #include "KeyFrame.h" #include "Converter.h" #include "ImuTypes.h" +#include +#include +#include #include namespace ORB_SLAM3 @@ -208,13 +211,128 @@ namespace ORB_SLAM3 return mCurrentFramePointClouds; } + void KeyFrame::SetAuxPointCloudFromDepth(const cv::Mat &auxDepth, double auxDepthTimestamp, + const std::string &auxDepthFrameId, float auxDepthMin, + float auxDepthMax, int auxDepthStride, + const std::string &auxDepthScaleMode) + { + if (auxDepth.empty() || auxDepth.type() != CV_32FC1) + return; + + cv::Mat depth = auxDepth; + float depthScale = 1.0f; + + if (auxDepthScaleMode == "map_median") + { + std::vector mapDepths; + std::vector auxDepths; + const Sophus::SE3f Tcw = GetPose(); + + for (size_t i = 0; i < mCurrentFrameMapPoints.size() && i < mvKeysUn.size(); ++i) + { + MapPoint *pMP = mCurrentFrameMapPoints[i]; + if (!pMP || pMP->isBad()) + continue; + + const cv::KeyPoint &kp = mvKeysUn[i]; + const int u = cvRound(kp.pt.x); + const int v = cvRound(kp.pt.y); + if (u < 0 || v < 0 || u >= depth.cols || v >= depth.rows) + continue; + + const float aux = depth.at(v, u); + if (!std::isfinite(aux) || aux <= 1e-6f) + continue; + + const Eigen::Vector3f Pc = Tcw * pMP->GetWorldPos(); + if (Pc.z() <= auxDepthMin || Pc.z() >= auxDepthMax) + continue; + + mapDepths.push_back(Pc.z()); + auxDepths.push_back(aux); + } + + if (!mapDepths.empty() && mapDepths.size() == auxDepths.size()) + { + std::nth_element(mapDepths.begin(), mapDepths.begin() + mapDepths.size() / 2, mapDepths.end()); + std::nth_element(auxDepths.begin(), auxDepths.begin() + auxDepths.size() / 2, auxDepths.end()); + const float medMap = mapDepths[mapDepths.size() / 2]; + const float medAux = auxDepths[auxDepths.size() / 2]; + if (medAux > 1e-6f) + depthScale = medMap / medAux; + } + } + + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + cloud->width = depth.cols; + cloud->height = depth.rows; + cloud->is_dense = false; + cloud->points.resize(static_cast(cloud->width) * cloud->height); + + const int stride = std::max(1, auxDepthStride); + const float nan = std::numeric_limits::quiet_NaN(); + + for (int v = 0; v < depth.rows; ++v) + { + for (int u = 0; u < depth.cols; ++u) + { + pcl::PointXYZRGB &pt = cloud->at(u, v); + pt.x = nan; + pt.y = nan; + pt.z = nan; + + if ((u % stride) != 0 || (v % stride) != 0) + continue; + + const float z = depth.at(v, u) * depthScale; + if (!std::isfinite(z) || z < auxDepthMin || z > auxDepthMax) + continue; + + pt.x = (static_cast(u) - cx) * z * invfx; + pt.y = (static_cast(v) - cy) * z * invfy; + pt.z = z; + + if (!mImage.empty() && u < mImage.cols && v < mImage.rows) + { + const cv::Vec3b bgr = mImage.channels() == 3 ? mImage.at(v, u) : cv::Vec3b(0, 0, 0); + pt.r = bgr[2]; + pt.g = bgr[1]; + pt.b = bgr[0]; + } + } + } + + mAuxPointCloud = cloud; + mbHasAuxPointCloud = true; + mAuxDepthTimestamp = auxDepthTimestamp; + mAuxDepthFrameId = auxDepthFrameId; + + // std::cout << "AuxDepth: projected and copied pointcloud into keyframe ID " << mnId + // << " for segmented image, depth timestamp " + // << mAuxDepthTimestamp << std::endl; + } + + pcl::PointCloud::Ptr KeyFrame::getAuxPointCloud() + { + return mbHasAuxPointCloud ? mAuxPointCloud : nullptr; + } + void KeyFrame::clearPointCloud() { - mCurrentFramePointClouds->clear(); - mCurrentFramePointClouds = nullptr; + if (mCurrentFramePointClouds) + { + mCurrentFramePointClouds->clear(); + mCurrentFramePointClouds = nullptr; + } // clear images mImage.release(); + if (mAuxPointCloud) + { + mAuxPointCloud->clear(); + mAuxPointCloud = nullptr; + } + mbHasAuxPointCloud = false; } std::vector::Ptr> KeyFrame::getClsCloudPtrs() const diff --git a/core/src/LocalMapping.cc b/core/src/LocalMapping.cc index c13a731f..10a8f6aa 100644 --- a/core/src/LocalMapping.cc +++ b/core/src/LocalMapping.cc @@ -35,7 +35,7 @@ namespace ORB_SLAM3 LocalMapping::LocalMapping(System *pSys, Atlas *pAtlas, const float bMonocular, bool bInertial, const string &_strSeqName) : mpSystem(pSys), mbMonocular(bMonocular), mbInertial(bInertial), mbResetRequested(false), mbResetRequestedActiveMap(false), mbFinishRequested(false), mbFinished(true), mpAtlas(pAtlas), bInitializing(false), mbAbortBA(false), mbStopped(false), mbStopRequested(false), mbNotStop(false), mbAcceptKeyFrames(true), - mIdxInit(0), mScale(1.0), mInitSect(0), mbNotBA1(true), mbNotBA2(true), mIdxIteration(0), infoInertial(Eigen::MatrixXd::Zero(9, 9)) + mIdxInit(0), mScale(1.0), mInitSect(0), mbNotBA1(true), mbNotBA2(true), mIdxIteration(0), infoInertial(Eigen::MatrixXd::Zero(9, 9)), mpCurrentKeyFrame(nullptr) { mNumLM = 0; mTinit = 0.f; @@ -59,6 +59,11 @@ namespace ORB_SLAM3 mpTracker = pTracker; } + void LocalMapping::SetKeyFrameCreatedCallback(KeyFrameCreatedCallback callback) + { + mKeyFrameCreatedCallback = std::move(callback); + } + void LocalMapping::Run() { mbFinished = false; @@ -132,7 +137,17 @@ namespace ORB_SLAM3 mTinit += mpCurrentKeyFrame->mTimeStamp - mpCurrentKeyFrame->mPrevKF->mTimeStamp; if (!mpCurrentKeyFrame->GetMap()->GetIniertialBA2()) { - if ((mTinit < 10.f) && (dist < 0.02)) + // Guard: don't reset within the first 3s of map creation. A map that + // starts with the sensor stationary (e.g. before the platform begins + // moving) has near-zero motion by construction, but the map itself is + // still valid — resetting here just discards a good map and restarts + // the whole init process for no benefit. + float elapsedSinceMapCreation = 0.f; + const std::vector &vpOrigins = mpCurrentKeyFrame->GetMap()->mvpKeyFrameOrigins; + if (!vpOrigins.empty()) + elapsedSinceMapCreation = mpCurrentKeyFrame->mTimeStamp - vpOrigins.front()->mTimeStamp; + + if ((mTinit < 10.f) && (dist < 0.02) && (elapsedSinceMapCreation > 3.0f)) { std::cout << "[Mapping] Not enough motion for map initializing. Reseting..." << std::endl; unique_lock lock(mMutexReset); @@ -339,6 +354,9 @@ namespace ORB_SLAM3 // Insert Keyframe in Map mpAtlas->AddKeyFrame(mpCurrentKeyFrame); + + if (mKeyFrameCreatedCallback) + mKeyFrameCreatedCallback(mpCurrentKeyFrame); } void LocalMapping::EmptyQueue() @@ -1115,6 +1133,10 @@ namespace ORB_SLAM3 mbResetRequested = false; mbResetRequestedActiveMap = false; + // mpAtlas->clearAtlas()/clearMap() deletes the KeyFrame mpCurrentKeyFrame + // points to; leaving it set would dangle until the next ProcessNewKeyFrame(). + mpCurrentKeyFrame = nullptr; + // Inertial parameters mTinit = 0.f; mIdxInit = 0; @@ -1138,6 +1160,10 @@ namespace ORB_SLAM3 mbBadImu = false; mbResetRequested = false; mbResetRequestedActiveMap = false; + + // Same dangling-pointer hazard as the full-Atlas reset above, for the + // active-map-only reset path (mpAtlas->clearMap()). + mpCurrentKeyFrame = nullptr; } } } diff --git a/core/src/Map.cc b/core/src/Map.cc index d0fd6c53..a4caa5a5 100644 --- a/core/src/Map.cc +++ b/core/src/Map.cc @@ -21,6 +21,8 @@ */ #include "Map.h" +#include "Semantic/Door.h" +#include "Geometric/Plane.h" #include @@ -510,13 +512,97 @@ namespace ORB_SLAM3 for (set::iterator sit = mspMarkers.begin(); sit != mspMarkers.end(); sit++) { - // MapPoint *pMP = *sit; [TODO] - // pMP->SetWorldPos(s * Ryw * pMP->GetWorldPos() + tyw); - // pMP->UpdateNormalAndDepth(); + Marker *pMarker = *sit; + Sophus::SE3f pose = pMarker->getGlobalPose(); + pose.translation() *= s; + pMarker->setGlobalPose(Tyw * pose); } + + for (set::iterator sit = mspDoors.begin(); sit != mspDoors.end(); sit++) + { + // Only the cached global pose needs correcting -- localPose is keyframe-relative + Door *pDoor = *sit; + Sophus::SE3f pose = pDoor->getGlobalPose(); + pose.translation() *= s; + pDoor->setGlobalPose(Tyw * pose); + } + + // g2o::Plane3D only has a rigid (rotation + translation) transform built in, no scale, so + // the similarity version is hand-rolled here: the normal is unaffected by a uniform scale + // (rotation only), while distance' = s*distance + normal'.dot(t) -- derived from requiring + // that a point x' = s*R*x + t satisfying the old plane equation also satisfies the new one. + Eigen::Matrix3d RywD = Ryw.cast(); + Eigen::Vector3d tywD = tyw.cast(); + auto rescalePlaneEquation = [&](const g2o::Plane3D &plane) -> g2o::Plane3D + { + Eigen::Vector3d normal = RywD * plane.normal(); + double distance = static_cast(s) * plane.distance() + normal.dot(tywD); + Eigen::Vector4d coeffs; + coeffs.head<3>() = normal; + coeffs(3) = -distance; + return g2o::Plane3D(coeffs); + }; + auto rescalePlane = [&](Plane *pPlane) + { + if (!pPlane) + return; + + pPlane->setCentroid(s * Ryw * pPlane->getCentroid() + tyw); + pPlane->setGlobalEquation(rescalePlaneEquation(pPlane->getGlobalEquation())); + pPlane->mPlaneGBA = rescalePlaneEquation(pPlane->mPlaneGBA); + + pcl::PointCloud::Ptr cloud = pPlane->getMapClouds(); + if (cloud && !cloud->empty()) + { + pcl::PointCloud::Ptr newCloud(new pcl::PointCloud()); + newCloud->reserve(cloud->size()); + for (const auto &point : cloud->points) + { + Eigen::Vector3f pNew = s * Ryw * Eigen::Vector3f(point.x, point.y, point.z) + tyw; + pcl::PointXYZRGBA newPoint = point; + newPoint.x = pNew.x(); + newPoint.y = pNew.y(); + newPoint.z = pNew.z(); + newCloud->push_back(newPoint); + } + pPlane->replaceMapClouds(newCloud); + } + }; + + // Two disjoint containers hold Plane* objects -- mspPlanes doesn't include room-wall planes + for (set::iterator sit = mspPlanes.begin(); sit != mspPlanes.end(); sit++) + rescalePlane(*sit); + for (auto &kv : mRoomWallPlaneIndex) + rescalePlane(kv.second); + + for (set::iterator sit = mspDetectedRooms.begin(); sit != mspDetectedRooms.end(); sit++) + (*sit)->setCentroid(static_cast(s) * RywD * (*sit)->getCentroid() + tywD); + for (set::iterator sit = mspMarkerBasedRooms.begin(); sit != mspMarkerBasedRooms.end(); sit++) + (*sit)->setCentroid(static_cast(s) * RywD * (*sit)->getCentroid() + tywD); + + for (set::iterator sit = mspFloors.begin(); sit != mspFloors.end(); sit++) + (*sit)->setCentroid(static_cast(s) * RywD * (*sit)->getCentroid() + tywD); + + mLastScaleCorrectionT = Tyw; + mLastScaleCorrectionS = s; + mnScaleCorrectionVersion++; + mnMapChange++; } + void Map::GetLastScaleCorrection(Sophus::SE3f &T, float &s) + { + unique_lock lock(mMutexMap); + T = mLastScaleCorrectionT; + s = mLastScaleCorrectionS; + } + + uint64_t Map::GetScaleCorrectionVersion() + { + unique_lock lock(mMutexMap); + return mnScaleCorrectionVersion; + } + void Map::SetInertialSensor() { unique_lock lock(mMutexMap); diff --git a/core/src/Optimizer.cc b/core/src/Optimizer.cc index 604849b6..441bb5a3 100644 --- a/core/src/Optimizer.cc +++ b/core/src/Optimizer.cc @@ -6263,6 +6263,18 @@ namespace ORB_SLAM3 } } + // Gauss-Newton here runs undamped: when the outlier rounds reject (nearly) all + // visual edges, the remaining system can be singular and the dense solve diverges + // to NaN. Writing that state back would abort in Sophus (SO3 from a NaN rotation) + // and stamping mpcpi with it would poison the next frame's optimization — discard + // the result instead and report zero inliers so tracking treats this as a failure. + if (!VP->estimate().Rwb.allFinite() || !VP->estimate().twb.allFinite() || + !VV->estimate().allFinite() || !VG->estimate().allFinite() || !VA->estimate().allFinite()) + { + std::cout << "[Optimizer] PoseInertialOptimizationLastKeyFrame diverged (non-finite estimate), discarding result" << std::endl; + return 0; + } + // Recover optimized pose, velocity and biases pFrame->SetImuPoseVelocity(VP->estimate().Rwb.cast(), VP->estimate().twb.cast(), VV->estimate().cast()); Vector6d b; @@ -6660,6 +6672,17 @@ namespace ORB_SLAM3 nInliers = nInliersMono + nInliersStereo; + // Same undamped-GN divergence guard as in PoseInertialOptimizationLastKeyFrame: + // discard a non-finite solution instead of aborting in Sophus / poisoning mpcpi. + // Skipping the mpcpi stamp/delete below is safe: the TrackLocalMap dispatch only + // selects this variant when the previous frame's mpcpi exists. + if (!VP->estimate().Rwb.allFinite() || !VP->estimate().twb.allFinite() || + !VV->estimate().allFinite() || !VG->estimate().allFinite() || !VA->estimate().allFinite()) + { + std::cout << "[Optimizer] PoseInertialOptimizationLastFrame diverged (non-finite estimate), discarding result" << std::endl; + return 0; + } + // Recover optimized pose, velocity and biases pFrame->SetImuPoseVelocity(VP->estimate().Rwb.cast(), VP->estimate().twb.cast(), VV->estimate().cast()); Vector6d b; diff --git a/core/src/SemanticSegmentation.cc b/core/src/SemanticSegmentation.cc index 317eddd0..c3701e6c 100644 --- a/core/src/SemanticSegmentation.cc +++ b/core/src/SemanticSegmentation.cc @@ -49,11 +49,13 @@ namespace ORB_SLAM3 KeyFrame *thisKF = mpAtlas->GetKeyFrameById(std::get<0>(segImgTuple)); if (thisKF == nullptr || thisKF->isBad()) continue; - const pcl::PointCloud::Ptr thisKFPointCloud = thisKF->getCurrentFramePointCloud(); + pcl::PointCloud::Ptr thisKFPointCloud = thisKF->getCurrentFramePointCloud(); + if (thisKFPointCloud == nullptr && thisKF->mbHasAuxPointCloud) + thisKFPointCloud = thisKF->getAuxPointCloud(); if (thisKFPointCloud == nullptr) { std::cout << "SemSeg: skipping KF ID: " << thisKF->mnId << ". Missing pointcloud..." << std::endl; - exit(1); + // exit(1); continue; } @@ -305,4 +307,4 @@ namespace ORB_SLAM3 // cast a vote for the plane semantics matchedPlane->castWeightedVote(planeType, confidence); } -} \ No newline at end of file +} diff --git a/core/src/SemanticsManager.cc b/core/src/SemanticsManager.cc index 206332c1..7c4efc04 100644 --- a/core/src/SemanticsManager.cc +++ b/core/src/SemanticsManager.cc @@ -52,7 +52,8 @@ namespace ORB_SLAM3 // Check for possible room candidates if (sysParams->room_seg.method == SystemParams::room_seg::Method::FREE_SPACE) detectRoom_FreeSpaceCluster(); - else if (sysParams->room_seg.method == SystemParams::room_seg::Method::GNN) + else if (sysParams->room_seg.method == SystemParams::room_seg::Method::GNN || + sysParams->room_seg.method == SystemParams::room_seg::Method::GNN_NEW) detectRoom_GNN(); // Re-associate rooms based on walls and clusters @@ -653,4 +654,4 @@ namespace ORB_SLAM3 } } } -} \ No newline at end of file +} diff --git a/core/src/Settings.cc b/core/src/Settings.cc index 8efa18b6..0b1992da 100644 --- a/core/src/Settings.cc +++ b/core/src/Settings.cc @@ -479,6 +479,15 @@ namespace ORB_SLAM3 insertKFsWhenLost_ = (bool)readParameter(fSettings, "IMU.InsertKFsWhenLost", found, false); else insertKFsWhenLost_ = true; + + // When RECENTLY_LOST, attempt an IMU-guided visual relocalization instead of only + // coasting on IMU pose prediction until the timeout expires. Opt-in: off by default + // since it hasn't been tuned/validated for every sensor configuration. + readParameter(fSettings, "IMU.ImuAidedReloc", found, false); + if (found) + imuAidedReloc_ = (bool)readParameter(fSettings, "IMU.ImuAidedReloc", found, false); + else + imuAidedReloc_ = false; } void Settings::readRGBD(cv::FileStorage &fSettings) diff --git a/core/src/System.cc b/core/src/System.cc index bdaa7bd7..9c547c0c 100644 --- a/core/src/System.cc +++ b/core/src/System.cc @@ -258,6 +258,24 @@ namespace ORB_SLAM3 mpSemanticSegmentation->AddSegmentedFrameToBuffer(tuple); } + void System::AttachAuxDepthToKeyFrame(uint64_t keyFrameId, const cv::Mat &auxDepth, + double auxDepthTimestamp, const std::string &auxDepthFrameId, + float auxDepthMin, float auxDepthMax, int auxDepthStride, + const std::string &auxDepthScaleMode) + { + ORB_SLAM3::KeyFrame *pKF = mpAtlas->GetKeyFrameById(keyFrameId); + if (pKF == nullptr || pKF->isBad()) + { + // std::cout << "AuxDepth: keyframe ID " << keyFrameId + // << " not available for segmented pointcloud attachment" << std::endl; + return; + } + + pKF->SetAuxPointCloudFromDepth(auxDepth, auxDepthTimestamp, auxDepthFrameId, + auxDepthMin, auxDepthMax, auxDepthStride, + auxDepthScaleMode); + } + std::vector> System::getSkeletonCluster() { return mpAtlas->GetSkeletoClusterPoints(); @@ -353,8 +371,7 @@ namespace ORB_SLAM3 unique_lock lock2(mMutexState); mTrackingState = mpTracker->mState; - mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints; - mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn; + UpdateTrackedPointsFromCurrentFrame(); return Tcw; } @@ -429,13 +446,12 @@ namespace ORB_SLAM3 unique_lock lock2(mMutexState); mTrackingState = mpTracker->mState; - mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints; - mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn; + UpdateTrackedPointsFromCurrentFrame(); return Tcw; } Sophus::SE3f System::TrackMonocular(const cv::Mat &im, const double ×tamp, const vector &vImuMeas, - string filename, const std::vector markers) + string filename, const std::vector markers, const cv::Mat &mask) { // Multi-thread to prevent race conditions { @@ -453,11 +469,14 @@ namespace ORB_SLAM3 // Obtain the images cv::Mat imToFeed = im.clone(); + cv::Mat maskToFeed = mask.clone(); if (settings_ && settings_->needToResize()) { cv::Mat resizedImage; cv::resize(im, resizedImage, settings_->newImSize()); imToFeed = resizedImage; + if (!mask.empty()) + cv::resize(mask, maskToFeed, settings_->newImSize(), 0, 0, cv::INTER_NEAREST); } // Check mode change @@ -504,15 +523,38 @@ namespace ORB_SLAM3 for (size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++) mpTracker->GrabImuData(vImuMeas[i_imu]); - Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed, timestamp, filename, markers, envDoors, envRooms); + Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed, timestamp, filename, markers, envDoors, envRooms, maskToFeed); unique_lock lock2(mMutexState); mTrackingState = mpTracker->mState; - mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints; - mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn; + UpdateTrackedPointsFromCurrentFrame(); return Tcw; } + void System::UpdateTrackedPointsFromCurrentFrame() + { + const Frame &f = mpTracker->mCurrentFrame; + const size_t n = std::min(f.mvpMapPoints.size(), f.mvKeysUn.size()); + + mTrackedMapPoints.clear(); + mTrackedKeyPointsUn.clear(); + mTrackedMapPoints.reserve(n); + mTrackedKeyPointsUn.reserve(n); + + for (size_t i = 0; i < n; i++) + { + // mvbOutlier[i] is set by the tracker's own pose optimization (PoseOptimization / + // inertial variants) when this keypoint<->map-point association is rejected this + // frame; the map point pointer itself is left non-null (see Frame::mvbOutlier usage + // elsewhere, e.g. GetMapPointMatches), so it must be checked separately here. + if (i < f.mvbOutlier.size() && f.mvbOutlier[i]) + continue; + + mTrackedMapPoints.push_back(f.mvpMapPoints[i]); + mTrackedKeyPointsUn.push_back(f.mvKeysUn[i]); + } + } + void System::ActivateLocalizationMode() { unique_lock lock(mMutexMode); @@ -525,6 +567,12 @@ namespace ORB_SLAM3 mbDeactivateLocalizationMode = true; } + void System::SetKeyFrameCreatedCallback(std::function callback) + { + if (mpLocalMapper) + mpLocalMapper->SetKeyFrameCreatedCallback(std::move(callback)); + } + bool System::MapChanged() { static int n = 0; @@ -1093,6 +1141,11 @@ namespace ORB_SLAM3 return mpTracker->GetImuVwb(); } + IMU::Bias System::GetImuBias() + { + return mpTracker->GetImuBias(); + } + bool System::isImuPreintegrated() { return mpTracker->isImuPreintegrated(); @@ -1125,6 +1178,11 @@ namespace ORB_SLAM3 return (GetTimeFromIMUInit() > 0.1); } + int System::GetLocalMappingQueueSize() + { + return mpLocalMapper->KeyframesInQueue(); + } + void System::ChangeDataset() { if (mpAtlas->GetCurrentMap()->KeyFramesInMap() < 12) diff --git a/core/src/Tracking.cc b/core/src/Tracking.cc index 6ea666a1..fd305b87 100644 --- a/core/src/Tracking.cc +++ b/core/src/Tracking.cc @@ -665,6 +665,7 @@ namespace ORB_SLAM3 mImuFreq = settings->imuFrequency(); imuThresh = settings->imuThreshold(); mInsertKFsLost = settings->insertKFsWhenLost(); + mbImuAidedReloc = settings->imuAidedReloc(); mImuPer = 1.0 / static_cast(mImuFreq); float Ng = settings->noiseGyro(); float Na = settings->noiseAcc(); @@ -1636,7 +1637,7 @@ namespace ORB_SLAM3 Sophus::SE3f Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp, string filename, const std::vector markers, const std::vector doors, - const std::vector rooms) + const std::vector rooms, const cv::Mat &mask) { // Set arguments to local variables env_doors = doors; @@ -1662,21 +1663,21 @@ namespace ORB_SLAM3 { if (mState == NOT_INITIALIZED || mState == NO_IMAGES_YET || (lastID - initID) < mMaxFrames) mCurrentFrame = Frame(im, mImGray, timestamp, mpIniORBextractor, mpORBVocabulary, - mpCamera, mDistCoef, mbf, mThDepth, NULL, IMU::Calib(), markers); + mpCamera, mDistCoef, mbf, mThDepth, NULL, IMU::Calib(), markers, mask); else mCurrentFrame = Frame(im, mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary, mpCamera, - mDistCoef, mbf, mThDepth, NULL, IMU::Calib(), markers); + mDistCoef, mbf, mThDepth, NULL, IMU::Calib(), markers, mask); } else if (mSensor == System::IMU_MONOCULAR) { if (mState == NOT_INITIALIZED || mState == NO_IMAGES_YET) { mCurrentFrame = Frame(im, mImGray, timestamp, mpIniORBextractor, mpORBVocabulary, - mpCamera, mDistCoef, mbf, mThDepth, &mLastFrame, *mpImuCalib, markers); + mpCamera, mDistCoef, mbf, mThDepth, &mLastFrame, *mpImuCalib, markers, mask); } else mCurrentFrame = Frame(im, mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary, - mpCamera, mDistCoef, mbf, mThDepth, &mLastFrame, *mpImuCalib, markers); + mpCamera, mDistCoef, mbf, mThDepth, &mLastFrame, *mpImuCalib, markers, mask); } if (mState == NO_IMAGES_YET) @@ -2055,6 +2056,26 @@ namespace ORB_SLAM3 else bOK = false; + // Attempt visual relocalization guided by the IMU-predicted pose, + // instead of only coasting on PredictStateIMU() until the timeout + // below expires and the map gets reset/recreated. Only after 0.5s + // lost: shorter blips are recovered by TrackLocalMap on the + // IMU-predicted pose alone (vanilla behavior), and triggering a + // relocalization there needlessly activates the 30-frame + // post-reloc vision-only grace window for a self-healing hiccup. + if (mbImuAidedReloc && pCurrentMap->isImuInitialized() && + (mCurrentFrame.mTimeStamp - mTimeStampLost) >= 0.5 && + (mCurrentFrame.mnId - mnLastImuRelocFrameId) >= 5) + { + mnLastImuRelocFrameId = mCurrentFrame.mnId; + if (RelocalizationWithIMU()) + { + mState = OK; + bOK = true; + std::cout << "[Tracking][IMU-Reloc] Relocalized at frame " << mCurrentFrame.mnId << std::endl; + } + } + if (mCurrentFrame.mTimeStamp - mTimeStampLost > time_recently_lost) { mState = LOST; @@ -2241,6 +2262,29 @@ namespace ORB_SLAM3 { if (bOK) { + // Post-relocalization grace window: TrackLocalMap optimizes these frames + // vision-only, so their world velocity (mVw) is never re-estimated — it + // stays whatever dead-reckoning predicted at the moment of the loss, and + // propagates unchanged frame-to-frame via the Frame constructor. Handing + // that ~1s-stale velocity to the first inertial optimization after the + // window made its stiff inertial edge reject every visual match and + // diverge to NaN (confirmed via gdb on Marco_Dynamics). Re-seed each + // grace-window frame's velocity from the vision-tracked pose difference — + // the lightweight version of what the ResetFrameIMU() stub below was + // meant to do. + if (mnLastRelocFrameId > 0 && mCurrentFrame.mnId > mnLastRelocFrameId && + mCurrentFrame.mnId <= (mnLastRelocFrameId + mnFramesToResetIMU) && + mCurrentFrame.HasPose() && mLastFrame.HasPose()) + { + const double dt = mCurrentFrame.mTimeStamp - mLastFrame.mTimeStamp; + if (dt > 1e-4) + { + const Eigen::Vector3f vwb = + (mCurrentFrame.GetImuPosition() - mLastFrame.GetImuPosition()) / static_cast(dt); + mCurrentFrame.SetVelocity(vwb); + } + } + if (mCurrentFrame.mnId == (mnLastRelocFrameId + mnFramesToResetIMU)) { cout << "RESETING FRAME!!!" << endl; @@ -3052,7 +3096,14 @@ namespace ORB_SLAM3 else { // if(!mbMapUpdated && mState == OK) // && (mnMatchesInliers>30)) - if (!mbMapUpdated) // && (mnMatchesInliers>30)) + // The LastFrame variant builds an EdgePriorPoseImu from mpPrevFrame->mpcpi and + // dereferences it unconditionally. mpcpi is only stamped by these two inertial + // optimizers, so any frame that last went through vision-only PoseOptimization + // (each frame in the post-relocalization grace window above) has mpcpi == null, + // and using it as the previous frame crashes. Unreachable in vanilla inertial + // mapping (relocalization never fires there), reachable with IMU-aided reloc. + // The LastKeyFrame variant anchors on mpLastKeyFrame instead and needs no prior. + if (!mbMapUpdated && mCurrentFrame.mpPrevFrame && mCurrentFrame.mpPrevFrame->mpcpi) // && (mnMatchesInliers>30)) { Verbose::PrintMess("TLM: PoseInertialOptimizationLastFrame ", Verbose::VERBOSITY_DEBUG); inliers = Optimizer::PoseInertialOptimizationLastFrame(&mCurrentFrame); // , !mpLastKeyFrame->GetMap()->GetIniertialBA1()); @@ -3100,7 +3151,7 @@ namespace ORB_SLAM3 // Decide if the tracking was succesful // More restrictive if there was a relocalization recently mpLocalMapper->mnMatchesInliers = mnMatchesInliers; - if (mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames && mnMatchesInliers < 50) + if (mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames && mnMatchesInliers < 25) return false; if ((mnMatchesInliers > 10) && (mState == RECENTLY_LOST)) @@ -3108,7 +3159,7 @@ namespace ORB_SLAM3 if (mSensor == System::IMU_MONOCULAR) { - if ((mnMatchesInliers < 15 && mpAtlas->isImuInitialized()) || (mnMatchesInliers < 50 && !mpAtlas->isImuInitialized())) + if ((mnMatchesInliers < 15 && mpAtlas->isImuInitialized()) || (mnMatchesInliers < 25 && !mpAtlas->isImuInitialized())) { return false; } @@ -3687,6 +3738,236 @@ namespace ORB_SLAM3 } } + // Sanity-check a relocalization pose candidate against the IMU-predicted body state: + // rejects matches that are geometrically plausible (enough inliers) but land far from + // where the IMU says the sensor actually is, which would otherwise silently teleport + // the trajectory to an unrelated part of the map. + bool Tracking::VerifyRelocAgainstIMU(const Sophus::SE3f &Tcw_reloc) + { + Sophus::SE3f Tbc = mCurrentFrame.mImuCalib.mTbc; + Sophus::SE3f Twb_reloc = (Tcw_reloc * Tbc).inverse(); + + Eigen::Vector3f pos_reloc = Twb_reloc.translation(); + Eigen::Vector3f pos_imu = mCurrentFrame.GetImuPosition(); + + float timeSinceLost = (float)(mCurrentFrame.mTimeStamp - mTimeStampLost); + float maxPosDist = max(3.0f, 2.0f + 1.0f * timeSinceLost); + + float posDist = (pos_reloc - pos_imu).norm(); + if (posDist > maxPosDist) + { + cout << "[Tracking][IMU-Reloc] Rejected: pos dist " << posDist << "m > " << maxPosDist << "m" << endl; + return false; + } + + Eigen::Matrix3f R_reloc = Twb_reloc.rotationMatrix(); + Eigen::Matrix3f R_imu = mCurrentFrame.GetImuRotation(); + float traceVal = ((R_reloc.transpose() * R_imu).trace() - 1.0f) / 2.0f; + traceVal = min(1.0f, max(-1.0f, traceVal)); + float angleDiff = acos(traceVal); + if (angleDiff > 0.35f) + { + cout << "[Tracking][IMU-Reloc] Rejected: angle diff " << angleDiff * 180.0f / M_PI << " deg > 20 deg" << endl; + return false; + } + + return true; + } + + // Relocalization attempt used while RECENTLY_LOST on inertial sensors: instead of only + // coasting on IMU dead-reckoning (PredictStateIMU) until the timeout expires, actively + // search for a visual match, filtered and verified against the IMU-predicted pose so + // the platform can recover tracking without a map reset. + bool Tracking::RelocalizationWithIMU() + { + mCurrentFrame.ComputeBoW(); + + vector vpCandidateKFs = + mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame, mpAtlas->GetCurrentMap()); + + if (vpCandidateKFs.empty()) + return false; + + // Spatial filter: reject candidates far from the IMU-predicted position + Eigen::Vector3f imuPos = mCurrentFrame.GetCameraCenter(); + float timeSinceLost = (float)(mCurrentFrame.mTimeStamp - mTimeStampLost); + float maxRadius = 3.0f + 1.0f * timeSinceLost; + + vector> vDistKF; + for (KeyFrame *pKF : vpCandidateKFs) + { + if (!pKF || pKF->isBad()) + continue; + float dist = (pKF->GetCameraCenter() - imuPos).norm(); + vDistKF.push_back({dist, pKF}); + } + sort(vDistKF.begin(), vDistKF.end()); + + vector vpFilteredKFs; + for (auto &p : vDistKF) + { + if (p.first <= maxRadius) + vpFilteredKFs.push_back(p.second); + } + + // Fallback: keep top 3 closest if the spatial filter rejected everything + if (vpFilteredKFs.empty()) + { + for (int i = 0; i < min(3, (int)vDistKF.size()); i++) + vpFilteredKFs.push_back(vDistKF[i].second); + } + + cout << "[Tracking][IMU-Reloc] " << vpFilteredKFs.size() << "/" << vpCandidateKFs.size() + << " candidates within " << maxRadius << "m (t_lost=" << timeSinceLost << "s)" << endl; + + ORBmatcher matcher(0.9, true); + + // Step A: projection-based matching using the IMU-predicted pose + for (KeyFrame *pKF : vpFilteredKFs) + { + set sFound; + int nProj = matcher.SearchByProjection(mCurrentFrame, pKF, sFound, 15, 100); + + if (nProj < 20) + continue; + + int nGood = Optimizer::PoseOptimization(&mCurrentFrame); + + if (nGood < 10) + continue; + + for (int io = 0; io < mCurrentFrame.N; io++) + if (mCurrentFrame.mvbOutlier[io]) + mCurrentFrame.mvpMapPoints[io] = static_cast(NULL); + + // Progressive refinement + if (nGood < 50) + { + sFound.clear(); + for (int ip = 0; ip < mCurrentFrame.N; ip++) + if (mCurrentFrame.mvpMapPoints[ip]) + sFound.insert(mCurrentFrame.mvpMapPoints[ip]); + int nExtra = matcher.SearchByProjection(mCurrentFrame, pKF, sFound, 5, 64); + + if (nGood + nExtra >= 30) + { + nGood = Optimizer::PoseOptimization(&mCurrentFrame); + + for (int io = 0; io < mCurrentFrame.N; io++) + if (mCurrentFrame.mvbOutlier[io]) + mCurrentFrame.mvpMapPoints[io] = static_cast(NULL); + } + } + + if (nGood >= 30 && VerifyRelocAgainstIMU(mCurrentFrame.GetPose())) + { + mnLastRelocFrameId = mCurrentFrame.mnId; + cout << "[Tracking][IMU-Reloc] Projection success: " << nGood << " inliers with KF" << pKF->mnId << endl; + return true; + } + } + + // Step B: BoW fallback with MLPnP RANSAC + ORBmatcher matcherBoW(0.75, true); + const int nKFs = vpFilteredKFs.size(); + vector vpSolvers(nKFs, nullptr); + vector> vvpMatches(nKFs); + vector vbDiscarded(nKFs, false); + int nCandidates = 0; + + for (int i = 0; i < nKFs; i++) + { + KeyFrame *pKF = vpFilteredKFs[i]; + if (pKF->isBad()) + { + vbDiscarded[i] = true; + continue; + } + + int nmatches = matcherBoW.SearchByBoW(pKF, mCurrentFrame, vvpMatches[i]); + if (nmatches < 15) + { + vbDiscarded[i] = true; + continue; + } + + vpSolvers[i] = new MLPnPsolver(mCurrentFrame, vvpMatches[i]); + vpSolvers[i]->SetRansacParameters(0.99, 10, 300, 6, 0.5, 5.991); + nCandidates++; + } + + bool bMatch = false; + while (nCandidates > 0 && !bMatch) + { + for (int i = 0; i < nKFs; i++) + { + if (vbDiscarded[i]) + continue; + + vector vbInliers; + int nInliers; + bool bNoMore; + Eigen::Matrix4f eigTcw; + bool bTcw = vpSolvers[i]->iterate(5, bNoMore, vbInliers, nInliers, eigTcw); + + if (bNoMore) + { + vbDiscarded[i] = true; + nCandidates--; + } + + if (bTcw) + { + mCurrentFrame.SetPose(Sophus::SE3f(eigTcw)); + + set sFound; + const int np = vbInliers.size(); + for (int j = 0; j < np; j++) + { + if (vbInliers[j]) + { + mCurrentFrame.mvpMapPoints[j] = vvpMatches[i][j]; + sFound.insert(vvpMatches[i][j]); + } + else + mCurrentFrame.mvpMapPoints[j] = NULL; + } + + int nGood = Optimizer::PoseOptimization(&mCurrentFrame); + if (nGood < 10) + continue; + + for (int io = 0; io < mCurrentFrame.N; io++) + if (mCurrentFrame.mvbOutlier[io]) + mCurrentFrame.mvpMapPoints[io] = static_cast(NULL); + + if (nGood < 50) + { + int nadditional = matcher.SearchByProjection(mCurrentFrame, vpFilteredKFs[i], sFound, 10, 100); + if (nadditional + nGood >= 30) + { + nGood = Optimizer::PoseOptimization(&mCurrentFrame); + for (int io = 0; io < mCurrentFrame.N; io++) + if (mCurrentFrame.mvbOutlier[io]) + mCurrentFrame.mvpMapPoints[io] = NULL; + } + } + + if (nGood >= 30 && VerifyRelocAgainstIMU(mCurrentFrame.GetPose())) + { + bMatch = true; + mnLastRelocFrameId = mCurrentFrame.mnId; + cout << "[Tracking][IMU-Reloc] BoW success: " << nGood << " inliers with KF" + << vpFilteredKFs[i]->mnId << endl; + break; + } + } + } + } + + return bMatch; + } + bool Tracking::Relocalization() { Verbose::PrintMess("Starting relocalization", Verbose::VERBOSITY_NORMAL); @@ -3953,29 +4234,18 @@ namespace ORB_SLAM3 mbReadyToInitializate = false; - unsigned int index = mnFirstFrameId; - for (Map *pMap : mpAtlas->GetAllMaps()) - if (pMap->GetAllKeyFrames().size() > 0) - if (index > pMap->GetLowerKFID()) - index = pMap->GetLowerKFID(); - - // Count lost frames - std::list lbLost; - int lostFrameCount = 0; - for (list::iterator ilbL = mlbLost.begin(); ilbL != mlbLost.end(); ilbL++) - { - if (index < mnInitialFrameId) - lbLost.push_back(*ilbL); - else - { - lbLost.push_back(true); - lostFrameCount += 1; - } - index++; - } + // mpAtlas->clearMap() above deletes the KeyFrames of the map being reset, so any + // pointer to one of them in mlRelativeFramePoses/mlpReferences/mlFrameTimes/mlbLost + // is now dangling; walking those lists later (e.g. SaveTrajectoryTUM at shutdown) + // would be undefined behavior otherwise. Mirrors Tracking::Reset()'s unconditional + // clear of the same four lists. + const int lostFrameCount = static_cast(mlbLost.size()); std::cout << "[Tracking] " << lostFrameCount << " frames were set to lost!" << endl; - mlbLost = lbLost; + mlRelativeFramePoses.clear(); + mlpReferences.clear(); + mlFrameTimes.clear(); + mlbLost.clear(); mnInitialFrameId = mCurrentFrame.mnId; mnLastRelocFrameId = mCurrentFrame.mnId; @@ -4161,6 +4431,11 @@ namespace ORB_SLAM3 return mCurrentFrame.GetVelocity(); } + IMU::Bias Tracking::GetImuBias() + { + return mCurrentFrame.mImuBias; + } + bool Tracking::isImuPreintegrated() { return mCurrentFrame.mpImuPreintegrated; diff --git a/core/src/Types/SystemParams.cc b/core/src/Types/SystemParams.cc index 4b79d27e..0b6eeb5c 100644 --- a/core/src/Types/SystemParams.cc +++ b/core/src/Types/SystemParams.cc @@ -50,7 +50,7 @@ namespace ORB_SLAM3 try { // General Parameters - general.env_database = mConfig["general"]["env_database"].as(); + general.env_database = mConfig["general"]["env_database"].as(""); general.mode_of_operation = static_cast(mConfig["general"]["mode_of_operation"].as()); // Marker Parameters diff --git a/docker/Jazzy.Dockerfile b/docker/Jazzy.Dockerfile index 7aa138bb..9686bc8b 100644 --- a/docker/Jazzy.Dockerfile +++ b/docker/Jazzy.Dockerfile @@ -65,7 +65,7 @@ RUN groupadd --gid $USER_GID $USERNAME \ # --- Python environment setup --- RUN pip3 install networkx==3.1 -RUN pip3 install --extra-index-url https://download.pytorch.org/whl/cu121 \ +RUN pip3 install --extra-index-url https://download.pytorch.org/whl/cu126 \ torch \ torchvision RUN apt remove --purge python3-typing-extensions -y @@ -121,6 +121,7 @@ RUN --mount=type=ssh git clone git@github.com:snt-arg/visual_sgraphs.git RUN --mount=type=ssh git clone git@github.com:snt-arg/situational_graphs_msgs.git RUN --mount=type=ssh git clone -b ros2-jazzy git@github.com:snt-arg/scene_segment_ros.git RUN --mount=type=ssh git clone -b ros2-master git@github.com:IntelRealSense/realsense-ros.git +RUN --mount=type=ssh git clone git@github.com:snt-arg/object_tracker_3d_ros.git # RUN --mount=type=ssh git clone -b humble-devel git@github.com:pal-robotics/aruco_ros.git # Repositories for GNN-based room detection and reasoning @@ -133,6 +134,10 @@ RUN --mount=type=ssh git clone -b ros2-master git@github.com:IntelRealSense/real WORKDIR /home/$USERNAME/workspace/src/visual_sgraphs/docker RUN pip3 install --break-system-packages --ignore-installed -r requirements.txt +# Install object tracker dependencies +RUN pip3 install --break-system-packages --ignore-installed \ + -r /home/$USERNAME/workspace/src/object_tracker_3d_ros/requirements.txt + # [Hint] Temp. fix for installing ROS2 Humble repositories (GNN-based room detection) in Jazzy # (Read more: https://github.com/ros2/ros2/issues/1702) # RUN pip3 install --break-system-packages setuptools==79.0.1 @@ -161,7 +166,8 @@ RUN apt-get update && apt-get install -y \ # Build the workspace WORKDIR /home/$USERNAME/workspace/ RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && rosdep install --from-paths src --ignore-src -r -y" -RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release" +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-skip realsense2_ros_mqtt_bridge" +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --packages-select realsense2_ros_mqtt_bridge" # --- Miscalleanous --- RUN ldconfig @@ -211,4 +217,4 @@ RUN echo "alias mprocs='mprocs -c /home/$USERNAME/workspace/src/visual_sgraphs/c ENTRYPOINT ["/entrypoint.sh"] USER $USERNAME CMD ["/bin/bash"] -SHELL ["/bin/bash"] \ No newline at end of file +SHELL ["/bin/bash"] diff --git a/docker/Jazzy_new.Dockerfile b/docker/Jazzy_new.Dockerfile new file mode 100644 index 00000000..d7acf026 --- /dev/null +++ b/docker/Jazzy_new.Dockerfile @@ -0,0 +1,278 @@ +FROM nvcr.io/nvidia/cuda-dl-base:25.04-cuda12.9-devel-ubuntu24.04 + +# Arguments +ARG USERNAME=user +ARG USER_UID=1000 +ARG USER_GID=$USER_UID +ARG DEBIAN_FRONTEND=noninteractive + +# Environment variables +ENV CUDA_HOME=/usr/local/cuda \ + LANG=en_US.UTF-8 \ + LC_ALL=en_US.UTF-8 \ + ROS_DISTRO=jazzy \ + PIP_BREAK_SYSTEM_PACKAGES=1 + +ENV PATH="/usr/src/tensorrt/bin:${PATH}" + +# --- Fix MPI issue --- +RUN mkdir -p /opt/hpcx/ompi/lib/x86_64-linux-gnu \ + && ln -s /opt/hpcx/ompi /opt/hpcx/ompi/lib/x86_64-linux-gnu/openmpi \ + && dpkg-reconfigure libc-bin + +# --- Handle user creation --- +RUN if id -u $USER_UID ; then userdel "$(id -un $USER_UID)" ; fi + +# --- System setup --- +RUN apt-get update && apt-get install -y --no-install-recommends \ + python3-pip \ + python-is-python3 \ + git \ + openssh-client \ + wget \ + vim \ + curl \ + libeigen3-dev \ + build-essential \ + locales \ + software-properties-common \ + lsb-release \ + gnupg2 && \ + locale-gen en_US en_US.UTF-8 && \ + update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 +RUN add-apt-repository universe + +# --- ROS 2 Jazzy APT source setup --- +RUN ROS_APT_SOURCE_VERSION=$(curl -s https://api.github.com/repos/ros-infrastructure/ros-apt-source/releases/latest | grep -F "tag_name" | awk -F\" '{print $4}') && \ + curl -L -o /tmp/ros2-apt-source.deb "https://github.com/ros-infrastructure/ros-apt-source/releases/download/${ROS_APT_SOURCE_VERSION}/ros2-apt-source_${ROS_APT_SOURCE_VERSION}.$(. /etc/os-release && echo $VERSION_CODENAME)_all.deb" && \ + apt install -y /tmp/ros2-apt-source.deb + +# --- Install ROS 2 Jazzy development tools --- +RUN apt update && apt upgrade -y && \ + apt install -y ros-dev-tools ros-${ROS_DISTRO}-desktop ros-${ROS_DISTRO}-rqt-tf-tree + +# --- Source ROS globally --- +RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /etc/bash.bashrc + +# --- Initialize rosdep --- +RUN rosdep init && rosdep update + +# --- Clean up --- +RUN rm -rf /var/lib/apt/lists/* /tmp/* + +# --- Create user --- +RUN groupadd --gid $USER_GID $USERNAME \ + && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \ + && echo "$USERNAME ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers + +# --- Python environment setup --- +RUN pip3 install networkx==3.1 +RUN pip3 uninstall -y torch torchvision torchaudio || true +RUN pip3 install --extra-index-url https://download.pytorch.org/whl/cu126 \ + torch==2.6.0+cu126 \ + torchvision==0.21.0+cu126 +RUN apt remove --purge python3-typing-extensions -y +RUN pip3 install typing-extensions==4.11.0 + +# --- CLIP and Detectron2 setup --- +ARG TORCH_CUDA_ARCH_LIST="7.5;7.0+PTX" +ENV FORCE_CUDA="1" +RUN pip3 install 'git+https://github.com/facebookresearch/detectron2.git' +RUN pip3 install 'git+https://github.com/openai/CLIP.git' + + +# --- SSH keys --- +# Define the SSH keys as build arguments for latter mounting +RUN mkdir -p -m 0600 ~/.ssh && ssh-keyscan github.com >> ~/.ssh/known_hosts + +# --- Clone repositories --- +RUN apt-get update && apt-get install -y \ + libepoxy-dev \ + libgl1-mesa-dev \ + libglu1-mesa-dev \ + freeglut3-dev \ + libglew-dev \ + cmake \ + build-essential \ + git + +# Pangolin +WORKDIR /opt/ +RUN git clone --branch v0.9.1 --depth 1 https://github.com/stevenlovegrove/Pangolin.git && \ + cd Pangolin && \ + mkdir build && cd build && \ + cmake .. && \ + make -j && \ + make install + +# Cmake +ARG version=3.22 +ARG build=1 +WORKDIR /tmp +RUN wget https://cmake.org/files/v$version/cmake-$version.$build.tar.gz + +RUN tar -xzvf cmake-$version.$build.tar.gz +WORKDIR /tmp/cmake-$version.$build +RUN ./bootstrap +RUN make -j8 +RUN make install + +WORKDIR /home/$USERNAME/workspace/src + +# Mount the SSH keys and clone the vS-Graphs repositories +RUN --mount=type=ssh git clone git@github.com:snt-arg/visual_sgraphs.git +RUN --mount=type=ssh git clone git@github.com:snt-arg/situational_graphs_msgs.git +RUN --mount=type=ssh git clone -b ros2-jazzy git@github.com:snt-arg/scene_segment_ros.git +RUN --mount=type=ssh git clone -b r/4.57.2 --depth 1 git@github.com:IntelRealSense/realsense-ros.git +# RUN --mount=type=ssh git clone -b humble-devel git@github.com:pal-robotics/aruco_ros.git + +# Repositories for GNN-based room detection and reasoning +# RUN --mount=type=ssh git clone -b develop git@github.com:snt-arg/situational_graphs_wrapper.git +# RUN --mount=type=ssh git clone -b develop git@github.com:snt-arg/situational_graphs_datasets.git +# RUN --mount=type=ssh git clone -b develop git@github.com:snt-arg/situational_graphs_reasoning.git +# RUN --mount=type=ssh git clone -b main git@github.com:snt-arg/situational_graphs_reasoning_msgs.git + +# Install the vS-Graphs dependencies +WORKDIR /home/$USERNAME/workspace/src/visual_sgraphs/docker +RUN pip3 install --break-system-packages --ignore-installed -r requirements.txt + +# Install object tracker dependencies when the package is present in the workspace +RUN if [ -f /home/$USERNAME/workspace/src/object_tracker_3d_ros/requirements.txt ]; then \ + pip3 install --break-system-packages --ignore-installed \ + -r /home/$USERNAME/workspace/src/object_tracker_3d_ros/requirements.txt ; \ + fi + +# [Hint] Temp. fix for installing ROS2 Humble repositories (GNN-based room detection) in Jazzy +# (Read more: https://github.com/ros2/ros2/issues/1702) +# RUN pip3 install --break-system-packages setuptools==79.0.1 + +# Install reasoning dependencies +# RUN pip3 install --break-system-packages shapely==2.1.1 torch-geometric==2.6.1 transforms3d==0.4.2 +# RUN mkdir -p /home/$USERNAME/workspace/install/situational_graphs_reasoning/share/situational_graphs_reasoning/reports \ +# && chown -R $USERNAME:$USERNAME /home/$USERNAME/workspace/install/situational_graphs_reasoning/share/situational_graphs_reasoning/reports + +# Install the EOMT dependencies +WORKDIR /home/$USERNAME/workspace/src/scene_segment_ros/src/ +RUN git clone https://github.com/tue-mps/eomt.git +RUN pip3 install --ignore-installed -r /home/${USERNAME}/workspace/src/scene_segment_ros/src/eomt/requirements.txt +RUN pip3 install "numpy<2.0" --force-reinstall + +# WORKDIR /home/$USERNAME/workspace/src/ + +# Download the yoso checkpoint +RUN wget https://github.com/hujiecpp/YOSO/releases/download/v0.1/yoso_res50_coco.pth +RUN mv yoso_res50_coco.pth /home/$USERNAME/workspace/src/scene_segment_ros/include/ + +# USER root +ENV DEBIAN_FRONTEND=noninteractive +RUN apt-get update && apt-get install -y \ + ros-${ROS_DISTRO}-rviz-visual-tools \ + ros-${ROS_DISTRO}-depth-image-proc \ + ros-${ROS_DISTRO}-backward-ros \ + ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \ + ros-${ROS_DISTRO}-diagnostic-updater \ + ros-${ROS_DISTRO}-pcl-ros + +# Build the workspace +WORKDIR /home/$USERNAME/workspace/ +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && rosdep install --from-paths src --ignore-src -r -y" +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-skip realsense2_ros_mqtt_bridge" +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --packages-select realsense2_ros_mqtt_bridge" + +# --- Miscalleanous --- +RUN ldconfig + +# --- Clean up --- +# Remove the apt list files +RUN rm -rf /var/lib/apt/lists/* + +# Remove packages no longer needed +RUN apt-get clean && apt-get autoremove -y + +# Remove the ssh keys +RUN rm -rf /root/.ssh/ + +# --- Build entrypoint --- +RUN echo "#!/bin/bash" >> /entrypoint.sh \ + && echo "echo \"source /opt/ros/$ROS_DISTRO/setup.bash\" >> ~/.bashrc" >> /entrypoint.sh \ + && echo "echo \"source /home/$USERNAME/workspace/install/setup.bash\" >> ~/.bashrc" >> /entrypoint.sh \ + && echo 'exec "$@"' >> /entrypoint.sh \ + && chmod a+x /entrypoint.sh + +# ------------------------------------ +# Download Vox2Ros Toolkit for Voxblox +# ------------------------------------ +WORKDIR /home/$USERNAME/workspace/vsgraphs_tools +RUN curl -L https://raw.githubusercontent.com/snt-arg/vsgraphs_tools/refs/heads/main/Voxblox/relay_jazzy.py -o /home/$USERNAME/workspace/vsgraphs_tools/relay_jazzy.py +RUN chmod +x /home/$USERNAME/workspace/vsgraphs_tools/relay_jazzy.py + +USER $USERNAME +RUN sudo chown -R $USERNAME:$USERNAME /home/$USERNAME/workspace +WORKDIR /home/$USERNAME/workspace/ + +# --------------------------- +# Download and Install mprocs +# --------------------------- +RUN curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | bash -s -- -y \ + && . "$HOME/.cargo/env" \ + && cargo install mprocs + + +# note: you might have to reinstall numpy and opencv python pkgs and rebuilding cause the system packages are pointed instead +# note: torch version might be wrong (cu130 wheel), reinstall torch and torchvision with the correct versions if you encounter issues related to torch. +RUN sudo rm -rf \ + /usr/local/lib/python3.12/dist-packages/torch \ + /usr/local/lib/python3.12/dist-packages/torch-* \ + /usr/local/lib/python3.12/dist-packages/torchvision \ + /usr/local/lib/python3.12/dist-packages/torchvision-* \ + /usr/local/lib/python3.12/dist-packages/torchaudio \ + /usr/local/lib/python3.12/dist-packages/torchaudio-* \ + /usr/local/lib/python3.12/dist-packages/functorch \ + /usr/local/lib/python3.12/dist-packages/torchgen + +RUN python3 -m pip install --user --break-system-packages --no-cache-dir --force-reinstall \ + torch torchvision torchaudio \ + --index-url https://download.pytorch.org/whl/cu126 + +# Purge numpy/scipy from BOTH the user site and the system dist-packages before +# reinstalling. Earlier layers install numpy/scipy system-wide with +# `--ignore-installed`, which does not clean up files from a differently-laid-out +# previous version. If those stale files are left in dist-packages, they can +# shadow the pinned --user install below (e.g. a stray flat +# scipy/sparse/linalg/_propack*.so shadowing the real _propack/ package), +# breaking `import scipy` at runtime even though `pip list` looks correct. +RUN rm -rf ~/.local/lib/python3.12/site-packages/numpy \ + ~/.local/lib/python3.12/site-packages/numpy-*.dist-info \ + ~/.local/lib/python3.12/site-packages/numpy.libs \ + ~/.local/lib/python3.12/site-packages/scipy \ + ~/.local/lib/python3.12/site-packages/scipy-*.dist-info \ + ~/.local/lib/python3.12/site-packages/scipy.libs +RUN sudo rm -rf \ + /usr/local/lib/python3.12/dist-packages/numpy \ + /usr/local/lib/python3.12/dist-packages/numpy-* \ + /usr/local/lib/python3.12/dist-packages/numpy.libs \ + /usr/local/lib/python3.12/dist-packages/scipy \ + /usr/local/lib/python3.12/dist-packages/scipy-* \ + /usr/local/lib/python3.12/dist-packages/scipy.libs + +RUN python3 -m pip install --user --break-system-packages --no-cache-dir "numpy==1.26.4" +RUN python3 -m pip install --user --break-system-packages --no-cache-dir --force-reinstall --no-deps \ + "scipy==1.13.1" +RUN python3 -m pip install --user --break-system-packages --no-cache-dir --force-reinstall --no-deps \ + "ultralytics==8.4.68" + +WORKDIR /home/$USERNAME/workspace/ +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-skip realsense2_ros_mqtt_bridge" +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --packages-select realsense2_ros_mqtt_bridge" + +# -------------------------- +# Aliases and Environment Setup +# -------------------------- +RUN echo "alias mprocs='mprocs -c /home/$USERNAME/workspace/src/visual_sgraphs/config/mprocs.yml'" >> ~/.bashrc && \ + echo "alias rel_vox='python /home/$USERNAME/workspace/vsgraphs_tools/relay_jazzy.py --mode voxblox_client'" >> ~/.bashrc && \ + echo "alias rel_pcl='python /home/$USERNAME/workspace/vsgraphs_tools/relay_jazzy.py --mode pc_server'" >> ~/.bashrc + +ENTRYPOINT ["/entrypoint.sh"] +USER $USERNAME +CMD ["/bin/bash"] +SHELL ["/bin/bash"] diff --git a/docker/README.md b/docker/README.md index 09afaf96..f769ccbc 100644 --- a/docker/README.md +++ b/docker/README.md @@ -63,4 +63,37 @@ docker exec -it vsgraphs_ros2 bash ros2 launch vs_graphs rgbd.launch.py ``` -You can even use the **mprocs** tool provided inside the Docker image, by simply running `mprocs` (an alias for `mprocs -c [path]/visual_sgraphs/config/mprocs.yml`) and choose the prepared command sets there. \ No newline at end of file +You can even use the **mprocs** tool provided inside the Docker image, by simply running `mprocs` (an alias for `mprocs -c [path]/visual_sgraphs/config/mprocs.yml`) and choose the prepared command sets there. + +### Solve cuda mismatch after build +Remove wrong versions and Reinstall torch / numpy: +```bash +sudo rm -rf \ + /usr/local/lib/python3.12/dist-packages/torch \ + /usr/local/lib/python3.12/dist-packages/torch-* \ + /usr/local/lib/python3.12/dist-packages/torchvision \ + /usr/local/lib/python3.12/dist-packages/torchvision-* \ + /usr/local/lib/python3.12/dist-packages/torchaudio \ + /usr/local/lib/python3.12/dist-packages/torchaudio-* \ + /usr/local/lib/python3.12/dist-packages/functorch \ + /usr/local/lib/python3.12/dist-packages/torchgen + +python3 -m pip install --user --break-system-packages --no-cache-dir --force-reinstall \ + torch torchvision torchaudio \ + --index-url https://download.pytorch.org/whl/cu126 + +rm -rf ~/.local/lib/python3.12/site-packages/numpy \ + ~/.local/lib/python3.12/site-packages/numpy-*.dist-info \ + ~/.local/lib/python3.12/site-packages/numpy.libs + +python3 -m pip install --user --break-system-packages --no-cache-dir "numpy==1.26.4" +``` +verify: +```bash +python3 - <<'PY' +import torch, numpy +print("torch:", torch.__version__, torch.__file__, torch.version.cuda, torch.cuda.is_available()) +print("numpy:", numpy.__version__, numpy.__file__) +PY +``` +and build the workspace. \ No newline at end of file diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index 74429dcc..f4719e16 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -3,7 +3,7 @@ services: image: vsgraphs_ros2 build: context: . - dockerfile: Jazzy.Dockerfile + dockerfile: Jazzy_new.Dockerfile ssh: - default args: @@ -14,7 +14,7 @@ services: environment: DISPLAY: $DISPLAY XAUTHORITY: $XAUTHORITY - SSH_AUTH_SOCK: $SSH_AUTH_SOCK + SSH_AUTH_SOCK: /ssh-agent NVIDIA_VISIBLE_DEVICES: all NVIDIA_DRIVER_CAPABILITIES: all TERM: $TERM @@ -30,7 +30,7 @@ services: # - ~/.Xauthority:/home/user/.Xauthority:rw - $XAUTHORITY:$XAUTHORITY:rw # SSH - - $SSH_AUTH_SOCK:$SSH_AUTH_SOCK + - $SSH_AUTH_SOCK:/ssh-agent # TMUX - ~/.tmux:/home/$USERNAME/.tmux:ro - ~/.tmux.conf:/home/$USERNAME/.tmux.conf:ro @@ -42,13 +42,27 @@ services: # - ~/.bash_history:/root/.bash_history # - container_home_cache:/root/.cache # Optional: persist cache # Working Directory - vS-Graphs (** MODIFY **) - - "/home/$USERNAME/[PATH]/src/visual_sgraphs:/home/$USERNAME/workspace/src/visual_sgraphs:rw" + - "/home/$USERNAME/workspaces/dv_sgraphs/src/visual_sgraphs:/home/$USERNAME/workspace/src/visual_sgraphs:rw" # Working Directory - Scene Segmenter (** MODIFY **) - - "/home/$USERNAME/[PATH]/src/scene_segment_ros:/home/$USERNAME/workspace/src/scene_segment_ros:rw" + - "/home/$USERNAME/workspaces/dv_sgraphs/src/scene_segment_ros:/home/$USERNAME/workspace/src/scene_segment_ros:rw" # Working Directory - Situational Graphs Messages (** MODIFY **) - - "/home/$USERNAME/[PATH]/src/situational_graphs_msgs:/home/$USERNAME/workspace/src/situational_graphs_msgs:rw" + - "/home/$USERNAME/workspaces/dv_sgraphs/src/situational_graphs_msgs:/home/$USERNAME/workspace/src/situational_graphs_msgs:rw" + # Working Directory - 3D object tracker (** MODIFY **) + # - "/home/$USERNAME/workspaces/dv_sgraphs/src/object_tracker_3d_ros:/home/$USERNAME/workspace/src/object_tracker_3d_ros:rw" + # Working Directory - Dynamic keypoint tracker (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/dynamic_keypoint_tracker:/home/$USERNAME/workspace/src/dynamic_keypoint_tracker:rw" + # Working Directory - Keyframe depth estimator (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/keyframe_depth_estimator:/home/$USERNAME/workspace/src/keyframe_depth_estimator:rw" + # Working Directory - Keyframe depth validator (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/keyframe_depth_validator:/home/$USERNAME/workspace/src/keyframe_depth_validator:rw" + # Working Directory - Dynamic keypoint 3D lifter (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/dynamic_keypoint_3d_lifter:/home/$USERNAME/workspace/src/dynamic_keypoint_3d_lifter:rw" + # Working Directory - Object motion estimator (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/object_motion_estimator:/home/$USERNAME/workspace/src/object_motion_estimator:rw" + # Working Directory - ORB-SLAM3 bag runner (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/orbslam3_bag_runner:/home/$USERNAME/workspace/src/orbslam3_bag_runner:rw" # Data Directory (** MODIFY **) - - "/media/$USERNAME/AT_SSD/Datasets:/home/$USERNAME/datasets:rw" + - "/media/$USERNAME/muffin/datasets:/home/$USERNAME/datasets:rw" network_mode: "host" command: tail -f /dev/null @@ -57,13 +71,27 @@ services: - volume-init volume-init: image: busybox - command: ["chown", "-R", "1000:1000", "/workspace/src/visual_sgraphs"] + command: ["chown", "-R", "1000:1000", "/home/$USERNAME/workspace/src/visual_sgraphs"] volumes: # Working Directory - vS-Graphs (** MODIFY **) - - "/home/$USERNAME/[PATH]/src/visual_sgraphs:/home/$USERNAME/workspace/src/visual_sgraphs:rw" + - "/home/$USERNAME/workspaces/dv_sgraphs/src/visual_sgraphs:/home/$USERNAME/workspace/src/visual_sgraphs:rw" # Working Directory - Scene Segmenter (** MODIFY **) - - "/home/$USERNAME/[PATH]/src/scene_segment_ros:/home/$USERNAME/workspace/src/scene_segment_ros:rw" + - "/home/$USERNAME/workspaces/dv_sgraphs/src/scene_segment_ros:/home/$USERNAME/workspace/src/scene_segment_ros:rw" # Working Directory - Situational Graphs Messages (** MODIFY **) - - "/home/$USERNAME/[PATH]/src/situational_graphs_msgs:/home/$USERNAME/workspace/src/situational_graphs_msgs:rw" + - "/home/$USERNAME/workspaces/dv_sgraphs/src/situational_graphs_msgs:/home/$USERNAME/workspace/src/situational_graphs_msgs:rw" + # Working Directory - 3D object tracker (** MODIFY **) + # - "/home/$USERNAME/workspaces/dv_sgraphs/src/object_tracker_3d_ros:/home/$USERNAME/workspace/src/object_tracker_3d_ros:rw" + # Working Directory - Dynamic keypoint tracker (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/dynamic_keypoint_tracker:/home/$USERNAME/workspace/src/dynamic_keypoint_tracker:rw" + # Working Directory - Keyframe depth estimator (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/keyframe_depth_estimator:/home/$USERNAME/workspace/src/keyframe_depth_estimator:rw" + # Working Directory - Keyframe depth validator (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/keyframe_depth_validator:/home/$USERNAME/workspace/src/keyframe_depth_validator:rw" + # Working Directory - Dynamic keypoint 3D lifter (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/dynamic_keypoint_3d_lifter:/home/$USERNAME/workspace/src/dynamic_keypoint_3d_lifter:rw" + # Working Directory - Object motion estimator (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/object_motion_estimator:/home/$USERNAME/workspace/src/object_motion_estimator:rw" + # Working Directory - ORB-SLAM3 bag runner (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/orbslam3_bag_runner:/home/$USERNAME/workspace/src/orbslam3_bag_runner:rw" entrypoint: "" restart: "no" diff --git a/docker/requirements.txt b/docker/requirements.txt index 108b326a..89716ffb 100644 --- a/docker/requirements.txt +++ b/docker/requirements.txt @@ -1,11 +1,11 @@ networkx==3.1 -ultralytics == 8.0.120 +ultralytics==8.4.68 matplotlib>=3.2.2 opencv-python==4.6.0.66 Pillow>=7.1.2 PyYAML>=5.3.1 requests>=2.23.0 -scipy>=1.4.1 +scipy==1.13.1 tqdm>=4.64.0 pandas>=1.1.4 seaborn>=0.11.0 @@ -15,4 +15,4 @@ transformers ftfy regex timm -evo \ No newline at end of file +evo diff --git a/evaluation/evaluate_uhumans2.py b/evaluation/evaluate_uhumans2.py new file mode 100644 index 00000000..40e03f15 --- /dev/null +++ b/evaluation/evaluate_uhumans2.py @@ -0,0 +1,546 @@ +#!/usr/bin/env python3 + +""" +Evaluation node for uHumans2-style datasets. + +Compares the live SLAM output of a mono-inertial vS-Graphs run against the +ground truth published on /tf by the dataset itself: + + - Camera platform: /tf world -> base_link_gt (ground truth) + /vs_graphs/body_odom (estimate, world -> imu) + - Humans: /tf world -> object_ (ground truth, 0..N humans) + /dynamic_object_points_3d (estimate, per-keyframe 3D points) + +Camera and human estimates share the same SLAM-internal "world" frame, so a +single rigid (rotation + translation, no scale) alignment is computed from the +camera trajectory and applied to both before computing errors. Scale is not +applied (mono-inertial SLAM should already be metric); the recovered scale is +reported only as a diagnostic to sanity-check IMU scale correctness. + +Human-to-ground-truth association is solved independently at every keyframe +via Hungarian assignment (minimum total distance) with a distance gate, since +estimated track ids are not guaranteed to match ground-truth human ids and the +number of humans visible at any instant may be 0, 1, or many. + +Usage (after sourcing the workspace and while a run is playing back): + python3 evaluate_uhumans2.py --ros-args \ + -p output_dir:=/path/to/results \ + -p body_odom_topic:=/vs_graphs/body_odom \ + -p human_points_topic:=/dynamic_object_points_3d + +The node buffers samples as they arrive and finalizes automatically once no +new messages have been seen for `idle_timeout_s` seconds (i.e. the bag/run has +ended), writing three CSV files to `output_dir`: + - camera_pose_errors.csv (per-sample camera position/orientation error) + - human_pose_errors.csv (per-keyframe, per-track human position error) + - human_track_summary.csv (per estimated track_id: dominant matched GT id, + match purity, mean/RMSE position error) +Ctrl+C also triggers a graceful finalize before exit. +""" + +import csv +import math +import os +import re +import threading +from collections import defaultdict + +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.time import Time +from rclpy.duration import Duration + +from tf2_ros import Buffer, TransformListener +from tf2_ros import TransformException + +from nav_msgs.msg import Odometry +from dynamic_keypoint_3d_lifter.msg import DynamicObjectPoints3D + +OBJECT_FRAME_RE_TEMPLATE = r"^{prefix}(\d+)$" + + +def stamp_to_sec(stamp) -> float: + return float(stamp.sec) + float(stamp.nanosec) * 1e-9 + + +def rigid_align(src: np.ndarray, dst: np.ndarray): + """Closed-form rigid (rotation + translation, no scale) alignment mapping + `src` points onto `dst` points (Umeyama 1991, scale term computed only as + a diagnostic and not applied). src, dst: (N,3) arrays of corresponding + points, N >= 3. + + Returns (R (3,3), t (3,), scale_diagnostic (float)). + """ + n = src.shape[0] + src_mean = src.mean(axis=0) + dst_mean = dst.mean(axis=0) + src_c = src - src_mean + dst_c = dst - dst_mean + + h = src_c.T @ dst_c + u, s, vt = np.linalg.svd(h) + d = np.sign(np.linalg.det(vt.T @ u.T)) + if d == 0: + d = 1.0 + correction = np.diag([1.0, 1.0, d]) + r = vt.T @ correction @ u.T + t = dst_mean - r @ src_mean + + var_src = float((src_c ** 2).sum()) / n + scale_diag = float((s * np.array([1.0, 1.0, d])).sum() / (n * var_src)) if var_src > 0 else float("nan") + return r, t, scale_diag + + +# The environment this was developed against has a broken scipy install +# (scipy.sparse.linalg._propack is missing symbols, which scipy.optimize and +# scipy.spatial both transitively import), so quaternion composition and the +# min-cost assignment below are implemented directly in numpy instead of +# depending on scipy. Both are covered by self-tests during development +# (Hungarian algorithm checked against brute force, quaternion <-> matrix +# round-trip and composition checked against a QR-based random rotation +# generator) - see conversation history for the verification runs. + +def quat_multiply(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: + """Hamilton product q1 * q2, both in ROS [x, y, z, w] convention.""" + x1, y1, z1, w1 = q1 + x2, y2, z2, w2 = q2 + return np.array([ + w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2, + w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2, + w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2, + w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2, + ]) + + +def quat_from_matrix(m: np.ndarray) -> np.ndarray: + """Rotation matrix (3,3) -> unit quaternion [x, y, z, w] (Shepperd's method).""" + trace = m[0, 0] + m[1, 1] + m[2, 2] + if trace > 0: + s = 0.5 / math.sqrt(trace + 1.0) + w = 0.25 / s + x = (m[2, 1] - m[1, 2]) * s + y = (m[0, 2] - m[2, 0]) * s + z = (m[1, 0] - m[0, 1]) * s + elif m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]: + s = 2.0 * math.sqrt(1.0 + m[0, 0] - m[1, 1] - m[2, 2]) + w = (m[2, 1] - m[1, 2]) / s + x = 0.25 * s + y = (m[0, 1] + m[1, 0]) / s + z = (m[0, 2] + m[2, 0]) / s + elif m[1, 1] > m[2, 2]: + s = 2.0 * math.sqrt(1.0 + m[1, 1] - m[0, 0] - m[2, 2]) + w = (m[0, 2] - m[2, 0]) / s + x = (m[0, 1] + m[1, 0]) / s + y = 0.25 * s + z = (m[1, 2] + m[2, 1]) / s + else: + s = 2.0 * math.sqrt(1.0 + m[2, 2] - m[0, 0] - m[1, 1]) + w = (m[1, 0] - m[0, 1]) / s + x = (m[0, 2] + m[2, 0]) / s + y = (m[1, 2] + m[2, 1]) / s + z = 0.25 * s + q = np.array([x, y, z, w]) + return q / np.linalg.norm(q) + + +def quat_angle_deg(q1: np.ndarray, q2: np.ndarray) -> float: + """Geodesic angle in degrees between two unit quaternions (double-cover safe).""" + dot = abs(float(np.dot(q1, q2))) + dot = min(1.0, max(-1.0, dot)) + return math.degrees(2.0 * math.acos(dot)) + + +def hungarian_min_cost_assignment(cost: np.ndarray): + """Minimum-cost bipartite assignment (Kuhn-Munkres with potentials, O(n^2 m)). + cost: (n_rows, n_cols). Returns (row_indices, col_indices) covering every row + if n_rows <= n_cols, or every column otherwise - i.e. min(n_rows, n_cols) pairs, + matching the semantics of scipy.optimize.linear_sum_assignment for a fully + finite cost matrix. Self-contained to avoid this environment's broken scipy + install (see module docstring above). + """ + cost = np.asarray(cost, dtype=float) + n_rows, n_cols = cost.shape + if n_rows == 0 or n_cols == 0: + return np.array([], dtype=int), np.array([], dtype=int) + + transposed = n_rows > n_cols + work = cost.T if transposed else cost + n, m = work.shape # n <= m guaranteed here + + inf = float("inf") + u = [0.0] * (n + 1) + v = [0.0] * (m + 1) + p = [0] * (m + 1) # p[j] = row (1-indexed) currently matched to column j + way = [0] * (m + 1) + a = np.zeros((n + 1, m + 1)) + a[1:, 1:] = work + + for i in range(1, n + 1): + p[0] = i + j0 = 0 + minv = [inf] * (m + 1) + used = [False] * (m + 1) + while True: + used[j0] = True + i0 = p[j0] + delta = inf + j1 = -1 + for j in range(1, m + 1): + if not used[j]: + cur = a[i0][j] - u[i0] - v[j] + if cur < minv[j]: + minv[j] = cur + way[j] = j0 + if minv[j] < delta: + delta = minv[j] + j1 = j + for j in range(m + 1): + if used[j]: + u[p[j]] += delta + v[j] -= delta + else: + minv[j] -= delta + j0 = j1 + if p[j0] == 0: + break + while j0: + j1 = way[j0] + p[j0] = p[j1] + j0 = j1 + + row_ind, col_ind = [], [] + for j in range(1, m + 1): + if p[j] != 0: + row_ind.append(p[j] - 1) + col_ind.append(j - 1) + row_ind = np.array(row_ind, dtype=int) + col_ind = np.array(col_ind, dtype=int) + order = np.argsort(row_ind) + row_ind, col_ind = row_ind[order], col_ind[order] + + if transposed: + row_ind, col_ind = col_ind, row_ind + return row_ind, col_ind + + +class UHumans2Evaluator(Node): + def __init__(self): + super().__init__("uhumans2_evaluator") + + self.declare_parameter("world_frame", "world") + self.declare_parameter("camera_gt_frame", "base_link_gt") + self.declare_parameter("object_frame_prefix", "object_") + self.declare_parameter("body_odom_topic", "/vs_graphs/body_odom") + self.declare_parameter("human_points_topic", "/dynamic_object_points_3d") + self.declare_parameter("output_dir", ".") + self.declare_parameter("idle_timeout_s", 5.0) + self.declare_parameter("tf_lookup_timeout_s", 0.05) + self.declare_parameter("max_match_distance_m", 1.5) + self.declare_parameter("tf_cache_time_s", 60.0) + + self.world_frame = self.get_parameter("world_frame").value + self.camera_gt_frame = self.get_parameter("camera_gt_frame").value + self.object_frame_prefix = self.get_parameter("object_frame_prefix").value + self.body_odom_topic = self.get_parameter("body_odom_topic").value + self.human_points_topic = self.get_parameter("human_points_topic").value + self.output_dir = self.get_parameter("output_dir").value + self.idle_timeout_s = float(self.get_parameter("idle_timeout_s").value) + self.tf_lookup_timeout_s = float(self.get_parameter("tf_lookup_timeout_s").value) + self.max_match_distance_m = float(self.get_parameter("max_match_distance_m").value) + tf_cache_time_s = float(self.get_parameter("tf_cache_time_s").value) + + self._object_frame_re = re.compile( + OBJECT_FRAME_RE_TEMPLATE.format(prefix=re.escape(self.object_frame_prefix))) + + self.tf_buffer = Buffer(cache_time=Duration(seconds=tf_cache_time_s)) + self.tf_listener = TransformListener(self.tf_buffer, self, spin_thread=False) + + self._lock = threading.Lock() + self.camera_samples = [] + self.human_samples = [] + self._finalized = False + self.last_msg_time = self.get_clock().now() + + self.create_subscription(Odometry, self.body_odom_topic, self._odom_cb, 50) + self.create_subscription( + DynamicObjectPoints3D, self.human_points_topic, self._human_cb, 50) + self.watchdog_timer = self.create_timer(1.0, self._watchdog_cb) + + self.get_logger().info( + f"uhumans2_evaluator: world_frame='{self.world_frame}' " + f"camera_gt_frame='{self.camera_gt_frame}' " + f"body_odom_topic='{self.body_odom_topic}' " + f"human_points_topic='{self.human_points_topic}' " + f"output_dir='{self.output_dir}' idle_timeout_s={self.idle_timeout_s}") + + # ------------------------------------------------------------------ + # Subscriptions + # ------------------------------------------------------------------ + def _odom_cb(self, msg: Odometry): + self.last_msg_time = self.get_clock().now() + stamp = msg.header.stamp + try: + tf = self.tf_buffer.lookup_transform( + self.world_frame, self.camera_gt_frame, Time.from_msg(stamp), + timeout=Duration(seconds=self.tf_lookup_timeout_s)) + except TransformException as ex: + self.get_logger().warn( + f"No GT transform {self.world_frame}->{self.camera_gt_frame} at " + f"t={stamp_to_sec(stamp):.3f}: {ex}", throttle_duration_sec=2.0) + return + + est_pos = np.array([msg.pose.pose.position.x, msg.pose.pose.position.y, + msg.pose.pose.position.z]) + est_quat = np.array([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]) + gt_pos = np.array([tf.transform.translation.x, tf.transform.translation.y, + tf.transform.translation.z]) + gt_quat = np.array([tf.transform.rotation.x, tf.transform.rotation.y, + tf.transform.rotation.z, tf.transform.rotation.w]) + + with self._lock: + self.camera_samples.append({ + "t": stamp_to_sec(stamp), + "est_pos": est_pos, + "est_quat": est_quat, + "gt_pos": gt_pos, + "gt_quat": gt_quat, + }) + + def _discover_object_frames(self): + yaml_str = self.tf_buffer.all_frames_as_yaml() + ids = [] + for line in yaml_str.splitlines(): + line = line.strip() + if not line.endswith(":"): + continue + match = self._object_frame_re.match(line[:-1]) + if match: + ids.append(match.group(1)) + return ids + + def _human_cb(self, msg: DynamicObjectPoints3D): + self.last_msg_time = self.get_clock().now() + if not msg.objects: + return + stamp = msg.header.stamp + t = Time.from_msg(stamp) + + gt_snapshot = {} + for object_id in self._discover_object_frames(): + frame = f"{self.object_frame_prefix}{object_id}" + try: + tf = self.tf_buffer.lookup_transform( + self.world_frame, frame, t, + timeout=Duration(seconds=self.tf_lookup_timeout_s)) + except TransformException: + continue + gt_snapshot[object_id] = np.array([ + tf.transform.translation.x, tf.transform.translation.y, + tf.transform.translation.z]) + + new_samples = [] + for obj in msg.objects: + if not obj.points_world: + continue + pts = np.array([[p.x, p.y, p.z] for p in obj.points_world]) + new_samples.append({ + "t": stamp_to_sec(stamp), + "keyframe_id": msg.keyframe_id, + "track_id": obj.track_id, + "class_id": obj.class_id, + "centroid_raw": pts.mean(axis=0), + "n_points": pts.shape[0], + "gt_snapshot": gt_snapshot, + }) + + with self._lock: + self.human_samples.extend(new_samples) + + # ------------------------------------------------------------------ + # Finalization + # ------------------------------------------------------------------ + def _watchdog_cb(self): + if self._finalized: + return + now = self.get_clock().now() + idle_s = (now - self.last_msg_time).nanoseconds * 1e-9 + have_data = len(self.camera_samples) > 0 or len(self.human_samples) > 0 + if have_data and idle_s > self.idle_timeout_s: + self.get_logger().info( + f"No new data for {idle_s:.1f}s (>{self.idle_timeout_s}s) - " + "assuming the run has ended, finalizing evaluation.") + self.finalize() + + def finalize(self): + with self._lock: + if self._finalized: + return + self._finalized = True + camera_samples = list(self.camera_samples) + human_samples = list(self.human_samples) + + os.makedirs(self.output_dir, exist_ok=True) + + r, t, scale_diag = self._write_camera_csv(camera_samples) + self._write_human_csvs(human_samples, r, t) + + self.get_logger().info( + f"Evaluation complete. camera_samples={len(camera_samples)} " + f"human_samples={len(human_samples)} scale_diagnostic={scale_diag:.4f} " + f"(should be close to 1.0 for correctly-scaled mono-inertial output). " + f"CSVs written to '{self.output_dir}'.") + + def _write_camera_csv(self, camera_samples): + path = os.path.join(self.output_dir, "camera_pose_errors.csv") + if len(camera_samples) < 3: + self.get_logger().warn( + f"Only {len(camera_samples)} camera sample(s) with matching GT - " + "need >=3 to compute alignment. Writing raw (unaligned) values only.") + r, t, scale_diag = np.eye(3), np.zeros(3), float("nan") + else: + src = np.stack([s["est_pos"] for s in camera_samples]) + dst = np.stack([s["gt_pos"] for s in camera_samples]) + r, t, scale_diag = rigid_align(src, dst) + + q_r = quat_from_matrix(r) + pos_errors = [] + ang_errors = [] + with open(path, "w", newline="") as f: + writer = csv.writer(f) + writer.writerow([ + "timestamp", "gt_x", "gt_y", "gt_z", "gt_qx", "gt_qy", "gt_qz", "gt_qw", + "est_x_raw", "est_y_raw", "est_z_raw", + "est_x_aligned", "est_y_aligned", "est_z_aligned", + "est_qx_aligned", "est_qy_aligned", "est_qz_aligned", "est_qw_aligned", + "position_error_m", "orientation_error_deg", + ]) + for s in camera_samples: + aligned_pos = r @ s["est_pos"] + t + aligned_quat = quat_multiply(q_r, s["est_quat"]) + pos_err = float(np.linalg.norm(aligned_pos - s["gt_pos"])) + ang_err = quat_angle_deg(aligned_quat, s["gt_quat"]) + pos_errors.append(pos_err) + ang_errors.append(ang_err) + writer.writerow([ + f"{s['t']:.9f}", + *[f"{v:.6f}" for v in s["gt_pos"]], + *[f"{v:.6f}" for v in s["gt_quat"]], + *[f"{v:.6f}" for v in s["est_pos"]], + *[f"{v:.6f}" for v in aligned_pos], + *[f"{v:.6f}" for v in aligned_quat], + f"{pos_err:.6f}", f"{ang_err:.6f}", + ]) + + if pos_errors: + pos_errors_np = np.array(pos_errors) + ang_errors_np = np.array(ang_errors) + self.get_logger().info( + f"Camera ATE (rigid-aligned, {len(pos_errors)} samples): " + f"RMSE={np.sqrt((pos_errors_np ** 2).mean()):.4f} m " + f"mean={pos_errors_np.mean():.4f} m median={np.median(pos_errors_np):.4f} m " + f"max={pos_errors_np.max():.4f} m | " + f"orientation RMSE={np.sqrt((ang_errors_np ** 2).mean()):.3f} deg") + self.get_logger().info(f"Wrote {path}") + return r, t, scale_diag + + def _write_human_csvs(self, human_samples, r, t): + errors_path = os.path.join(self.output_dir, "human_pose_errors.csv") + summary_path = os.path.join(self.output_dir, "human_track_summary.csv") + + by_stamp = defaultdict(list) + for h in human_samples: + aligned = r @ h["centroid_raw"] + t + by_stamp[h["t"]].append({**h, "centroid_aligned": aligned}) + + track_match_counts = defaultdict(lambda: defaultdict(int)) + track_errors = defaultdict(list) + + with open(errors_path, "w", newline="") as f: + writer = csv.writer(f) + writer.writerow([ + "timestamp", "keyframe_id", "track_id", "class_id", "n_points", + "est_x", "est_y", "est_z", "matched_gt_id", + "gt_x", "gt_y", "gt_z", "position_error_m", "n_gt_visible", + ]) + for stamp in sorted(by_stamp.keys()): + entries = by_stamp[stamp] + gt_snapshot = entries[0]["gt_snapshot"] + gt_ids = list(gt_snapshot.keys()) + n_gt = len(gt_ids) + + matched = {} + if n_gt > 0: + cost = np.zeros((len(entries), n_gt)) + for i, e in enumerate(entries): + for j, gid in enumerate(gt_ids): + cost[i, j] = np.linalg.norm( + e["centroid_aligned"] - gt_snapshot[gid]) + row_ind, col_ind = hungarian_min_cost_assignment(cost) + for row, col in zip(row_ind, col_ind): + if cost[row, col] <= self.max_match_distance_m: + matched[row] = (gt_ids[col], cost[row, col]) + + for i, e in enumerate(entries): + if i in matched: + gid, err = matched[i] + gt_pos = gt_snapshot[gid] + track_match_counts[e["track_id"]][gid] += 1 + track_errors[e["track_id"]].append(err) + writer.writerow([ + f"{stamp:.9f}", e["keyframe_id"], e["track_id"], e["class_id"], + e["n_points"], *[f"{v:.6f}" for v in e["centroid_aligned"]], + gid, *[f"{v:.6f}" for v in gt_pos], f"{err:.6f}", n_gt, + ]) + else: + writer.writerow([ + f"{stamp:.9f}", e["keyframe_id"], e["track_id"], e["class_id"], + e["n_points"], *[f"{v:.6f}" for v in e["centroid_aligned"]], + -1, "", "", "", "", n_gt, + ]) + + with open(summary_path, "w", newline="") as f: + writer = csv.writer(f) + writer.writerow([ + "track_id", "dominant_gt_id", "n_frames_matched_dominant", + "n_frames_matched_any", "match_purity", "mean_position_error_m", + "rmse_position_error_m", + ]) + for track_id in sorted(track_match_counts.keys()): + gid_counts = track_match_counts[track_id] + dominant_gid, dominant_count = max(gid_counts.items(), key=lambda kv: kv[1]) + n_any = sum(gid_counts.values()) + errs = np.array(track_errors[track_id]) + purity = dominant_count / n_any if n_any else float("nan") + writer.writerow([ + track_id, dominant_gid, dominant_count, n_any, f"{purity:.4f}", + f"{errs.mean():.6f}", f"{math.sqrt((errs ** 2).mean()):.6f}", + ]) + + n_matched_rows = sum(len(v) for v in track_errors.values()) + self.get_logger().info( + f"Human evaluation: {len(human_samples)} raw estimate rows, " + f"{len(track_match_counts)} distinct track_ids, " + f"{n_matched_rows} matched to a GT human (max_match_distance_m=" + f"{self.max_match_distance_m}).") + self.get_logger().info(f"Wrote {errors_path}") + self.get_logger().info(f"Wrote {summary_path}") + + +def main(): + rclpy.init() + node = UHumans2Evaluator() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.finalize() + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/include/common.h b/include/common.h index f7ab45c9..a222e244 100644 --- a/include/common.h +++ b/include/common.h @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -57,6 +58,7 @@ #include #include #include +#include #include // #include // #include @@ -76,6 +78,9 @@ #include #include +#include +#include +#include #include #include #include @@ -113,6 +118,10 @@ // vS-Graphs Custom Messages #include #include +#include +#include +#include +#include using json = nlohmann::json; @@ -142,10 +151,14 @@ extern std::vector gnnRoomCandidates; extern rclcpp::Publisher::SharedPtr pubAllWalls_new; extern rclcpp::Publisher::SharedPtr pubAllWalls_legacy; -extern rclcpp::Time lastPlanePublishTime; +extern double lastPlanePublishTime; extern std::shared_ptr pubTrackingImage; extern rclcpp::Publisher::SharedPtr pubOdometry; +extern rclcpp::Publisher::SharedPtr pubImuBias; extern rclcpp::Publisher::SharedPtr pubKFImage; +extern rclcpp::Publisher::SharedPtr pubKeyFrameCreated; +extern rclcpp::Publisher::SharedPtr pubKeyFrameStaticMapPoints; +extern rclcpp::Publisher::SharedPtr pubStaticMapPoints; extern rclcpp::Publisher::SharedPtr pubCameraPose; extern rclcpp::Publisher::SharedPtr pubAllMappoints; extern rclcpp::Publisher::SharedPtr pubTrackedMappoints; @@ -175,10 +188,12 @@ void publishPlanes(std::vector, rclcpp::Time); void publishTFTransform(Sophus::SE3f, string, string, rclcpp::Time); void publishAllPoints(std::vector, rclcpp::Time); void publishTrackedPoints(std::vector, rclcpp::Time); +void publishStaticMapPoints(std::vector, rclcpp::Time); void publishFiducialMarkers(std::vector, rclcpp::Time); void publishKeyFrameImages(std::vector, rclcpp::Time); void publishKeyFrameMarkers(std::vector, rclcpp::Time); void publishBodyOdometry(Sophus::SE3f, Eigen::Vector3f, Eigen::Vector3f, rclcpp::Time); +void publishImuBias(const ORB_SLAM3::IMU::Bias &, rclcpp::Time); void publishStructuralElements(std::vector, std::vector, rclcpp::Time); /** @@ -273,4 +288,4 @@ void setGNNBasedRoomCandidates(const situational_graphs_msgs::msg::RoomsData &ms * Mainly designed for the new version of the GNN-based room detector * @param msgGNNRooms The message containing the detected room candidates */ -void setGNNBasedRoomCandidates(const vs_graphs::msg::VSGraphsAllDetectdetRooms &msgGNNRooms); \ No newline at end of file +void setGNNBasedRoomCandidates(const vs_graphs::msg::VSGraphsAllDetectdetRooms &msgGNNRooms); diff --git a/install_workspace_repos.sh b/install_workspace_repos.sh new file mode 100755 index 00000000..2b5320f6 --- /dev/null +++ b/install_workspace_repos.sh @@ -0,0 +1,71 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Clone/update the companion repositories used by this workspace. +# Run this script from anywhere inside a clone of visual_sgraphs. + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +WORKSPACE_SRC="$(cd "${SCRIPT_DIR}/.." && pwd)" + +GIT_PROTOCOL="${GIT_PROTOCOL:-ssh}" + +if [[ "${GIT_PROTOCOL}" != "ssh" && "${GIT_PROTOCOL}" != "https" ]]; then + echo "GIT_PROTOCOL must be either 'ssh' or 'https'." >&2 + exit 1 +fi + +repo_url() { + local owner="$1" + local repo="$2" + + if [[ "${GIT_PROTOCOL}" == "https" ]]; then + printf 'https://github.com/%s/%s.git' "${owner}" "${repo}" + else + printf 'git@github.com:%s/%s.git' "${owner}" "${repo}" + fi +} + +clone_or_update() { + local path="$1" + local owner="$2" + local repo="$3" + local branch="$4" + local target="${WORKSPACE_SRC}/${path}" + local url + url="$(repo_url "${owner}" "${repo}")" + + if [[ -d "${target}/.git" ]]; then + echo "Updating ${path} (${branch})" + git -C "${target}" fetch origin "${branch}" + git -C "${target}" checkout "${branch}" + git -C "${target}" pull --ff-only origin "${branch}" + return + fi + + if [[ -e "${target}" ]]; then + echo "Skipping ${path}: path exists but is not a git repository." >&2 + return + fi + + echo "Cloning ${path} (${branch})" + mkdir -p "$(dirname "${target}")" + git clone --branch "${branch}" "${url}" "${target}" +} + +echo "Workspace source directory: ${WORKSPACE_SRC}" +echo "Git protocol: ${GIT_PROTOCOL}" + +clone_or_update "dynamic_keypoint_3d_lifter" "snt-arg" "dynamic_keypoint_3d_lifter" "master" +clone_or_update "dynamic_keypoint_tracker" "snt-arg" "dynamic_keypoint_tracker" "master" +clone_or_update "keyframe_depth_estimator" "snt-arg" "keyframe_depth_estimator" "master" +clone_or_update "keyframe_depth_validator" "snt-arg" "keyframe_depth_validator" "master" +clone_or_update "object_motion_estimator" "snt-arg" "object_motion_estimator" "master" +clone_or_update "realsense-ros" "IntelRealSense" "realsense-ros" "ros2-master" +clone_or_update "scene_segment_ros" "snt-arg" "scene_segment_ros" "dynamic" +clone_or_update "situational_graphs_msgs" "snt-arg" "situational_graphs_msgs" "main" +clone_or_update "voxblox_ros2_minimal" "snt-arg" "voxblox_ros2_minimal" "master" +clone_or_update "bin/dynamic_s_graphs" "snt-arg" "dynamic_s_graphs" "master" +clone_or_update "bin/object_tracker_3d_ros" "snt-arg" "object_tracker_3d_ros" "master" +clone_or_update "scene_segment_ros/src/eomt" "tue-mps" "eomt" "master" + +echo "Done." diff --git a/launch/mono-imu.launch.py b/launch/mono-imu.launch.py new file mode 100644 index 00000000..2efa94dc --- /dev/null +++ b/launch/mono-imu.launch.py @@ -0,0 +1,1198 @@ +import os + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.conditions import IfCondition +from launch.actions import DeclareLaunchArgument +from launch_ros.descriptions import ComposableNode +from launch_ros.actions import ComposableNodeContainer +from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration, EqualsSubstitution +from launch_ros.parameter_descriptions import ParameterValue + + +def generate_launch_description(): + keyframe_depth_config = os.path.join( + get_package_share_directory("keyframe_depth_estimator"), + "config", + "config.yaml", + ) + keyframe_depth_validator_config = os.path.join( + get_package_share_directory("keyframe_depth_validator"), + "config", + "config.yaml", + ) + dynamic_keypoint_tracker_config = os.path.join( + get_package_share_directory("dynamic_keypoint_tracker"), + "config", + "config.yaml", + ) + dynamic_keypoint_3d_lifter_config = os.path.join( + get_package_share_directory("dynamic_keypoint_3d_lifter"), + "config", + "config.yaml", + ) + object_motion_estimator_config = os.path.join( + get_package_share_directory("object_motion_estimator"), + "config", + "config.yaml", + ) + + return LaunchDescription( + [ + # Global arguments declarations + DeclareLaunchArgument("offline", default_value="true"), + DeclareLaunchArgument("launch_rviz", default_value="true"), + DeclareLaunchArgument( + "rviz_config", + default_value=[ + get_package_share_directory("vs_graphs"), + "/config/Visualization/vsgraphs_rgbd.rviz", + ], + ), + DeclareLaunchArgument("colored_pointcloud", default_value="true"), + DeclareLaunchArgument("visualize_segmented_scene", default_value="true"), + DeclareLaunchArgument("use_aux_depth", default_value="false"), + DeclareLaunchArgument("launch_keyframe_depth_estimator", default_value="false"), # Pipeline DA3 + DeclareLaunchArgument("launch_keyframe_depth_validator", default_value="false"), # Pipeline DA3 + DeclareLaunchArgument("launch_dynamic_keypoint_tracker", default_value="true"), + DeclareLaunchArgument("launch_object_track_manager", default_value="true"), + DeclareLaunchArgument("launch_track_outlier_rejector", default_value="true"), + DeclareLaunchArgument("launch_dynamic_keypoint_3d_lifter", default_value="false"), # Pipeline DA3 + DeclareLaunchArgument("launch_dynamic_keypoint_interpolator", default_value="false"), # Pipeline DA3 + DeclareLaunchArgument("launch_object_motion_estimator", default_value="false"), # Pipeline DA3 + # D12: Pipeline B (object_ba_tracker_node) retired from the default launch graph -- + # its fixed-landmark BA output is no longer consumed by joint_window_node, which now + # gets its landmark seeds from landmark_initializer_node and optimizes landmarks + # in-graph. Flag kept (not removed) so the node can still be launched standalone for + # debugging/comparison; object_ba_tracker_node.cpp/.hpp are kept in the source tree + # unbuilt-from-launch (still compiled as an executable) since + # landmark_initializer_node's triangulation was ported from it. + DeclareLaunchArgument("launch_object_ba_tracker", default_value="false"), # Pipeline BA (D12: retired by default) + DeclareLaunchArgument("launch_landmark_initializer", default_value="true"), # D12: landmark seed source + DeclareLaunchArgument( + "landmark_seed_topic", default_value="/object_motion/landmark_seed" + ), + DeclareLaunchArgument("landmark_min_frames_for_triangulation", default_value="5"), + DeclareLaunchArgument("landmark_window_max_frames", default_value="15"), + DeclareLaunchArgument( + "keyframe_depth_model_path", + default_value=( + "/home/marco/workspace/src/keyframe_depth_estimator/include/models/" + "engines/da3metric_280x504.engine" + ), + ), + DeclareLaunchArgument( + "keyframe_depth_metric_topic", default_value="/keyframe_depth/metric" + ), + DeclareLaunchArgument( + "keyframe_depth_corrected_topic", default_value="/keyframe_depth/corrected" + ), + DeclareLaunchArgument( + "keyframe_depth_publish_debug_image", default_value="false" + ), + DeclareLaunchArgument( + "keyframe_depth_validator_publish_debug_image", default_value="false" + ), + DeclareLaunchArgument( + "keyframe_depth_validator_queue_depth", default_value="100" + ), + DeclareLaunchArgument( + "keyframe_depth_validator_debug_match_logging", default_value="false" + ), + DeclareLaunchArgument( + "keyframe_depth_validator_use_offline_metric_depth", default_value="true" + ), + DeclareLaunchArgument( + "keyframe_depth_validator_offline_sync_tolerance_ms", default_value="20.0" + ), + DeclareLaunchArgument( + "keyframe_depth_validator_tuning_mode", default_value="false" + ), + DeclareLaunchArgument( + "keyframe_depth_validator_tuning_history_size", default_value="200" + ), + DeclareLaunchArgument( + "keyframe_depth_validator_tuning_visualization_period_s", default_value="2.0" + ), + DeclareLaunchArgument( + "keyframe_depth_validator_rgbd_depth_topic", + default_value="/camera/realsense/aligned_depth_to_color/image_raw", + ), + DeclareLaunchArgument( + "keyframe_depth_validator_dynamic_masks_topic", + default_value="/camera/color/instance_masks", + ), + DeclareLaunchArgument( + "keyframe_depth_validator_rgbd_valid_range_min_m", default_value="0.3" + ), + DeclareLaunchArgument( + "keyframe_depth_validator_rgbd_valid_range_max_m", default_value="8.0" + ), + DeclareLaunchArgument( + "keyframe_depth_validator_rgbd_heatmap_max_abs_error_m", default_value="1.0" + ), + DeclareLaunchArgument( + "dynamic_keypoint_instance_masks_topic", + default_value="/camera/color/image_instance_masks_array", + ), + DeclareLaunchArgument( + "dynamic_keypoint_tracks_topic", + default_value="/dynamic_keypoint_tracks", + ), + DeclareLaunchArgument( + "dynamic_keypoint_object_tracks_topic", + default_value="/object_tracks", + ), + DeclareLaunchArgument( + "dynamic_keypoint_filtered_object_tracks_topic", + default_value="/object_tracks/filtered", + ), + DeclareLaunchArgument( + "dynamic_keypoint_publish_debug_image", + default_value="true", + ), + DeclareLaunchArgument( + "dynamic_keypoint_debug_image_width", + default_value="0", + ), + DeclareLaunchArgument( + "dynamic_keypoint_debug_image_height", + default_value="0", + ), + DeclareLaunchArgument( + "dynamic_keypoint_3d_lifter_output_topic", + default_value="/dynamic_object_points_3d", + ), + DeclareLaunchArgument( + "dynamic_keypoint_3d_lifter_low_confidence_behavior", + default_value="lift_and_flag", + ), + DeclareLaunchArgument( + "dynamic_keypoint_3d_lifter_low_confidence_max_residual_m", + default_value="0.25", + ), + DeclareLaunchArgument( + "dynamic_keypoint_3d_lifter_max_lift_distance_m", + default_value="7.0", + ), + DeclareLaunchArgument( + "dynamic_keypoint_3d_lifter_publish_debug_image", + default_value="true", + ), + DeclareLaunchArgument( + "dynamic_keypoint_3d_lifter_queue_depth", + default_value="100", + ), + DeclareLaunchArgument( + "dynamic_keypoint_3d_lifter_debug_match_logging", + default_value="false", + ), + DeclareLaunchArgument( + "dynamic_keypoint_interpolator_output_topic", + default_value="/dynamic_object_points_3d/interpolated", + ), + DeclareLaunchArgument( + "dynamic_keypoint_interpolator_queue_depth", + default_value="300", + ), + DeclareLaunchArgument( + "dynamic_keypoint_interpolator_gap_timeout_ms", + default_value="2000", + ), + DeclareLaunchArgument( + "dynamic_keypoint_interpolator_min_residual_path_length_px", + default_value="1.0", + ), + DeclareLaunchArgument( + "dynamic_keypoint_interpolator_publish_debug_image", + default_value="true", + ), + DeclareLaunchArgument( + "object_motion_input_mode", + default_value="keyframe_and_interpolated_weighted", + description=( + "Phase 4 input mode: keyframe_only, keyframe_and_interpolated, " + "or keyframe_and_interpolated_weighted" + ), + ), + DeclareLaunchArgument( + "object_motion_output_topic", + default_value="/object_motion/depth_model", + ), + DeclareLaunchArgument( + "object_motion_markers_topic", + default_value="/object_motion/depth_model/markers", + ), + DeclareLaunchArgument( + "object_motion_world_frame_id", + default_value="world", + ), + DeclareLaunchArgument( + "object_motion_queue_depth", + default_value="300", + ), + DeclareLaunchArgument( + "object_motion_gap_timeout_ms", + default_value="2000", + ), + DeclareLaunchArgument( + "object_motion_trajectory_window_size", + default_value="2147483647", + ), + DeclareLaunchArgument( + "object_motion_mad_k", + default_value="2.5", + ), + DeclareLaunchArgument( + "object_motion_ema_alpha_h", + default_value="0.4", + ), + DeclareLaunchArgument( + "object_motion_publish_debug_image", + default_value="true", + ), + DeclareLaunchArgument( + "object_motion_publish_markers", + default_value="true", + ), + DeclareLaunchArgument( + "object_motion_arrow_scale", + default_value="1.0", + ), + DeclareLaunchArgument( + "object_motion_arrow_time_scale_s", + default_value="1.0", + ), + DeclareLaunchArgument( + "object_motion_arrow_lifetime_s", + default_value="0.0", + ), + DeclareLaunchArgument( + "object_motion_clear_markers_on_track_lost", + default_value="false", + ), + DeclareLaunchArgument( + "object_motion_marker_line_width_m", + default_value="0.05", + ), + DeclareLaunchArgument( + "object_motion_marker_sphere_radius_m", + default_value="0.08", + ), + DeclareLaunchArgument( + "object_motion_marker_text_height_m", + default_value="0.15", + ), + DeclareLaunchArgument( + "object_ba_output_topic", + default_value="/object_motion/ba", + ), + DeclareLaunchArgument( + "object_ba_markers_topic", + default_value="/object_motion/ba/markers", + ), + DeclareLaunchArgument( + "object_ba_window_size", + default_value="20", + ), + DeclareLaunchArgument( + "object_ba_min_window_frames", + default_value="5", + ), + DeclareLaunchArgument( + "object_ba_min_point_observations", + default_value="6", + ), + DeclareLaunchArgument( + "object_ba_sigma_pixel", + default_value="1.5", + ), + DeclareLaunchArgument( + "object_ba_huber_delta_pixels", + default_value="3.0", + ), + DeclareLaunchArgument( + "object_ba_num_iterations", + default_value="10", + ), + DeclareLaunchArgument( + "object_ba_time_budget_ms", + default_value="15.0", + ), + DeclareLaunchArgument( + "object_ba_min_camera_displacement_m", + default_value="0.05", + ), + DeclareLaunchArgument( + "object_ba_alpha_scale", + default_value="0.3", + ), + DeclareLaunchArgument( + "object_ba_pose_topic_timeout_s", + default_value="5.0", + ), + DeclareLaunchArgument( + "object_ba_pose_sync_tolerance_ms", + default_value="10.0", + ), + DeclareLaunchArgument( + "object_ba_mad_k", + default_value="2.5", + ), + DeclareLaunchArgument( + "object_ba_max_landmark_anchor_norm_m", + default_value="20.0", + ), + DeclareLaunchArgument( + "object_ba_max_landmark_reprojection_error_px", + default_value="8.0", + ), + DeclareLaunchArgument( + "object_ba_min_scale_object_displacement_m", + default_value="0.10", + ), + DeclareLaunchArgument("keyframe_depth_sky_handling", default_value="true"), + DeclareLaunchArgument( + "aux_depth_topic", default_value="/camera/depth_da3/image_rect" + ), + DeclareLaunchArgument( + "semantic_scene_segmenter", + default_value="off", + description="The method to segment the semantic scene (if off, the baseline)", + choices=["yoso", "pfcn", "yolo26", "off"], + ), + # Topics + DeclareLaunchArgument("camera_frame", default_value="camera"), #left_cam + DeclareLaunchArgument("sensor_config", default_value="uHumans2_TESSE"), #UniLu_RealSense_D435i_640 + DeclareLaunchArgument( + "rgb_image_topic", default_value="/tesse/left_cam/rgb/image_raw" #"/camera/realsense/color/image_raw" + ), + DeclareLaunchArgument( + "rgb_camera_info_topic", + default_value="/tesse/left_cam/camera_info", # "/camera/realsense/color/camera_info", + ), + DeclareLaunchArgument( + "imu_topic", + default_value="/tesse/imu/noisy/imu", #"/camera/realsense/imu" + ), + # VS-Graphs Node + Node( + name="vs_graphs", + package="vs_graphs", + executable="ros_mono_inertial", + output="screen", + parameters=[ + {"use_sim_time": LaunchConfiguration("offline")}, + { + "voc_file": LaunchConfiguration( + "voc_file", + default=[ + get_package_share_directory("vs_graphs"), + "/Vocabulary/ORBvoc.txt.bin", + ], + ) + }, + { + "settings_file": LaunchConfiguration( + "settings_file", + default=[ + get_package_share_directory("vs_graphs"), + "/config/Monocular-Inertial/", + LaunchConfiguration("sensor_config"), + ".yaml", + ], + ) + }, + { + "sys_params_file": LaunchConfiguration( + "sys_params_file", + default=[ + get_package_share_directory("vs_graphs"), + "/config/system_params.yaml", + ], + ) + }, + {"yaw": 0.0}, + {"roll": 0.0}, + {"pitch": 0.0}, + {"frame_map": "map"}, + {"frame_imu": "imu"}, + {"frame_world": "world"}, + {"frame_camera": "camera_color_optical_frame"}, + {"enable_pangolin": False}, + {"static_transform": True}, + {"colored_pointcloud": False}, + {"publish_pointclouds": True}, + {"use_aux_depth": LaunchConfiguration("use_aux_depth")}, + {"aux_depth_topic": LaunchConfiguration("aux_depth_topic")}, + ], + remappings=[ + ("/imu", LaunchConfiguration("imu_topic")), + ("/camera/image_raw", LaunchConfiguration("rgb_image_topic")), + ], + ), + # Keyframe Metric Depth Estimator + Node( + condition=IfCondition( + LaunchConfiguration("launch_keyframe_depth_estimator") + ), + package="keyframe_depth_estimator", + executable="keyframe_depth_estimator_node", + name="keyframe_depth_estimator", + output="screen", + parameters=[ + keyframe_depth_config, + { + "use_sim_time": LaunchConfiguration("offline"), + "keyframe_created_topic": "/orbslam3/keyframe_created", + "camera_info_topic": LaunchConfiguration( + "rgb_camera_info_topic" + ), + "settings_file": [ + get_package_share_directory("vs_graphs"), + "/config/Monocular-Inertial/", + LaunchConfiguration("sensor_config"), + ".yaml", + ], + "metric_depth_topic": LaunchConfiguration( + "keyframe_depth_metric_topic" + ), + "model_path": LaunchConfiguration( + "keyframe_depth_model_path" + ), + "publish_debug_image": ParameterValue( + LaunchConfiguration("keyframe_depth_publish_debug_image"), + value_type=bool, + ), + "sky_handling": ParameterValue( + LaunchConfiguration("keyframe_depth_sky_handling"), + value_type=bool, + ), + }, + ], + ), + # Keyframe Depth Validator + Node( + condition=IfCondition( + LaunchConfiguration("launch_keyframe_depth_validator") + ), + package="keyframe_depth_validator", + executable="keyframe_depth_validator_node", + name="keyframe_depth_validator", + output="screen", + parameters=[ + keyframe_depth_validator_config, + { + "use_sim_time": LaunchConfiguration("offline"), + "metric_depth_topic": LaunchConfiguration( + "keyframe_depth_metric_topic" + ), + "static_correspondences_topic": ( + "/orbslam3/keyframe_static_map_points" + ), + "corrected_depth_topic": LaunchConfiguration( + "keyframe_depth_corrected_topic" + ), + "queue_depth": ParameterValue( + LaunchConfiguration( + "keyframe_depth_validator_queue_depth" + ), + value_type=int, + ), + "publish_debug_image": ParameterValue( + LaunchConfiguration( + "keyframe_depth_validator_publish_debug_image" + ), + value_type=bool, + ), + "debug_match_logging": ParameterValue( + LaunchConfiguration( + "keyframe_depth_validator_debug_match_logging" + ), + value_type=bool, + ), + "use_offline_metric_depth": ParameterValue( + LaunchConfiguration( + "keyframe_depth_validator_use_offline_metric_depth" + ), + value_type=bool, + ), + "offline_metric_depth_sync_tolerance_ms": ParameterValue( + LaunchConfiguration( + "keyframe_depth_validator_offline_sync_tolerance_ms" + ), + value_type=float, + ), + "tuning_mode": ParameterValue( + LaunchConfiguration( + "keyframe_depth_validator_tuning_mode" + ), + value_type=bool, + ), + "tuning_history_size": ParameterValue( + LaunchConfiguration( + "keyframe_depth_validator_tuning_history_size" + ), + value_type=int, + ), + "tuning_visualization_period_s": ParameterValue( + LaunchConfiguration( + "keyframe_depth_validator_tuning_visualization_period_s" + ), + value_type=float, + ), + "rgbd_depth_topic": LaunchConfiguration( + "keyframe_depth_validator_rgbd_depth_topic" + ), + "dynamic_masks_topic": LaunchConfiguration( + "keyframe_depth_validator_dynamic_masks_topic" + ), + "rgbd_valid_range_min_m": ParameterValue( + LaunchConfiguration( + "keyframe_depth_validator_rgbd_valid_range_min_m" + ), + value_type=float, + ), + "rgbd_valid_range_max_m": ParameterValue( + LaunchConfiguration( + "keyframe_depth_validator_rgbd_valid_range_max_m" + ), + value_type=float, + ), + "rgbd_heatmap_max_abs_error_m": ParameterValue( + LaunchConfiguration( + "keyframe_depth_validator_rgbd_heatmap_max_abs_error_m" + ), + value_type=float, + ), + }, + ], + ), + # Dynamic Keypoint Tracker + Node( + condition=IfCondition( + LaunchConfiguration("launch_dynamic_keypoint_tracker") + ), + package="dynamic_keypoint_tracker", + executable="dynamic_keypoint_tracker_node", + name="dynamic_keypoint_tracker", + output="screen", + parameters=[ + { + "use_sim_time": LaunchConfiguration("offline"), + "image_topic": LaunchConfiguration("rgb_image_topic"), + "instance_masks_topic": LaunchConfiguration( + "dynamic_keypoint_instance_masks_topic" + ), + "tracks_topic": LaunchConfiguration( + "dynamic_keypoint_tracks_topic" + ), + "publish_debug_image": ParameterValue( + LaunchConfiguration("dynamic_keypoint_publish_debug_image"), + value_type=bool, + ), + }, + ], + ), + # Dynamic Object Track Manager + Node( + condition=IfCondition( + LaunchConfiguration("launch_object_track_manager") + ), + package="dynamic_keypoint_tracker", + executable="object_track_manager_node", + name="object_track_manager", + output="screen", + parameters=[ + dynamic_keypoint_tracker_config, + { + "use_sim_time": LaunchConfiguration("offline"), + "instance_masks_topic": LaunchConfiguration( + "dynamic_keypoint_instance_masks_topic" + ), + "dynamic_keypoint_tracks_topic": LaunchConfiguration( + "dynamic_keypoint_tracks_topic" + ), + "image_topic": LaunchConfiguration("rgb_image_topic"), + "object_tracks_topic": LaunchConfiguration( + "dynamic_keypoint_object_tracks_topic" + ), + "publish_debug_image": ParameterValue( + LaunchConfiguration("dynamic_keypoint_publish_debug_image"), + value_type=bool, + ), + }, + ], + ), + # Dynamic Track Outlier Rejector + Node( + condition=IfCondition( + LaunchConfiguration("launch_track_outlier_rejector") + ), + package="dynamic_keypoint_tracker", + executable="track_outlier_rejector_node", + name="track_outlier_rejector", + output="screen", + parameters=[ + { + "use_sim_time": LaunchConfiguration("offline"), + "object_tracks_topic": LaunchConfiguration( + "dynamic_keypoint_object_tracks_topic" + ), + "filtered_object_tracks_topic": LaunchConfiguration( + "dynamic_keypoint_filtered_object_tracks_topic" + ), + "publish_debug_image": ParameterValue( + LaunchConfiguration("dynamic_keypoint_publish_debug_image"), + value_type=bool, + ), + "debug_image_width": ParameterValue( + LaunchConfiguration("dynamic_keypoint_debug_image_width"), + value_type=int, + ), + "debug_image_height": ParameterValue( + LaunchConfiguration("dynamic_keypoint_debug_image_height"), + value_type=int, + ), + }, + ], + ), + # Dynamic Keypoint 3D Lifter + Node( + condition=IfCondition( + LaunchConfiguration("launch_dynamic_keypoint_3d_lifter") + ), + package="dynamic_keypoint_3d_lifter", + executable="dynamic_keypoint_3d_lifter_node", + name="dynamic_keypoint_3d_lifter", + output="screen", + parameters=[ + dynamic_keypoint_3d_lifter_config, + { + "use_sim_time": LaunchConfiguration("offline"), + "object_tracks_topic": LaunchConfiguration( + "dynamic_keypoint_filtered_object_tracks_topic" + ), + "corrected_depth_topic": LaunchConfiguration( + "keyframe_depth_corrected_topic" + ), + "keyframe_pose_topic": "/vs_graphs/camera_pose", + "camera_info_topic": LaunchConfiguration("rgb_camera_info_topic"), + "settings_file": [ + get_package_share_directory("vs_graphs"), + "/config/Monocular-Inertial/", + LaunchConfiguration("sensor_config"), + ".yaml", + ], + "output_topic": LaunchConfiguration( + "dynamic_keypoint_3d_lifter_output_topic" + ), + "queue_depth": ParameterValue( + LaunchConfiguration("dynamic_keypoint_3d_lifter_queue_depth"), + value_type=int, + ), + "low_confidence_behavior": LaunchConfiguration( + "dynamic_keypoint_3d_lifter_low_confidence_behavior" + ), + "low_confidence_max_residual_m": ParameterValue( + LaunchConfiguration( + "dynamic_keypoint_3d_lifter_low_confidence_max_residual_m" + ), + value_type=float, + ), + "max_lift_distance_m": ParameterValue( + LaunchConfiguration( + "dynamic_keypoint_3d_lifter_max_lift_distance_m" + ), + value_type=float, + ), + "publish_debug_image": ParameterValue( + LaunchConfiguration( + "dynamic_keypoint_3d_lifter_publish_debug_image" + ), + value_type=bool, + ), + "debug_match_logging": ParameterValue( + LaunchConfiguration( + "dynamic_keypoint_3d_lifter_debug_match_logging" + ), + value_type=bool, + ), + }, + ], + ), + # Dynamic Keypoint Interpolator + Node( + condition=IfCondition( + LaunchConfiguration("launch_dynamic_keypoint_interpolator") + ), + package="dynamic_keypoint_3d_lifter", + executable="dynamic_keypoint_interpolator_node", + name="dynamic_keypoint_interpolator", + output="screen", + parameters=[ + dynamic_keypoint_3d_lifter_config, + { + "use_sim_time": LaunchConfiguration("offline"), + "lifted_points_topic": LaunchConfiguration( + "dynamic_keypoint_3d_lifter_output_topic" + ), + "object_tracks_topic": LaunchConfiguration( + "dynamic_keypoint_filtered_object_tracks_topic" + ), + "camera_pose_topic": "/vs_graphs/camera_pose", + "camera_info_topic": LaunchConfiguration("rgb_camera_info_topic"), + "settings_file": [ + get_package_share_directory("vs_graphs"), + "/config/Monocular-Inertial/", + LaunchConfiguration("sensor_config"), + ".yaml", + ], + "output_topic": LaunchConfiguration( + "dynamic_keypoint_interpolator_output_topic" + ), + "queue_depth": ParameterValue( + LaunchConfiguration("dynamic_keypoint_interpolator_queue_depth"), + value_type=int, + ), + "gap_timeout_ms": ParameterValue( + LaunchConfiguration("dynamic_keypoint_interpolator_gap_timeout_ms"), + value_type=int, + ), + "min_residual_path_length_px": ParameterValue( + LaunchConfiguration( + "dynamic_keypoint_interpolator_min_residual_path_length_px" + ), + value_type=float, + ), + "publish_debug_image": ParameterValue( + LaunchConfiguration( + "dynamic_keypoint_interpolator_publish_debug_image" + ), + value_type=bool, + ), + }, + ], + ), + # Object Motion Estimator + Node( + condition=IfCondition( + LaunchConfiguration("launch_object_motion_estimator") + ), + package="object_motion_estimator", + executable="object_motion_estimator_node", + name="object_motion_estimator", + output="screen", + parameters=[ + object_motion_estimator_config, + { + "use_sim_time": LaunchConfiguration("offline"), + "input_mode": LaunchConfiguration("object_motion_input_mode"), + "dynamic_object_points_topic": LaunchConfiguration( + "dynamic_keypoint_3d_lifter_output_topic" + ), + "interpolated_points_topic": LaunchConfiguration( + "dynamic_keypoint_interpolator_output_topic" + ), + "object_track_events_topic": "/object_tracks/events", + "camera_pose_topic": "/vs_graphs/camera_pose", + "camera_info_topic": LaunchConfiguration("rgb_camera_info_topic"), + "settings_file": [ + get_package_share_directory("vs_graphs"), + "/config/Monocular-Inertial/", + LaunchConfiguration("sensor_config"), + ".yaml", + ], + "image_topic": LaunchConfiguration("rgb_image_topic"), + "output_topic": LaunchConfiguration("object_motion_output_topic"), + "markers_topic": LaunchConfiguration("object_motion_markers_topic"), + "world_frame_id": LaunchConfiguration("object_motion_world_frame_id"), + "queue_depth": ParameterValue( + LaunchConfiguration("object_motion_queue_depth"), + value_type=int, + ), + "gap_timeout_ms": ParameterValue( + LaunchConfiguration("object_motion_gap_timeout_ms"), + value_type=int, + ), + "trajectory_window_size": ParameterValue( + LaunchConfiguration("object_motion_trajectory_window_size"), + value_type=int, + ), + "mad_k": ParameterValue( + LaunchConfiguration("object_motion_mad_k"), + value_type=float, + ), + "ema_alpha_h": ParameterValue( + LaunchConfiguration("object_motion_ema_alpha_h"), + value_type=float, + ), + "publish_debug_image": ParameterValue( + LaunchConfiguration("object_motion_publish_debug_image"), + value_type=bool, + ), + "publish_markers": ParameterValue( + LaunchConfiguration("object_motion_publish_markers"), + value_type=bool, + ), + "arrow_scale": ParameterValue( + LaunchConfiguration("object_motion_arrow_scale"), + value_type=float, + ), + "arrow_time_scale_s": ParameterValue( + LaunchConfiguration("object_motion_arrow_time_scale_s"), + value_type=float, + ), + "arrow_lifetime_s": ParameterValue( + LaunchConfiguration("object_motion_arrow_lifetime_s"), + value_type=float, + ), + "clear_markers_on_track_lost": ParameterValue( + LaunchConfiguration("object_motion_clear_markers_on_track_lost"), + value_type=bool, + ), + "marker_line_width_m": ParameterValue( + LaunchConfiguration("object_motion_marker_line_width_m"), + value_type=float, + ), + "marker_sphere_radius_m": ParameterValue( + LaunchConfiguration("object_motion_marker_sphere_radius_m"), + value_type=float, + ), + "marker_text_height_m": ParameterValue( + LaunchConfiguration("object_motion_marker_text_height_m"), + value_type=float, + ), + }, + ], + ), + # Object BA Tracker (Pipeline B) + Node( + condition=IfCondition( + LaunchConfiguration("launch_object_ba_tracker") + ), + package="object_motion_estimator", + executable="object_ba_tracker_node", + name="object_ba_tracker", + output="screen", + parameters=[ + object_motion_estimator_config, + { + "use_sim_time": LaunchConfiguration("offline"), + "object_tracks_topic": LaunchConfiguration( + "dynamic_keypoint_filtered_object_tracks_topic" + ), + "camera_pose_topic": "/vs_graphs/camera_pose", + "camera_info_topic": LaunchConfiguration("rgb_camera_info_topic"), + "settings_file": [ + get_package_share_directory("vs_graphs"), + "/config/Monocular-Inertial/", + LaunchConfiguration("sensor_config"), + ".yaml", + ], + "object_track_events_topic": "/object_tracks/events", + "output_topic": LaunchConfiguration("object_ba_output_topic"), + "markers_topic": LaunchConfiguration("object_ba_markers_topic"), + "world_frame_id": LaunchConfiguration("object_motion_world_frame_id"), + "queue_depth": ParameterValue( + LaunchConfiguration("object_motion_queue_depth"), + value_type=int, + ), + "window_size": ParameterValue( + LaunchConfiguration("object_ba_window_size"), + value_type=int, + ), + "min_window_frames": ParameterValue( + LaunchConfiguration("object_ba_min_window_frames"), + value_type=int, + ), + "min_point_observations": ParameterValue( + LaunchConfiguration("object_ba_min_point_observations"), + value_type=int, + ), + "sigma_pixel": ParameterValue( + LaunchConfiguration("object_ba_sigma_pixel"), + value_type=float, + ), + "huber_delta_pixels": ParameterValue( + LaunchConfiguration("object_ba_huber_delta_pixels"), + value_type=float, + ), + "num_iterations": ParameterValue( + LaunchConfiguration("object_ba_num_iterations"), + value_type=int, + ), + "ba_time_budget_ms": ParameterValue( + LaunchConfiguration("object_ba_time_budget_ms"), + value_type=float, + ), + "min_camera_displacement_m": ParameterValue( + LaunchConfiguration("object_ba_min_camera_displacement_m"), + value_type=float, + ), + "alpha_scale": ParameterValue( + LaunchConfiguration("object_ba_alpha_scale"), + value_type=float, + ), + "pose_topic_timeout_s": ParameterValue( + LaunchConfiguration("object_ba_pose_topic_timeout_s"), + value_type=float, + ), + "pose_sync_tolerance_ms": ParameterValue( + LaunchConfiguration("object_ba_pose_sync_tolerance_ms"), + value_type=float, + ), + "mad_k": ParameterValue( + LaunchConfiguration("object_ba_mad_k"), + value_type=float, + ), + "max_landmark_anchor_norm_m": ParameterValue( + LaunchConfiguration("object_ba_max_landmark_anchor_norm_m"), + value_type=float, + ), + "max_landmark_reprojection_error_px": ParameterValue( + LaunchConfiguration("object_ba_max_landmark_reprojection_error_px"), + value_type=float, + ), + "min_scale_object_displacement_m": ParameterValue( + LaunchConfiguration("object_ba_min_scale_object_displacement_m"), + value_type=float, + ), + "trajectory_window_size": ParameterValue( + LaunchConfiguration("object_motion_trajectory_window_size"), + value_type=int, + ), + "publish_markers": ParameterValue( + LaunchConfiguration("object_motion_publish_markers"), + value_type=bool, + ), + "arrow_time_scale_s": ParameterValue( + LaunchConfiguration("object_motion_arrow_time_scale_s"), + value_type=float, + ), + "arrow_lifetime_s": ParameterValue( + LaunchConfiguration("object_motion_arrow_lifetime_s"), + value_type=float, + ), + "clear_markers_on_track_lost": ParameterValue( + LaunchConfiguration("object_motion_clear_markers_on_track_lost"), + value_type=bool, + ), + "marker_line_width_m": ParameterValue( + LaunchConfiguration("object_motion_marker_line_width_m"), + value_type=float, + ), + "marker_sphere_radius_m": ParameterValue( + LaunchConfiguration("object_motion_marker_sphere_radius_m"), + value_type=float, + ), + "marker_text_height_m": ParameterValue( + LaunchConfiguration("object_motion_marker_text_height_m"), + value_type=float, + ), + }, + ], + ), + # D12 Step 2: Landmark Initializer -- replaces Pipeline BA as joint_window_node's + # landmark-seed source. Triangulation-only, no optimizer, publishes a one-shot seed + # per track_id (see landmark_initializer_node.hpp doc comment). + Node( + condition=IfCondition( + LaunchConfiguration("launch_landmark_initializer") + ), + package="object_motion_estimator", + executable="landmark_initializer_node", + name="landmark_initializer", + output="screen", + parameters=[ + { + "use_sim_time": LaunchConfiguration("offline"), + "object_tracks_topic": LaunchConfiguration( + "dynamic_keypoint_filtered_object_tracks_topic" + ), + "object_track_events_topic": "/object_tracks/events", + "camera_pose_topic": "/vs_graphs/camera_pose", + "camera_info_topic": LaunchConfiguration("rgb_camera_info_topic"), + "settings_file": [ + get_package_share_directory("vs_graphs"), + "/config/Monocular-Inertial/", + LaunchConfiguration("sensor_config"), + ".yaml", + ], + "output_topic": LaunchConfiguration("landmark_seed_topic"), + "pose_sync_tolerance_ms": ParameterValue( + LaunchConfiguration("object_ba_pose_sync_tolerance_ms"), + value_type=float, + ), + "min_frames_for_triangulation": ParameterValue( + LaunchConfiguration("landmark_min_frames_for_triangulation"), + value_type=int, + ), + "window_max_frames": ParameterValue( + LaunchConfiguration("landmark_window_max_frames"), + value_type=int, + ), + "queue_depth": ParameterValue( + LaunchConfiguration("object_motion_queue_depth"), + value_type=int, + ), + }, + ], + ), + # # Static Transforms + # Node( + # package="tf2_ros", + # name="map_to_map_elevated", # For Voxblox Skeleton + # executable="static_transform_publisher", + # arguments=["0", "0", "0", "0", "0", "0", "map", "map_elevated"], + # ), + # Node( + # name="bc_to_se", + # package="tf2_ros", + # executable="static_transform_publisher", + # arguments=["0", "0", "3", "0", "0", "0", "build_comp", "struc_elem"], + # ), + # Node( + # package="tf2_ros", + # name="world_to_bc", + # executable="static_transform_publisher", + # arguments=["0", "0", "3", "0", "0", "0", "world", "build_comp"], + # ), + # Node( + # package="tf2_ros", + # name="camera_to_imu", + # executable="static_transform_publisher", + # arguments=[ + # "-0.011739999987185001", + # "-0.005520000122487545", + # "0.005100000184029341", + # "0", + # "0", + # "0", + # "camera", + # "imu", + # ], + # ), + # Node( + # package="tf2_ros", + # name="camera_to_camera_optical", + # executable="static_transform_publisher", + # arguments=[ + # "0.00011983246804447845", + # "0.014999180100858212", + # "0.00015637179603800178", + # "-1.5730284322450263", + # "0.019080586334886032", + # "-1.574238659946616", + # "camera", + # "camera_color_optical_frame", + # # RealSense: camera_color_optical_frame, OpenLoris: d400_color + # ], + # ), + # RViz + Node( + condition=IfCondition(LaunchConfiguration("launch_rviz")), + package="rviz2", + executable="rviz2", + name="rviz", + arguments=[ + "-d", + LaunchConfiguration("rviz_config"), + ], + output="screen", + ), + # Semantic Scene Segmenter Node (based on semantic_scene_segmenter argument) + Node( + condition=IfCondition( + EqualsSubstitution( + LaunchConfiguration("semantic_scene_segmenter"), "yoso" + ) + ), + name="segmenter_ros", + package="segmenter_ros", + executable="segmenter_yoso.py", + output="screen", + parameters=[ + {"visualize": LaunchConfiguration("visualize_segmented_scene")} + ], + arguments=[ + "--ros-args", + "--params-file", + [ + get_package_share_directory("segmenter_ros"), + "/config/cfg_yoso.yaml", + ], + ], + ), + Node( + condition=IfCondition( + EqualsSubstitution( + LaunchConfiguration("semantic_scene_segmenter"), "pfcn" + ) + ), + name="segmenter_ros", + package="segmenter_ros", + executable="segmenter_pFCN.py", + output="screen", + parameters=[ + {"visualize": LaunchConfiguration("visualize_segmented_scene")} + ], + arguments=[ + "--ros-args", + "--params-file", + [ + get_package_share_directory("segmenter_ros"), + "/config/cfg_pFCN.yaml", + ], + ], + ), + Node( + condition=IfCondition( + EqualsSubstitution( + LaunchConfiguration("semantic_scene_segmenter"), "yolo26" + ) + ), + name="segmenter_ros", + package="segmenter_ros", + executable="frame_segmenter_yolo26.py", + output="screen", + parameters=[ + {"visualize": LaunchConfiguration("visualize_segmented_scene")}, + { + "params.ros_topics.raw_image_topic": LaunchConfiguration( + "rgb_image_topic" + ) + }, + ], + arguments=[ + "--ros-args", + "--params-file", + [ + get_package_share_directory("segmenter_ros"), + "/config/cfg_yolo26.yaml", + ], + ], + ), + # Structural Element Detectors + # Node( + # name="situational_graphs_reasoning", + # package="situational_graphs_reasoning", + # executable="situational_graphs_reasoning", + # output="screen", + # # parameters=[ + # # os.path.join( + # # get_package_share_directory("situational_graphs_reasoning"), + # # "config", + # # "params.yaml", + # # ) + # # ], + # # remappings=[ + # # ( + # # "situational_graphs_reasoning/graphs", + # # "/s_graphs/graph_structure", + # # ), + # # ], + # ), + ] + ) diff --git a/launch/rgbd.launch.py b/launch/rgbd.launch.py index e4e43437..04ded819 100644 --- a/launch/rgbd.launch.py +++ b/launch/rgbd.launch.py @@ -19,7 +19,7 @@ def generate_launch_description(): "semantic_scene_segmenter", default_value="yoso", description="The method to segment the semantic scene (if off, the baseline)", - choices=["yoso", "pfcn", "off"], + choices=["yoso", "pfcn", "eomt", "off"], ), # Topics DeclareLaunchArgument("camera_frame", default_value="camera"), @@ -215,6 +215,28 @@ def generate_launch_description(): ], ], ), + Node( + condition=IfCondition( + EqualsSubstitution( + LaunchConfiguration("semantic_scene_segmenter"), "eomt" + ) + ), + name="segmenter_ros", + package="segmenter_ros", + executable="segmenter_eomt.py", + output="screen", + parameters=[ + {"visualize": LaunchConfiguration("visualize_segmented_scene")} + ], + arguments=[ + "--ros-args", + "--params-file", + [ + get_package_share_directory("segmenter_ros"), + "/config/cfg_eomt.yaml", + ], + ], + ), # Structural Element Detectors # Node( # name="situational_graphs_reasoning", diff --git a/msg/ImuBiasEstimate.msg b/msg/ImuBiasEstimate.msg new file mode 100644 index 00000000..4ca64bd9 --- /dev/null +++ b/msg/ImuBiasEstimate.msg @@ -0,0 +1,7 @@ +std_msgs/Header header +float64 bax +float64 bay +float64 baz +float64 bwx +float64 bwy +float64 bwz diff --git a/msg/MapReadyEvent.msg b/msg/MapReadyEvent.msg new file mode 100644 index 00000000..b63dac7b --- /dev/null +++ b/msg/MapReadyEvent.msg @@ -0,0 +1,3 @@ +std_msgs/Header header +uint64 map_id +bool is_ready diff --git a/msg/MapRescaleEvent.msg b/msg/MapRescaleEvent.msg new file mode 100644 index 00000000..81224fb3 --- /dev/null +++ b/msg/MapRescaleEvent.msg @@ -0,0 +1,6 @@ +std_msgs/Header header +uint64 map_id +float64 scale +geometry_msgs/Quaternion rotation +geometry_msgs/Vector3 translation +uint64 rescale_count diff --git a/msg/MapResetEvent.msg b/msg/MapResetEvent.msg new file mode 100644 index 00000000..96ff8e75 --- /dev/null +++ b/msg/MapResetEvent.msg @@ -0,0 +1,4 @@ +std_msgs/Header header +uint64 previous_map_id +uint64 current_map_id +uint64 reset_count diff --git a/package.xml b/package.xml index 87915a25..75ac5bf9 100644 --- a/package.xml +++ b/package.xml @@ -25,6 +25,8 @@ rosidl_default_generators backward_ros segmenter_ros + keyframe_depth_estimator + keyframe_depth_validator rviz_visual_tools situational_graphs_msgs rosidl_default_runtime @@ -37,4 +39,4 @@ ament_cmake - \ No newline at end of file + diff --git a/src/common.cc b/src/common.cc index 8103cb27..528cba56 100644 --- a/src/common.cc +++ b/src/common.cc @@ -21,6 +21,9 @@ */ #include "common.h" +#include +#include +#include #include // Variables for ORB-SLAM3 @@ -43,13 +46,17 @@ std::vector> skeletonClusterPoints; std::shared_ptr staticTfBroadcaster; std::string frameWorld, frameCamera, frameImu, frameMap, frameBC, frameSE; -rclcpp::Time lastPlanePublishTime(0, 0, RCL_ROS_TIME); +double lastPlanePublishTime = -std::numeric_limits::infinity(); rclcpp::Publisher::SharedPtr pubKeyFrameList; rclcpp::Publisher::SharedPtr pubOdometry; +rclcpp::Publisher::SharedPtr pubImuBias; rclcpp::Publisher::SharedPtr pubDoor; rclcpp::Publisher::SharedPtr pubAllMappoints; rclcpp::Publisher::SharedPtr pubCameraPose; rclcpp::Publisher::SharedPtr pubKFImage; +rclcpp::Publisher::SharedPtr pubKeyFrameCreated; +rclcpp::Publisher::SharedPtr pubKeyFrameStaticMapPoints; +rclcpp::Publisher::SharedPtr pubStaticMapPoints; rclcpp::Publisher::SharedPtr pubBuildingComponents; rclcpp::Publisher::SharedPtr pubTrackedMappoints; rclcpp::Publisher::SharedPtr pubFreespaceCluster; @@ -62,6 +69,42 @@ rclcpp::Publisher::SharedPtr pubKeyFrameMa rclcpp::Publisher::SharedPtr pubFiducialMarker; rclcpp::Publisher::SharedPtr pubStructuralElements; rclcpp::Publisher::SharedPtr pubAllWalls_legacy; +rclcpp::Publisher::SharedPtr pubMapReset; +rclcpp::Publisher::SharedPtr pubMapReady; +rclcpp::Publisher::SharedPtr pubMapRescale; + +// Marker ids published in the previous cycle, keyed by RViz marker namespace. An entity stops +// appearing in the SLAM system's GetAllX() lists either because it was individually pruned, or +// because its owning map is no longer the active one (e.g. after a tracking-loss reset) -- in +// both cases the corresponding marker(s) must be explicitly DELETEd, since marker.lifetime is 0 +// (forever) and RViz otherwise keeps drawing them indefinitely. +std::unordered_map> lastPublishedMarkerIds; + +// Appends DELETE markers for any id previously published under `ns` that is absent from +// `currentIds` this cycle, then records `currentIds` as the new baseline for `ns`. +void appendDeletedMarkers( + visualization_msgs::msg::MarkerArray &markerArray, + const std::string &ns, + const std::set ¤tIds, + const std::string &frameId, + const rclcpp::Time &stamp) +{ + std::set &previousIds = lastPublishedMarkerIds[ns]; + for (int id : previousIds) + { + if (currentIds.count(id)) + continue; + + visualization_msgs::msg::Marker deleteMarker; + deleteMarker.ns = ns; + deleteMarker.id = id; + deleteMarker.header.frame_id = frameId; + deleteMarker.header.stamp = stamp; + deleteMarker.action = visualization_msgs::msg::Marker::DELETE; + markerArray.markers.push_back(deleteMarker); + } + previousIds = currentIds; +} void saveMapService( std::shared_ptr req, @@ -125,6 +168,83 @@ void setupServices(std::shared_ptr node, const std::string &node_n node_name + "/save_traj", &saveTrajectoryService); } +void publishKeyFrameCreatedEvent(const ORB_SLAM3::KeyFrame *keyframe) +{ + if (!pubKeyFrameCreated || keyframe == nullptr || keyframe->mImage.empty()) + return; + + try + { + cv::Mat keyframeImageBgr; + if (keyframe->mImage.channels() == 1) + cv::cvtColor(keyframe->mImage, keyframeImageBgr, cv::COLOR_GRAY2BGR); + else if (keyframe->mImage.channels() == 4) + cv::cvtColor(keyframe->mImage, keyframeImageBgr, cv::COLOR_BGRA2BGR); + else if (keyframe->mImage.channels() == 3) + keyframeImageBgr = keyframe->mImage; + else + return; + + std_msgs::msg::Header header; + header.stamp = rclcpp::Time(static_cast(keyframe->mTimeStamp * 1e9)); + header.frame_id = frameCamera; + + keyframe_depth_estimator::msg::KeyFrameCreated event; + event.header = header; + event.keyframe_id = keyframe->mnId; + auto image_msg = cv_bridge::CvImage( + header, sensor_msgs::image_encodings::BGR8, keyframeImageBgr).toImageMsg(); + event.image = *image_msg; + pubKeyFrameCreated->publish(event); + + if (pubKeyFrameStaticMapPoints) + { + keyframe_depth_validator::msg::StaticMapPointCorrespondences points_msg; + points_msg.header = header; + points_msg.keyframe_id = keyframe->mnId; + + ORB_SLAM3::KeyFrame *mutable_keyframe = const_cast(keyframe); + const std::vector map_points = + mutable_keyframe->GetMapPointMatches(); + const Sophus::SE3f Tcw = mutable_keyframe->GetPose(); + points_msg.u.reserve(map_points.size()); + points_msg.v.reserve(map_points.size()); + points_msg.z_orb.reserve(map_points.size()); + points_msg.map_point_ids.reserve(map_points.size()); + + for (std::size_t i = 0; i < map_points.size() && i < keyframe->mvKeysUn.size(); ++i) + { + ORB_SLAM3::MapPoint *map_point = map_points[i]; + if (map_point == nullptr || map_point->isBad()) + continue; + + const cv::Point2f pixel = keyframe->mvKeysUn[i].pt; + if (pixel.x < 0.0F || pixel.y < 0.0F || + pixel.x >= static_cast(keyframeImageBgr.cols) || + pixel.y >= static_cast(keyframeImageBgr.rows)) + continue; + + const Eigen::Vector3f Xc = Tcw * map_point->GetWorldPos(); + if (!std::isfinite(Xc.z()) || Xc.z() <= 0.0F) + continue; + + points_msg.u.push_back(pixel.x); + points_msg.v.push_back(pixel.y); + points_msg.z_orb.push_back(Xc.z()); + points_msg.map_point_ids.push_back(static_cast(map_point->mnId)); + } + + pubKeyFrameStaticMapPoints->publish(points_msg); + } + } + catch (const std::exception &e) + { + RCLCPP_ERROR(rclcpp::get_logger("visual_sgraphs"), + "Failed to publish KeyFrameCreated event for keyframe %lu: %s", + keyframe->mnId, e.what()); + } +} + void setupPublishers(std::shared_ptr node, std::shared_ptr image_transport, const std::string &node_name) { // Basic @@ -132,6 +252,15 @@ void setupPublishers(std::shared_ptr node, std::shared_ptrcreate_publisher(node_name + "/all_points", 1); pubCameraPose = node->create_publisher(node_name + "/camera_pose", 1); pubKFImage = node->create_publisher(node_name + "/keyframe_image", 50); + pubKeyFrameCreated = + node->create_publisher( + "/orbslam3/keyframe_created", 10); + pubKeyFrameStaticMapPoints = + node->create_publisher( + "/orbslam3/keyframe_static_map_points", 10); + pubStaticMapPoints = + node->create_publisher( + "/orbslam3/static_map_points", 10); pubTrackedMappoints = node->create_publisher(node_name + "/tracked_points", 1); pubWorldFramePointCloud = node->create_publisher(node_name + "/points_map", 1); pubKeyFrameMarker = node->create_publisher(node_name + "/kf_markers", 1); @@ -155,17 +284,161 @@ void setupPublishers(std::shared_ptr node, std::shared_ptrcreate_publisher(node_name + "/structural_elements", 1); + // Map reset notifications, for downstream nodes that keep their own per-map state + pubMapReset = node->create_publisher(node_name + "/map_reset", 1); + + // Map readiness (scale/IMU initialized), so downstream nodes know when it's safe to start + // creating entities/tracks. Unlike the other publishers here, this uses transient_local + // ("latched") durability: readiness is a level, not a pure edge, so a node that (re)connects + // later must learn the current state immediately instead of waiting for the next transition. + pubMapReady = node->create_publisher( + node_name + "/map_ready", rclcpp::QoS(1).transient_local()); + + // Map rescale notifications (scale/gravity refinement, or a map merge), so downstream nodes + // that buffer their own world-frame positions/poses can apply the same correction instead of + // going stale relative to the (now rescaled) map. + pubMapRescale = node->create_publisher(node_name + "/map_rescale", 1); + // Get body odometry if IMU data is also available if (sensorType == ORB_SLAM3::System::IMU_MONOCULAR || sensorType == ORB_SLAM3::System::IMU_STEREO || sensorType == ORB_SLAM3::System::IMU_RGBD) + { pubOdometry = node->create_publisher(node_name + "/body_odom", 1); + // Latest accel/gyro bias estimate, published alongside body_odom so downstream nodes + // that build their own IMU::Preintegrated factors (rather than reimplementing Forster + // preintegration) have the bias value ORB-SLAM3's own inertial edges are using. + pubImuBias = node->create_publisher(node_name + "/imu_bias", 1); + } + + if (pSLAM) + pSLAM->SetKeyFrameCreatedCallback(publishKeyFrameCreatedEvent); tfBuffer_ = std::make_shared(node->get_clock()); tfListener_ = std::make_shared(*tfBuffer_); } +// Announces when the current active map changes (tracking-loss reset, new map created), so that +// downstream nodes keeping their own per-map/per-track state (e.g. object_motion_estimator) can +// clear it instead of silently mixing data across two unrelated coordinate frames. Detecting this +// needs no core changes: Map::GetId() is already a stable, globally unique, monotonically +// increasing id assigned once per Map object. +void publishMapResetIfChanged(rclcpp::Time msgTime) +{ + static uint64_t lastMapId = std::numeric_limits::max(); // sentinel: nothing seen yet + static uint64_t resetCount = 0; + + uint64_t currentMapId = pSLAM->GetCurrentMap()->GetId(); + if (currentMapId == lastMapId) + return; + + // Only the transition to a *different already-seen-once* map is a reset; the very first map + // observed at startup is not. + if (lastMapId != std::numeric_limits::max()) + { + vs_graphs::msg::MapResetEvent event; + event.header.stamp = msgTime; + event.header.frame_id = frameWorld; + event.previous_map_id = lastMapId; + event.current_map_id = currentMapId; + event.reset_count = ++resetCount; + pubMapReset->publish(event); + } + lastMapId = currentMapId; +} + +// Announces whether the current map is safe to create entities/tracks against -- non-inertial +// sensors are always ready once a map exists (no separate scale phase); inertial sensors aren't +// ready until GetIniertialBA2() (scale/gravity actually converged, not just the earliest +// bias-only isImuInitialized() stage -- mirrors the core-side guard in Atlas::AddMapDoor etc. and +// the existing precedent for this exact pairing in Tracking.cc). Publishes once per map (whatever +// the state is at first sight) and again only on each actual true/false flip thereafter. +void publishMapReadyIfChanged(rclcpp::Time msgTime) +{ + static uint64_t lastMapId = std::numeric_limits::max(); + static bool lastPublishedReady = false; + static bool havePublishedForThisMap = false; + + ORB_SLAM3::Map *pMap = pSLAM->GetCurrentMap(); + uint64_t currentMapId = pMap->GetId(); + bool currentlyReady = !pMap->IsInertial() || (pMap->isImuInitialized() && pMap->GetIniertialBA2()); + + if (currentMapId != lastMapId) + { + lastMapId = currentMapId; + havePublishedForThisMap = false; + } + if (havePublishedForThisMap && currentlyReady == lastPublishedReady) + return; + + vs_graphs::msg::MapReadyEvent event; + event.header.stamp = msgTime; + event.header.frame_id = frameWorld; + event.map_id = currentMapId; + event.is_ready = currentlyReady; + pubMapReady->publish(event); + lastPublishedReady = currentlyReady; + havePublishedForThisMap = true; +} + +// Announces the similarity correction (scale, rotation, translation) whenever +// Map::ApplyScaledRotation runs (IMU scale/gravity refinement, or a map merge) -- downstream nodes +// that buffer their own world-frame positions/poses (e.g. object_motion_estimator) apply the same +// correction to stay consistent with the now-rescaled map. Uses Map::GetScaleCorrectionVersion(), +// a dedicated counter -- unlike GetMapChangeIndex()/mnMapChange, which is bumped by many unrelated +// events (BA, loop closure, merge welding) and would give false positives here. +void publishMapRescaleIfChanged(rclcpp::Time msgTime) +{ + static uint64_t lastMapId = std::numeric_limits::max(); + static uint64_t lastVersion = 0; + + ORB_SLAM3::Map *pMap = pSLAM->GetCurrentMap(); + uint64_t currentMapId = pMap->GetId(); + if (currentMapId != lastMapId) + { + // A new map's version counter starts back at 0 -- reset our bookkeeping too, so a new + // map's first rescale isn't skipped just because its version happens to match whatever + // the previous map's counter last was. + lastMapId = currentMapId; + lastVersion = 0; + } + + uint64_t currentVersion = pMap->GetScaleCorrectionVersion(); + if (currentVersion == lastVersion) + return; + lastVersion = currentVersion; + if (currentVersion == 0) + return; // no rescale has happened yet for this map + + Sophus::SE3f T; + float s; + pMap->GetLastScaleCorrection(T, s); + + vs_graphs::msg::MapRescaleEvent event; + event.header.stamp = msgTime; + event.header.frame_id = frameWorld; + event.map_id = pMap->GetId(); + event.scale = static_cast(s); + event.rotation.x = T.unit_quaternion().x(); + event.rotation.y = T.unit_quaternion().y(); + event.rotation.z = T.unit_quaternion().z(); + event.rotation.w = T.unit_quaternion().w(); + event.translation.x = T.translation().x(); + event.translation.y = T.translation().y(); + event.translation.z = T.translation().z(); + event.rescale_count = currentVersion; + pubMapRescale->publish(event); +} + void publishTopics(rclcpp::Time msgTime, Eigen::Vector3f Wbb, const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msgPCL) { + // Detect and announce a map reset/readiness/rescale change as early as possible in the cycle, + // even if the current pose is temporarily invalid (NaN, handled below) -- downstream consumers + // need to know their world-frame assumptions broke (or that it's now safe to resume, or that a + // correction needs applying) right away, not only once tracking recovers. + publishMapResetIfChanged(msgTime); + publishMapReadyIfChanged(msgTime); + publishMapRescaleIfChanged(msgTime); + Sophus::SE3f Twc = pSLAM->GetCamTwc(); // Avoid publishing NaN @@ -174,6 +447,7 @@ void publishTopics(rclcpp::Time msgTime, Eigen::Vector3f Wbb, const sensor_msgs: // Common topics publishCameraPose(Twc, msgTime); + publishStaticMapPoints(pSLAM->GetTrackedMapPoints(), msgTime); publishTFTransform(Twc, frameWorld, frameCamera, msgTime); publishFramePointCloud(Twc, msgPCL, msgTime); @@ -221,6 +495,7 @@ void publishTopics(rclcpp::Time msgTime, Eigen::Vector3f Wbb, const sensor_msgs: publishTFTransform(Twb, frameWorld, frameImu, msgTime); publishBodyOdometry(Twb, Vwb, Wwb, msgTime); + publishImuBias(pSLAM->GetImuBias(), msgTime); } } @@ -251,6 +526,22 @@ void publishBodyOdometry(Sophus::SE3f Twb_SE3f, Eigen::Vector3f Vwb_E3f, Eigen:: pubOdometry->publish(odom_msg); } +void publishImuBias(const ORB_SLAM3::IMU::Bias &bias, rclcpp::Time msgTime) +{ + vs_graphs::msg::ImuBiasEstimate bias_msg; + bias_msg.header.frame_id = frameImu; + bias_msg.header.stamp = msgTime; + + bias_msg.bax = bias.bax; + bias_msg.bay = bias.bay; + bias_msg.baz = bias.baz; + bias_msg.bwx = bias.bwx; + bias_msg.bwy = bias.bwy; + bias_msg.bwz = bias.bwz; + + pubImuBias->publish(bias_msg); +} + void publishCameraPose(Sophus::SE3f Tcw_SE3f, rclcpp::Time msgTime) { geometry_msgs::msg::PoseStamped poseMsg; @@ -415,6 +706,9 @@ void publishKeyFrameImages(std::vector keyframe_vec, rclc if (keyframe->isPublished) continue; + if (keyframe->mImage.empty()) + continue; + // Create an object of VSGraphDataMsg segmenter_ros::msg::VSGraphDataMsg vsGraphPublisher = segmenter_ros::msg::VSGraphDataMsg(); std_msgs::msg::Header header; @@ -422,8 +716,18 @@ void publishKeyFrameImages(std::vector keyframe_vec, rclc header.frame_id = frameWorld; std_msgs::msg::UInt64 kfId; kfId.data = keyframe->mnId; + cv::Mat keyframeImageBgr; + if (keyframe->mImage.channels() == 1) + cv::cvtColor(keyframe->mImage, keyframeImageBgr, cv::COLOR_GRAY2BGR); + else if (keyframe->mImage.channels() == 4) + cv::cvtColor(keyframe->mImage, keyframeImageBgr, cv::COLOR_BGRA2BGR); + else if (keyframe->mImage.channels() == 3) + keyframeImageBgr = keyframe->mImage; + else + continue; + const sensor_msgs::msg::Image::SharedPtr rendered_image_msg = - cv_bridge::CvImage(header, "bgr8", keyframe->mImage).toImageMsg(); + cv_bridge::CvImage(header, sensor_msgs::image_encodings::BGR8, keyframeImageBgr).toImageMsg(); vsGraphPublisher.header = header; vsGraphPublisher.key_frame_id = kfId; @@ -596,6 +900,54 @@ void publishTrackedPoints(std::vector trackedMapPoints, r pubTrackedMappoints->publish(cloud); } +void publishStaticMapPoints(std::vector trackedMapPoints, rclcpp::Time msgTime) +{ + if (!pubStaticMapPoints) + return; + + keyframe_depth_validator::msg::StaticMapPoints points_msg; + points_msg.header.stamp = msgTime; + points_msg.header.frame_id = frameWorld; + + const std::vector &trackedKeyPointsUn = pSLAM->GetTrackedKeyPointsUn(); + const Sophus::SE3f Tcw = pSLAM->GetCamTwc().inverse(); + + const std::size_t count = std::min(trackedMapPoints.size(), trackedKeyPointsUn.size()); + points_msg.point_ids.reserve(count); + points_msg.positions_world.reserve(count); + points_msg.pixels.reserve(count); + points_msg.num_observations.reserve(count); + + for (std::size_t i = 0; i < count; ++i) + { + ORB_SLAM3::MapPoint *map_point = trackedMapPoints[i]; + if (map_point == nullptr || map_point->isBad()) + continue; + + const Eigen::Vector3f pos_world = map_point->GetWorldPos(); + const Eigen::Vector3f p_cam = Tcw * pos_world; + if (!std::isfinite(p_cam.z()) || p_cam.z() <= 0.0F) + continue; + + geometry_msgs::msg::Point p_world_msg; + p_world_msg.x = static_cast(pos_world.x()); + p_world_msg.y = static_cast(pos_world.y()); + p_world_msg.z = static_cast(pos_world.z()); + + geometry_msgs::msg::Point p_pixel_msg; + p_pixel_msg.x = static_cast(trackedKeyPointsUn[i].pt.x); + p_pixel_msg.y = static_cast(trackedKeyPointsUn[i].pt.y); + p_pixel_msg.z = 0.0; + + points_msg.point_ids.push_back(static_cast(map_point->mnId)); + points_msg.positions_world.push_back(p_world_msg); + points_msg.pixels.push_back(p_pixel_msg); + points_msg.num_observations.push_back(static_cast(map_point->Observations())); + } + + pubStaticMapPoints->publish(points_msg); +} + void publishFramePointCloud(Sophus::SE3f Twc, const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msgPCL, rclcpp::Time msgTime) { if (!msgPCL) @@ -628,13 +980,16 @@ void publishAllPoints(std::vector allMapPoints, rclcpp::T void publishKeyFrameMarkers(std::vector keyframe_vec, rclcpp::Time msgTime) { sort(keyframe_vec.begin(), keyframe_vec.end(), ORB_SLAM3::KeyFrame::lId); - if (keyframe_vec.size() == 0) - return; + // kf_markers is a single SPHERE_LIST marker (fixed id) whose points are fully replaced + // below, so an empty keyframe_vec (e.g. right after a map reset) must still be published + // -- with an empty points list -- rather than skipped, otherwise the previous map's whole + // trajectory marker is left stuck on screen forever (marker lifetime is 0). visualization_msgs::msg::MarkerArray markerArray; visualization_msgs::msg::Marker kf_markers; kf_markers.header.frame_id = frameWorld; + kf_markers.header.stamp = msgTime; kf_markers.ns = "kf_markers"; kf_markers.type = visualization_msgs::msg::Marker::SPHERE_LIST; kf_markers.action = visualization_msgs::msg::Marker::ADD; @@ -698,17 +1053,17 @@ void publishKeyFrameMarkers(std::vector keyframe_vec, rcl void publishFiducialMarkers(std::vector markers, rclcpp::Time msgTime) { - int numMarkers = markers.size(); - if (numMarkers == 0) - return; - visualization_msgs::msg::MarkerArray markerArray; - markerArray.markers.resize(numMarkers); + std::set currentIds; + rclcpp::Time now = rclcpp::Clock().now(); // rclcpp::Time().now(); - for (int idx = 0; idx < numMarkers; idx++) + for (auto *marker : markers) { + int id = marker->getId(); + currentIds.insert(id); + visualization_msgs::msg::Marker fiducial_marker; - Sophus::SE3f markerPose = markers[idx]->getGlobalPose(); + Sophus::SE3f markerPose = marker->getGlobalPose(); fiducial_marker.color.a = 0; fiducial_marker.scale.x = 0.2; @@ -717,8 +1072,8 @@ void publishFiducialMarkers(std::vector markers, rclcpp::Ti fiducial_marker.ns = "fiducial_markers"; fiducial_marker.lifetime = rclcpp::Duration::from_seconds(0); fiducial_marker.action = fiducial_marker.ADD; - fiducial_marker.id = markerArray.markers.size(); - fiducial_marker.header.stamp = rclcpp::Clock().now(); // rclcpp::Time().now(); + fiducial_marker.id = id; + fiducial_marker.header.stamp = now; fiducial_marker.mesh_use_embedded_materials = true; fiducial_marker.header.frame_id = frameBC; fiducial_marker.type = visualization_msgs::msg::Marker::MESH_RESOURCE; @@ -736,23 +1091,23 @@ void publishFiducialMarkers(std::vector markers, rclcpp::Ti markerArray.markers.push_back(fiducial_marker); } + appendDeletedMarkers(markerArray, "fiducial_markers", currentIds, frameBC, now); pubFiducialMarker->publish(markerArray); } void publishDoors(std::vector doors) { - // If there are no doors, return - int numDoors = doors.size(); - if (numDoors == 0) - return; - // Variables visualization_msgs::msg::MarkerArray doorArray; - doorArray.markers.resize(numDoors); + std::set currentIds; + rclcpp::Time now = rclcpp::Clock().now(); - for (int idx = 0; idx < numDoors; idx++) + for (auto *doorPtr : doors) { - Sophus::SE3f doorPose = doors[idx]->getGlobalPose(); + int id = doorPtr->getId(); + currentIds.insert(id); + + Sophus::SE3f doorPose = doorPtr->getGlobalPose(); visualization_msgs::msg::Marker door, doorLines, doorLabel; // Door values @@ -763,8 +1118,8 @@ void publishDoors(std::vector doors) door.scale.z = 0.5; door.action = door.ADD; door.lifetime = rclcpp::Duration::from_seconds(0); - door.id = doorArray.markers.size(); - door.header.stamp = rclcpp::Clock().now(); // rclcpp::Time().now(); + door.id = id; + door.header.stamp = now; // rclcpp::Time().now(); door.mesh_use_embedded_materials = true; door.header.frame_id = frameBC; door.type = visualization_msgs::msg::Marker::MESH_RESOURCE; @@ -792,9 +1147,9 @@ void publishDoors(std::vector doors) doorLabel.ns = "doorLabel"; doorLabel.action = doorLabel.ADD; doorLabel.lifetime = rclcpp::Duration::from_seconds(0); - doorLabel.text = doors[idx]->getName(); - doorLabel.id = doorArray.markers.size(); - doorLabel.header.stamp = rclcpp::Clock().now(); // rclcpp::Time().now(); + doorLabel.text = doorPtr->getName(); + doorLabel.id = id; + doorLabel.header.stamp = now; // rclcpp::Time().now(); doorLabel.header.frame_id = frameBC; doorLabel.pose.position.x = door.pose.position.x; doorLabel.pose.position.z = door.pose.position.z; @@ -813,15 +1168,15 @@ void publishDoors(std::vector doors) doorLines.ns = "doorLines"; doorLines.action = doorLines.ADD; doorLines.lifetime = rclcpp::Duration::from_seconds(0); - doorLines.id = doorArray.markers.size(); - doorLines.header.stamp = rclcpp::Clock().now(); // rclcpp::Time().now(); + doorLines.id = id; + doorLines.header.stamp = now; // rclcpp::Time().now(); doorLines.header.frame_id = frameBC; doorLines.type = visualization_msgs::msg::Marker::LINE_LIST; geometry_msgs::msg::Point point1; - point1.x = doors[idx]->getMarker()->getGlobalPose().translation().x(); - point1.y = doors[idx]->getMarker()->getGlobalPose().translation().y(); - point1.z = doors[idx]->getMarker()->getGlobalPose().translation().z(); + point1.x = doorPtr->getMarker()->getGlobalPose().translation().x(); + point1.y = doorPtr->getMarker()->getGlobalPose().translation().y(); + point1.z = doorPtr->getMarker()->getGlobalPose().translation().z(); doorLines.points.push_back(point1); geometry_msgs::msg::Point point2; @@ -833,25 +1188,23 @@ void publishDoors(std::vector doors) doorArray.markers.push_back(doorLines); } + appendDeletedMarkers(doorArray, "doors", currentIds, frameBC, now); + appendDeletedMarkers(doorArray, "doorLabel", currentIds, frameBC, now); + appendDeletedMarkers(doorArray, "doorLines", currentIds, frameBC, now); pubDoor->publish(doorArray); } void publishPlanes(std::vector planes, rclcpp::Time msgTime) { - // Publish the planes, if any - int numPlanes = planes.size(); - if (numPlanes == 0) - return; - // Check if sufficient time has passed since the last plane publication - if ((msgTime - lastPlanePublishTime).seconds() < 3.0) + if (msgTime.seconds() - lastPlanePublishTime < 3.0) return; - lastPlanePublishTime = msgTime; + lastPlanePublishTime = msgTime.seconds(); // Variables visualization_msgs::msg::MarkerArray planeLabelArray; - planeLabelArray.markers.resize(numPlanes); + std::set currentPlaneIds; visualization_msgs::msg::Marker planeLabel, planeNormal; geometry_msgs::msg::Point normalStartPoint, normalEndPoint; @@ -867,7 +1220,8 @@ void publishPlanes(std::vector planes, rclcpp::Time msgTime) Eigen::Vector3d normal = plane->getGlobalEquation().normal(); const std::string planeLabelText = "Plane#" + std::to_string(plane->getId()); - // If the plane is undefined, skip it + // If the plane is undefined, skip it (it is left out of currentPlaneIds, so any + // previously-published label/normal for it is auto-deleted by appendDeletedMarkers) if (plane->getPlaneType() == ORB_SLAM3::Plane::planeVariant::UNDEFINED) continue; @@ -876,6 +1230,8 @@ void publishPlanes(std::vector planes, rclcpp::Time msgTime) if (planeClouds == nullptr || planeClouds->empty()) continue; + currentPlaneIds.insert(plane->getId()); + for (const auto &point : planeClouds->points) { pcl::PointXYZRGB newPoint; @@ -927,7 +1283,7 @@ void publishPlanes(std::vector planes, rclcpp::Time msgTime) planeNormal.color.r = color[0] / 255.0; planeNormal.color.g = color[1] / 255.0; planeNormal.color.b = color[2] / 255.0; - planeNormal.id = plane->getId() + numPlanes; + planeNormal.id = plane->getId(); planeNormal.lifetime = rclcpp::Duration::from_seconds(0); planeNormal.type = visualization_msgs::msg::Marker::ARROW; @@ -950,6 +1306,10 @@ void publishPlanes(std::vector planes, rclcpp::Time msgTime) planeLabelArray.markers.push_back(planeNormal); } + appendDeletedMarkers(planeLabelArray, "planeLabel", currentPlaneIds, frameBC, msgTime); + appendDeletedMarkers(planeLabelArray, "planeNormal", currentPlaneIds, frameBC, msgTime); + pubPlaneLabel->publish(planeLabelArray); + if (aggregatedCloud->empty()) return; @@ -963,63 +1323,33 @@ void publishPlanes(std::vector planes, rclcpp::Time msgTime) // Publish the point cloud pubBuildingComponents->publish(cloudMsg); - pubPlaneLabel->publish(planeLabelArray); } void publishStructuralElements(std::vector rooms, std::vector floors, rclcpp::Time msgTime) { - // Publish rooms, if any int numRooms = rooms.size(); int numFloors = floors.size(); - // If there are no rooms or floors, return - if (numRooms == 0 && numFloors == 0) - return; - // Variables double textOffset = -0.5; double floorToRoomOffset = -2.0; // Visualization markers visualization_msgs::msg::MarkerArray roomArray, floorArray; - roomArray.markers.resize(numRooms); - floorArray.markers.resize(numFloors); + std::set currentRoomIds, currentFloorIds; // Publish rooms, if any - if (numRooms > 0) + for (int idx = 0; idx < numRooms; idx++) { - for (int idx = 0; idx < numRooms; idx++) - { - // Skip if the room is bad - if (rooms[idx]->isBad()) { - // Variables - visualization_msgs::msg::Marker delRoom, delRoomLabel, delRoomWallLine; - // Delete previous marker for this room - delRoom.id = idx; - delRoom.ns = "room"; - delRoom.header.stamp = msgTime; - delRoom.header.frame_id = frameSE; - delRoom.action = visualization_msgs::msg::Marker::DELETE; - // Delete previous marker for this room label - delRoomLabel.id = idx; - delRoomLabel.ns = "roomLabel"; - delRoomLabel.header.stamp = msgTime; - delRoomLabel.header.frame_id = frameSE; - delRoomLabel.action = visualization_msgs::msg::Marker::DELETE; - // Delete previous room-wall lines - delRoomWallLine.id = idx; - delRoomWallLine.ns = "roomWallLine"; - delRoomWallLine.header.stamp = msgTime; - delRoomWallLine.header.frame_id = frameWorld; - delRoomWallLine.action = visualization_msgs::msg::Marker::DELETE; - // Push the delete markers and skip them - roomArray.markers.push_back(delRoom); - roomArray.markers.push_back(delRoomLabel); - roomArray.markers.push_back(delRoomWallLine); + // A bad room is simply left out of currentRoomIds below, so it is + // auto-deleted by appendDeletedMarkers once it drops out of the room list + if (rooms[idx]->isBad()) continue; - } - + + int id = rooms[idx]->getId(); + currentRoomIds.insert(id); + // Variables std::string roomName = rooms[idx]->getName(); geometry_msgs::msg::PointStamped roomPoint, roomPointTr; @@ -1035,7 +1365,7 @@ void publishStructuralElements(std::vector rooms, visualization_msgs::msg::Marker room, roomWallLine, roomDoorLine, roomMarkerLine, roomLabel; // Room values - room.id = idx; + room.id = id; room.ns = "room"; room.scale.x = 0.3; room.scale.y = 0.3; @@ -1060,7 +1390,7 @@ void publishStructuralElements(std::vector rooms, roomArray.markers.push_back(room); // Room label (name) - roomLabel.id = idx; + roomLabel.id = id; roomLabel.color.a = 1; roomLabel.color.r = 0; roomLabel.color.g = 0; @@ -1079,7 +1409,7 @@ void publishStructuralElements(std::vector rooms, roomArray.markers.push_back(roomLabel); // Room to Wall connection line - roomWallLine.id = idx; + roomWallLine.id = id; roomWallLine.color.a = 0.9; roomWallLine.color.r = 0.0; roomWallLine.color.g = 0.0; @@ -1159,11 +1489,12 @@ void publishStructuralElements(std::vector rooms, roomArray.markers.push_back(roomWallLine); } - pubStructuralElements->publish(roomArray); - } + appendDeletedMarkers(roomArray, "room", currentRoomIds, frameSE, msgTime); + appendDeletedMarkers(roomArray, "roomLabel", currentRoomIds, frameSE, msgTime); + appendDeletedMarkers(roomArray, "roomWallLine", currentRoomIds, frameWorld, msgTime); + pubStructuralElements->publish(roomArray); // Publish floors, if any - if (numFloors > 0) { // Variables std::vector color = {0.3, 0.6, 0.7}; @@ -1171,10 +1502,14 @@ void publishStructuralElements(std::vector rooms, // Loop through all the floors for (int floorId = 0; floorId < numFloors; floorId++) { - // If the floor has no rooms, skip it + // If the floor has no rooms, skip it, so it is auto-deleted by appendDeletedMarkers + // below once it drops out of currentFloorIds if (floors[floorId]->getRooms().size() == 0) continue; - + + int id = floors[floorId]->getId(); + currentFloorIds.insert(id); + // Variables std::string floorName = floors[floorId]->getName(); geometry_msgs::msg::PointStamped floorPoint, floorPointTr; @@ -1182,7 +1517,7 @@ void publishStructuralElements(std::vector rooms, visualization_msgs::msg::Marker floorMarker, floorLabel, floorRoomLine; // Floor marker (cube) - floorMarker.id = floorId; + floorMarker.id = id; floorMarker.ns = "floors"; floorMarker.scale.x = 0.4; floorMarker.scale.y = 0.4; @@ -1210,7 +1545,7 @@ void publishStructuralElements(std::vector rooms, floorLabel.color.g = 0; floorLabel.color.b = 0; floorLabel.scale.z = 0.2; - floorLabel.id = floorId; + floorLabel.id = id; floorLabel.text = floorName; floorLabel.ns = "floorLabels"; floorLabel.header.stamp = msgTime; @@ -1235,7 +1570,7 @@ void publishStructuralElements(std::vector rooms, } // Floor to Room connection line - floorRoomLine.id = floorId; + floorRoomLine.id = id; floorRoomLine.color.a = 0.9; floorRoomLine.color.r = 0.0; floorRoomLine.color.g = 0.0; @@ -1312,9 +1647,12 @@ void publishStructuralElements(std::vector rooms, floorArray.markers.push_back(floorMarker); floorArray.markers.push_back(floorRoomLine); } - - pubStructuralElements->publish(floorArray); } + + appendDeletedMarkers(floorArray, "floors", currentFloorIds, frameSE, msgTime); + appendDeletedMarkers(floorArray, "floorLabels", currentFloorIds, frameSE, msgTime); + appendDeletedMarkers(floorArray, "floorRoomEdges", currentFloorIds, frameWorld, msgTime); + pubStructuralElements->publish(floorArray); } sensor_msgs::msg::PointCloud2 mapPointToPointcloud(std::vector mapPoints, rclcpp::Time msgTime) @@ -1555,4 +1893,4 @@ void setGNNBasedRoomCandidates(const vs_graphs::msg::VSGraphsAllDetectdetRooms & // [TODO] Define a 'setGNNRoomCandidates' in System.h pSLAM->setGNNRoomCandidates(gnnRoomCandidates); -} \ No newline at end of file +} diff --git a/src/ros_mono_inertial.cc b/src/ros_mono_inertial.cc index 58d329dc..a446f405 100644 --- a/src/ros_mono_inertial.cc +++ b/src/ros_mono_inertial.cc @@ -11,277 +11,978 @@ * This file is part of vS-Graphs, which is free software: you can redistribute it * and/or modify it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or (at your option) any later version. - * - * vS-Graphs is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS - * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along with this program. - * If not, see . */ #include "common.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + using namespace std; -class ImuGrabber +namespace { -public: - ImuGrabber() {}; +constexpr double kMaxImuGap = 0.5; +constexpr double kMaxFrameImuDt = 0.1; +constexpr double kDefaultInstanceMaskTimeTolerance = 0.03; +constexpr double kMinOrbMaskAllowedRatio = 0.05; +constexpr float kMaxAccelNorm = 150.0f; +constexpr float kMaxGyroNorm = 50.0f; + +bool isFiniteVector(const Eigen::Vector3f &v) +{ + return std::isfinite(v.x()) && std::isfinite(v.y()) && std::isfinite(v.z()); +} +} - void GrabImu(const sensor_msgs::ImuConstPtr &imu_msg); +// --------------------------------------------------------------------------- +// Helpers +// --------------------------------------------------------------------------- - std::mutex mBufMutex; - queue imuBuf; -}; +/** + * Linearly interpolate a single accel sample to @target_time +*/ +static ORB_SLAM3::IMU::Point interpolateAccel( + const double target_time, + const Eigen::Vector3f &cur_acc, const double cur_t, + const Eigen::Vector3f &prev_acc, const double prev_t, + const Eigen::Vector3f &gyr) +{ + Eigen::Vector3f acc; + + if (prev_t == 0.0) + { + acc = cur_acc; + } + else if (target_time >= cur_t) + { + acc = cur_acc; + } + else if (target_time > prev_t) + { + const double factor = (target_time - prev_t) / (cur_t - prev_t); + acc = prev_acc + static_cast(factor) * (cur_acc - prev_acc); + } + else + { + acc = prev_acc; + } -class ImageGrabber + return ORB_SLAM3::IMU::Point( + acc.x(), acc.y(), acc.z(), + gyr.x(), gyr.y(), gyr.z(), + target_time); +} + +// --------------------------------------------------------------------------- +// Shared IMU + Image state +// --------------------------------------------------------------------------- + +struct SharedState { -public: - ImageGrabber(ImuGrabber *pImuGb) : mpImuGb(pImuGb) {} + std::mutex mtx; + std::condition_variable image_ready_cv; - void SyncWithImu(); + struct ImagePacket + { + cv::Mat image; + double timestamp = -1.0; + cv::Mat instance_mask; + double instance_mask_time_diff = std::numeric_limits::max(); + double min_marker_time_diff = std::numeric_limits::max(); + std::vector matched_markers; + }; + + struct AuxDepthFrame + { + rclcpp::Time stamp; + double timestamp_sec = 0.0; + std::string frame_id; + sensor_msgs::msg::Image::ConstSharedPtr msg; + }; + + struct InstanceMaskFrame + { + rclcpp::Time stamp; + double timestamp_sec = 0.0; + std::string frame_id; + cv::Mat mask; + }; + + // Raw gyro — collected at ~200 Hz + std::deque gyro_timestamps; + std::deque gyro_data; + + // Accel interpolated to gyro timestamps + std::deque accel_timestamps_sync; + std::deque accel_data_sync; + + // Running accel state for interpolation + Eigen::Vector3f prev_accel_data; + double prev_accel_timestamp = 0.0; + Eigen::Vector3f cur_accel_data; + double cur_accel_timestamp = 0.0; + + // Image queue keeps timestamps monotonic even if tracking briefly lags. + std::queue image_queue; + std::size_t max_image_queue_size = 300; + int dropped_frames = 0; + + std::deque aux_depth_buffer; + std::size_t max_aux_depth_buffer_size = 120; + + std::deque instance_mask_buffer; + std::size_t max_instance_mask_buffer_size = 300; + double instance_mask_time_tolerance = kDefaultInstanceMaskTimeTolerance; + + // Marker state (populated inside the image callback path) + double min_marker_time_diff = std::numeric_limits::max(); + std::vector matched_markers; +}; - void GrabImage(const sensor_msgs::ImageConstPtr &msg); - // void GrabArUcoMarker(const aruco_msgs::MarkerArray &msg); - cv::Mat GetImage(const sensor_msgs::ImageConstPtr &img_msg); - void GrabSegmentation(const segmenter_ros::SegmenterDataMsg &msgSegImage); - void GrabVoxbloxSkeletonGraph(const visualization_msgs::MarkerArray &msgSkeletonGraph); +// --------------------------------------------------------------------------- +// ImuGrabber — thin ROS2 subscriber node, pushes directly into SharedStat +// --------------------------------------------------------------------------- + +class ImuGrabber : public rclcpp::Node +{ +public: + explicit ImuGrabber(std::shared_ptr state) + : rclcpp::Node("imu_grabber", rclcpp::NodeOptions().use_global_arguments(false)) + , state_(std::move(state)) + {} - ImuGrabber *mpImuGb; - std::mutex mBufMutex; - queue img0Buf; + void GrabImu(const sensor_msgs::msg::Imu::ConstSharedPtr &msg); private: - // Marker detection - double minMarkerTimeDiff; - std::vector matchedMarkers; + std::shared_ptr state_; }; -int main(int argc, char **argv) +void ImuGrabber::GrabImu(const sensor_msgs::msg::Imu::ConstSharedPtr &msg) { - ros::init(argc, argv, "Mono_Inertial"); - ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); + const double t = rclcpp::Time(msg->header.stamp).seconds(); - if (argc > 1) - ROS_WARN("Arguments supplied via command line are ignored."); + const Eigen::Vector3f acc(msg->linear_acceleration.x, + msg->linear_acceleration.y, + msg->linear_acceleration.z); + const Eigen::Vector3f gyr(msg->angular_velocity.x, + msg->angular_velocity.y, + msg->angular_velocity.z); + + if (!std::isfinite(t) || !isFiniteVector(acc) || !isFiniteVector(gyr) || + acc.norm() > kMaxAccelNorm || gyr.norm() > kMaxGyroNorm) + { + RCLCPP_WARN(this->get_logger(), "Dropping invalid IMU sample at t=%.9f", t); + return; + } - std::string node_name = ros::this_node::getName(); + std::lock_guard lock(state_->mtx); - ros::NodeHandle nodeHandler; - image_transport::ImageTransport image_transport(nodeHandler); + if (!state_->gyro_timestamps.empty()) + { + const double last_t = state_->gyro_timestamps.back(); + if (t <= last_t) + { + RCLCPP_WARN(this->get_logger(), "Dropping non-monotonic IMU sample t=%.9f after %.9f", t, last_t); + return; + } + if (t - last_t > kMaxImuGap) + { + RCLCPP_WARN(this->get_logger(), "Large IMU gap %.6f s; clearing stale IMU buffers", t - last_t); + state_->gyro_timestamps.clear(); + state_->gyro_data.clear(); + state_->accel_timestamps_sync.clear(); + state_->accel_data_sync.clear(); + state_->prev_accel_data = Eigen::Vector3f::Zero(); + state_->cur_accel_data = Eigen::Vector3f::Zero(); + state_->prev_accel_timestamp = 0.0; + state_->cur_accel_timestamp = 0.0; + } + } - std::string voc_file, settings_file, sys_params_file; - nodeHandler.param(node_name + "/sys_params_file", sys_params_file, "file_not_set"); - nodeHandler.param(node_name + "/voc_file", voc_file, "file_not_set"); - nodeHandler.param(node_name + "/settings_file", settings_file, "file_not_set"); + // --- Gyro path (high-rate, ~200 Hz) --- + // Just buffer; accel will be interpolated to these timestamps + state_->gyro_data.push_back(gyr); + state_->gyro_timestamps.push_back(t); - if (voc_file == "file_not_set" || settings_file == "file_not_set") + // --- Accel path (lower-rate, ~60 Hz or same rate depending on IMU) --- + // ROS Imu message carries both in the same message, so we treat every + // message as potentially carrying a new accel sample. We detect a new + // accel sample by checking if t > cur_accel_timestamp + if (t > state_->cur_accel_timestamp) { - ROS_ERROR("Please provide voc_file and settings_file in the launch file"); - ros::shutdown(); - return 1; + state_->prev_accel_data = state_->cur_accel_data; + state_->prev_accel_timestamp = state_->cur_accel_timestamp; + state_->cur_accel_data = acc; + state_->cur_accel_timestamp = t; } + // std::cout << "[ROS] Received IMU message at time " << t << std::endl; - if (sys_params_file == "file_not_set") + // Interpolate accel to cover any gyro timestamps not yet synced + while (state_->gyro_timestamps.size() > state_->accel_timestamps_sync.size()) { - ROS_ERROR("Please provide the YAML file containing system parameters in the launch file!"); - ros::shutdown(); - return 1; + // std::cout << "[ROS] Interpolating accel for gyro timestamp " << state_->gyro_timestamps[state_->accel_timestamps_sync.size()] << std::endl; + const int idx = static_cast(state_->accel_timestamps_sync.size()); + const double target_time = state_->gyro_timestamps[idx]; + const Eigen::Vector3f &gyro_at_idx = state_->gyro_data[idx]; + + ORB_SLAM3::IMU::Point pt = interpolateAccel( + target_time, + state_->cur_accel_data, state_->cur_accel_timestamp, + state_->prev_accel_data, state_->prev_accel_timestamp, + gyro_at_idx); + + state_->accel_data_sync.push_back(pt.a); // Eigen::Vector3f member + state_->accel_timestamps_sync.push_back(target_time); } - bool enable_pangolin; - nodeHandler.param(node_name + "/enable_pangolin", enable_pangolin, true); + state_->image_ready_cv.notify_all(); +} - nodeHandler.param(node_name + "/yaw", yaw, 0.0); - nodeHandler.param(node_name + "/roll", roll, 0.0); - nodeHandler.param(node_name + "/pitch", pitch, 0.0); +// --------------------------------------------------------------------------- +// ImageGrabber — handles image callback and the main SyncWithImu loop +// --------------------------------------------------------------------------- - nodeHandler.param(node_name + "/frame_map", frameMap, "map"); - nodeHandler.param(node_name + "/frame_imu", frameImu, "imu"); - nodeHandler.param(node_name + "/frame_camera", frameCamera, "camera"); - nodeHandler.param(node_name + "/frame_world", frameWorld, "world"); - nodeHandler.param(node_name + "/static_transform", pubStaticTransform, false); - nodeHandler.param(node_name + "/frame_building_component", frameBC, "build_comp"); - nodeHandler.param(node_name + "/frame_structural_element", frameSE, "struc_elem"); +class ImageGrabber : public rclcpp::Node +{ +public: + struct AuxDepthOptions + { + bool use_aux_depth = true; + bool encoding_is_metric = true; + double time_tolerance = 0.05; + float min_depth = 0.2f; + float max_depth = 20.0f; + int stride = 2; + std::string scale_mode = "none"; + }; + + struct SegmentOptions + { + std::string instance_mask_topic = "/camera/color/image_instance_masks"; + double instance_mask_time_tolerance = kDefaultInstanceMaskTimeTolerance; + std::size_t max_instance_mask_buffer_size = 300; + }; + + ImageGrabber( + std::shared_ptr state, + AuxDepthOptions aux_depth_options, + SegmentOptions segment_options) + : rclcpp::Node("image_grabber", rclcpp::NodeOptions().use_global_arguments(false)) + , state_(std::move(state)) + , aux_depth_options_(std::move(aux_depth_options)) + , segment_options_(std::move(segment_options)) + { + state_->max_instance_mask_buffer_size = segment_options_.max_instance_mask_buffer_size; + state_->instance_mask_time_tolerance = segment_options_.instance_mask_time_tolerance; + } - // Create SLAM system. It initializes all system threads and gets ready to process frames. - ImuGrabber imugb; - ImageGrabber igb(&imugb); - sensorType = ORB_SLAM3::System::IMU_MONOCULAR; + ~ImageGrabber() = default; - pSLAM = new ORB_SLAM3::System(voc_file, settings_file, sys_params_file, sensorType, enable_pangolin); + void GrabImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg); + void GrabAuxDepth(const sensor_msgs::msg::Image::ConstSharedPtr &msg); + void GrabInstanceMask(const sensor_msgs::msg::Image::ConstSharedPtr &msg); + void GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg &msg); + void GrabVoxbloxSkeletonGraph(const visualization_msgs::msg::MarkerArray &msg); - // Subscribe to get raw images and IMU data - ros::Subscriber sub_imu = nodeHandler.subscribe("/imu", 1000, &ImuGrabber::GrabImu, &imugb); - ros::Subscriber sub_img = nodeHandler.subscribe("/camera/image_raw", 500, &ImageGrabber::GrabImage, &igb); + // Entry point for the sync thread + void SyncWithImu(); - // Subscribe to the markers detected by `aruco_ros` library - // ros::Subscriber sub_aruco = nodeHandler.subscribe("/aruco_marker_publisher/markers", 1, - // &ImageGrabber::GrabArUcoMarker, &igb); + std::atomic mustStop{false}; +private: + bool FindClosestAuxDepth(double rgb_time, SharedState::AuxDepthFrame &depth_frame, double &dt_abs); + bool ConvertAuxDepthImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg, cv::Mat &depth_m); + + std::shared_ptr state_; + AuxDepthOptions aux_depth_options_; + SegmentOptions segment_options_; +}; + +// --------------------------------------------------------------------------- +// GrabImage +// --------------------------------------------------------------------------- + +void ImageGrabber::GrabImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg) +{ + cv_bridge::CvImageConstPtr cv_ptr; + try + { + cv_ptr = cv_bridge::toCvShare(msg); + } + catch (cv_bridge::Exception &e) + { + RCLCPP_ERROR(this->get_logger(), "[Error] cv_bridge exception: %s", e.what()); + return; + } - // Subscriber for images obtained from the Semantic Segmentater - ros::Subscriber sub_segmented_img = nodeHandler.subscribe("/camera/color/image_segment", 50, - &ImageGrabber::GrabSegmentation, &igb); + const double new_ts = rclcpp::Time(cv_ptr->header.stamp).seconds(); - // Subscriber to get the mesh from voxblox - ros::Subscriber voxblox_skeleton_mesh = nodeHandler.subscribe("/voxblox_skeletonizer/sparse_graph", 1, - &ImageGrabber::GrabVoxbloxSkeletonGraph, &igb); + // std::cout << "[ROS] Received image message at time " << new_ts << std::endl; - setupPublishers(nodeHandler, image_transport, node_name); - setupServices(nodeHandler, node_name); + // Resolve marker association for this timestamp. + auto [min_diff, markers] = findNearestMarker(new_ts); - // Syncing images with IMU - std::thread sync_thread(&ImageGrabber::SyncWithImu, &igb); + std::unique_lock lock(state_->mtx); - ros::spin(); + if (!state_->image_queue.empty() && std::abs(state_->image_queue.back().timestamp - new_ts) < 1e-3) + return; - // Stop all threads - pSLAM->Shutdown(); - ros::shutdown(); + if (state_->image_queue.size() >= state_->max_image_queue_size) + { + state_->image_queue.pop(); + state_->dropped_frames++; + } - return 0; + SharedState::ImagePacket packet; + if (cv_ptr->image.channels() == 1 || cv_ptr->image.channels() == 3 || cv_ptr->image.channels() == 4) + { + packet.image = cv_ptr->image.clone(); + } + else + { + RCLCPP_ERROR(this->get_logger(), + "[Error] Unsupported image encoding '%s' with %d channels.", + msg->encoding.c_str(), cv_ptr->image.channels()); + return; + } + packet.timestamp = new_ts; + packet.min_marker_time_diff = min_diff; + packet.matched_markers = std::move(markers); + + state_->image_queue.push(std::move(packet)); + state_->image_ready_cv.notify_all(); } -void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr &img_msg) +void ImageGrabber::GrabAuxDepth(const sensor_msgs::msg::Image::ConstSharedPtr &msg) { - mBufMutex.lock(); - if (!img0Buf.empty()) - img0Buf.pop(); - img0Buf.push(img_msg); - mBufMutex.unlock(); + if (!aux_depth_options_.use_aux_depth) + return; + + SharedState::AuxDepthFrame frame; + frame.stamp = rclcpp::Time(msg->header.stamp); + frame.timestamp_sec = frame.stamp.seconds(); + frame.frame_id = msg->header.frame_id; + frame.msg = msg; + + std::lock_guard lock(state_->mtx); + state_->aux_depth_buffer.push_back(std::move(frame)); + while (state_->aux_depth_buffer.size() > state_->max_aux_depth_buffer_size) + state_->aux_depth_buffer.pop_front(); } -cv::Mat ImageGrabber::GetImage(const sensor_msgs::ImageConstPtr &img_msg) +void ImageGrabber::GrabInstanceMask(const sensor_msgs::msg::Image::ConstSharedPtr &msg) { - // Copy the ros image message to cv::Mat. + if (!msg) + return; + cv_bridge::CvImageConstPtr cv_ptr; try { - cv_ptr = cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::MONO8); + cv_ptr = cv_bridge::toCvShare(msg); } catch (cv_bridge::Exception &e) { - ROS_ERROR("cv_bridge exception: %s", e.what()); + RCLCPP_ERROR(this->get_logger(), "[Error] Instance mask cv_bridge exception: %s", e.what()); + return; } - // Find the marker with the minimum time difference compared to the current frame - std::pair> result = - findNearestMarker(cv_ptr->header.stamp.toSec()); - minMarkerTimeDiff = result.first; - matchedMarkers = result.second; + if (cv_ptr->image.type() != CV_16UC1 && + cv_ptr->image.type() != CV_32SC1 && + cv_ptr->image.type() != CV_8UC1) + { + RCLCPP_WARN(this->get_logger(), "Instance mask: unsupported encoding '%s'", msg->encoding.c_str()); + return; + } + + SharedState::InstanceMaskFrame frame; + frame.stamp = rclcpp::Time(msg->header.stamp); + frame.timestamp_sec = frame.stamp.seconds(); + frame.frame_id = msg->header.frame_id; + frame.mask = cv_ptr->image.clone(); + + std::lock_guard lock(state_->mtx); + state_->instance_mask_buffer.push_back(std::move(frame)); + while (state_->instance_mask_buffer.size() > state_->max_instance_mask_buffer_size) + state_->instance_mask_buffer.pop_front(); + state_->image_ready_cv.notify_all(); +} + +bool ImageGrabber::ConvertAuxDepthImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg, cv::Mat &depth_m) +{ + if (!msg) + return false; + + cv_bridge::CvImageConstPtr cv_ptr; + try + { + cv_ptr = cv_bridge::toCvShare(msg); + } + catch (cv_bridge::Exception &e) + { + RCLCPP_ERROR(this->get_logger(), "[Error] AuxDepth cv_bridge exception: %s", e.what()); + return false; + } - if (cv_ptr->image.type() == 0) + bool depth_is_metric = aux_depth_options_.encoding_is_metric; + if (msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1 || cv_ptr->image.type() == CV_32FC1) { - return cv_ptr->image.clone(); + depth_m = cv_ptr->image.clone(); + } + else if (msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1 || cv_ptr->image.type() == CV_16UC1) + { + cv_ptr->image.convertTo(depth_m, CV_32FC1, 0.001); + depth_is_metric = true; } else { - std::cout << "Error type" << std::endl; - return cv_ptr->image.clone(); + RCLCPP_WARN(this->get_logger(), "AuxDepth: unsupported encoding '%s'", msg->encoding.c_str()); + return false; + } + + for (int y = 0; y < depth_m.rows; ++y) + { + float *row = depth_m.ptr(y); + for (int x = 0; x < depth_m.cols; ++x) + { + const float d = row[x]; + if (!std::isfinite(d) || + (depth_is_metric && (d < aux_depth_options_.min_depth || d > aux_depth_options_.max_depth))) + row[x] = std::numeric_limits::quiet_NaN(); + } + } + + return true; +} + +bool ImageGrabber::FindClosestAuxDepth(double rgb_time, SharedState::AuxDepthFrame &depth_frame, double &dt_abs) +{ + { + std::lock_guard lock(state_->mtx); + + if (!aux_depth_options_.use_aux_depth || state_->aux_depth_buffer.empty()) + return false; + + auto best_it = state_->aux_depth_buffer.end(); + dt_abs = std::numeric_limits::max(); + + for (auto it = state_->aux_depth_buffer.begin(); it != state_->aux_depth_buffer.end(); ++it) + { + const double dt = std::abs(rgb_time - it->timestamp_sec); + if (dt < dt_abs) + { + dt_abs = dt; + best_it = it; + } + } + + if (best_it == state_->aux_depth_buffer.end() || dt_abs > aux_depth_options_.time_tolerance) + return false; + + depth_frame.stamp = best_it->stamp; + depth_frame.timestamp_sec = best_it->timestamp_sec; + depth_frame.frame_id = best_it->frame_id; + depth_frame.msg = best_it->msg; } + + return true; } +// --------------------------------------------------------------------------- +// SyncWithImu +// --------------------------------------------------------------------------- + void ImageGrabber::SyncWithImu() { - while (1) + while (!mustStop && !pSLAM->isShutDown()) { - if (!img0Buf.empty() && !mpImuGb->imuBuf.empty()) + cv::Mat im; + cv::Mat instanceMask; + double tIm = 0.0; + rclcpp::Time msgTime; + Eigen::Vector3f Wbb = Eigen::Vector3f::Zero(); + std::vector vImuMeas; + double min_marker_diff; + std::vector matched_markers; + std::size_t ready_imu_count = 0; + + // --- Critical section: wait, copy, clear --- { - cv::Mat im; - double tIm = 0; - - tIm = img0Buf.front()->header.stamp.toSec(); - if (tIm > mpImuGb->imuBuf.back()->header.stamp.toSec()) - continue; - - this->mBufMutex.lock(); - im = GetImage(img0Buf.front()); - rclcpp::Time msg_time = img0Buf.front()->header.stamp; - img0Buf.pop(); - this->mBufMutex.unlock(); - - vector vImuMeas; - Eigen::Vector3f Wbb; - mpImuGb->mBufMutex.lock(); - if (!mpImuGb->imuBuf.empty()) + std::unique_lock lk(state_->mtx); + state_->image_ready_cv.wait(lk, [this] { + return mustStop || + (!state_->image_queue.empty() && + !state_->instance_mask_buffer.empty() && + !state_->gyro_timestamps.empty() && + state_->gyro_timestamps.back() >= state_->image_queue.front().timestamp); + }); + + if (mustStop) + break; + + // Log dropped frames + if (state_->dropped_frames > 1) + RCLCPP_WARN(this->get_logger(), "%d dropped frames", state_->dropped_frames - 1); + state_->dropped_frames = 0; + + tIm = state_->image_queue.front().timestamp; + msgTime = rclcpp::Time(static_cast(tIm * 1e9)); + + const std::size_t synced_imu_size = + std::min(state_->gyro_timestamps.size(), state_->accel_data_sync.size()); + while (ready_imu_count < synced_imu_size && + state_->gyro_timestamps[ready_imu_count] <= tIm) + { + ++ready_imu_count; + } + + if (ready_imu_count < 2) + { + state_->image_queue.pop(); + } + else { - // Load imu measurements from buffer - vImuMeas.clear(); - while (!mpImuGb->imuBuf.empty() && mpImuGb->imuBuf.front()->header.stamp.toSec() <= tIm) + // Copy next queued image in timestamp order. Do this only after + // the IMU buffer has reached this image timestamp. + SharedState::ImagePacket packet = std::move(state_->image_queue.front()); + state_->image_queue.pop(); + + im = std::move(packet.image); + + auto best_mask_it = state_->instance_mask_buffer.end(); + double best_mask_dt = std::numeric_limits::max(); + while (!state_->instance_mask_buffer.empty() && + state_->instance_mask_buffer.front().timestamp_sec < + tIm - state_->instance_mask_time_tolerance) { - double t = mpImuGb->imuBuf.front()->header.stamp.toSec(); - cv::Point3f acc(mpImuGb->imuBuf.front()->linear_acceleration.x, mpImuGb->imuBuf.front()->linear_acceleration.y, mpImuGb->imuBuf.front()->linear_acceleration.z); - cv::Point3f gyr(mpImuGb->imuBuf.front()->angular_velocity.x, mpImuGb->imuBuf.front()->angular_velocity.y, mpImuGb->imuBuf.front()->angular_velocity.z); - vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc, gyr, t)); - Wbb << mpImuGb->imuBuf.front()->angular_velocity.x, mpImuGb->imuBuf.front()->angular_velocity.y, mpImuGb->imuBuf.front()->angular_velocity.z; - mpImuGb->imuBuf.pop(); + state_->instance_mask_buffer.pop_front(); + } + for (auto it = state_->instance_mask_buffer.begin(); + it != state_->instance_mask_buffer.end(); ++it) + { + const double dt = std::abs(tIm - it->timestamp_sec); + if (dt < best_mask_dt) + { + best_mask_dt = dt; + best_mask_it = it; + } + } + if (best_mask_it != state_->instance_mask_buffer.end() && + best_mask_dt <= state_->instance_mask_time_tolerance) + { + instanceMask = best_mask_it->mask.clone(); + packet.instance_mask_time_diff = best_mask_dt; + state_->instance_mask_buffer.erase( + state_->instance_mask_buffer.begin(), std::next(best_mask_it)); + } + + // Copy marker state + min_marker_diff = packet.min_marker_time_diff; + matched_markers = std::move(packet.matched_markers); + + // Build IMU measurement vector from synced accel + gyro, consuming + // only samples up to this image timestamp. Future IMU samples stay + // buffered for the next frame, matching the ORB-SLAM3 ROS1 wrapper. + for (std::size_t i = 0; i < ready_imu_count; ++i) + { + const double imuTime = state_->gyro_timestamps.front(); + const Eigen::Vector3f acc = state_->accel_data_sync.front(); + const Eigen::Vector3f gyr = state_->gyro_data.front(); + + vImuMeas.emplace_back( + acc.x(), acc.y(), acc.z(), + gyr.x(), gyr.y(), gyr.z(), + imuTime); + Wbb = gyr; + + state_->gyro_data.pop_front(); + state_->gyro_timestamps.pop_front(); + state_->accel_data_sync.pop_front(); + state_->accel_timestamps_sync.pop_front(); } } - mpImuGb->mBufMutex.unlock(); + } + // Lock released — TrackMonocular runs outside lock - // ORB-SLAM3 runs in TrackMonocular() - if (minMarkerTimeDiff < 0.05) + if (im.empty()) + continue; + + if (vImuMeas.size() < 2) + { + RCLCPP_WARN(this->get_logger(), + "Skipping frame t=%.9f because vImuMeas has only %zu samples", + tIm, vImuMeas.size()); + continue; + } + if (vImuMeas.back().t > tIm) + { + RCLCPP_WARN(this->get_logger(), + "Skipping frame t=%.9f because last IMU t=%.9f is newer than image", + tIm, vImuMeas.back().t); + continue; + } + + bool invalidImuPacket = false; + for (std::size_t i = 0; i < vImuMeas.size(); ++i) + { + const ORB_SLAM3::IMU::Point &imu = vImuMeas[i]; + if (!std::isfinite(imu.t) || !isFiniteVector(imu.a) || !isFiniteVector(imu.w) || + imu.a.norm() > kMaxAccelNorm || imu.w.norm() > kMaxGyroNorm) { - Sophus::SE3f Tcw = pSLAM->TrackMonocular(im, tIm, vImuMeas, - "", matchedMarkers); - markersBuffer.clear(); + invalidImuPacket = true; + break; } - else - Sophus::SE3f Tcw = pSLAM->TrackMonocular(im, tIm, vImuMeas); - publishTopics(msg_time, Wbb); + if (i > 0) + { + const double dt = imu.t - vImuMeas[i - 1].t; + if (dt <= 0.0 || dt > kMaxFrameImuDt) + { + invalidImuPacket = true; + break; + } + } + } + if (invalidImuPacket) + { + RCLCPP_WARN(this->get_logger(), "Skipping frame t=%.9f because IMU packet is invalid", tIm); + continue; } - std::chrono::milliseconds tSleep(1); - std::this_thread::sleep_for(tSleep); - } -} + // Image scaling + const float imageScale = pSLAM->GetImageScale(); + if (imageScale != 1.f) + { + const int w = static_cast(im.cols * imageScale); + const int h = static_cast(im.rows * imageScale); + cv::resize(im, im, cv::Size(w, h)); + if (!instanceMask.empty()) + cv::resize(instanceMask, instanceMask, cv::Size(w, h), 0, 0, cv::INTER_NEAREST); + } -void ImuGrabber::GrabImu(const sensor_msgs::ImuConstPtr &imu_msg) -{ - mBufMutex.lock(); - imuBuf.push(imu_msg); - mBufMutex.unlock(); + // ORB extractor masks are CV_8UC1; non-zero pixels allow feature extraction. + cv::Mat orbMask; + if (!instanceMask.empty()) + { + cv::compare(instanceMask, 0, orbMask, cv::CMP_EQ); + const double allowedRatio = + static_cast(cv::countNonZero(orbMask)) / + static_cast(orbMask.total()); + if (allowedRatio < kMinOrbMaskAllowedRatio) + orbMask.release(); + } - return; + // Track + if (min_marker_diff < 0.05) + { + pSLAM->TrackMonocular(im, tIm, vImuMeas, "", matched_markers, orbMask); + markersBuffer.clear(); + } + else + { + pSLAM->TrackMonocular(im, tIm, vImuMeas, "", std::vector{}, orbMask); + } + publishTopics(msgTime, Wbb); + } } -// void ImageGrabber::GrabArUcoMarker(const aruco_msgs::MarkerArray &markerArray) -// { -// // Pass the visited markers to a buffer to be processed later -// // addMarkersToBuffer(markerArray); -// } - -void ImageGrabber::GrabSegmentation(const segmenter_ros::SegmenterDataMsg &msgSegImage) +void ImageGrabber::GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg &msgSegImage) { - // Fetch the segmentation results - cv_bridge::CvImageConstPtr cv_imgSeg; - uint64_t keyFrameId = msgSegImage.keyFrameId.data; + cv_bridge::CvImageConstPtr cvImgSeg; + const uint64_t keyFrameId = msgSegImage.key_frame_id.data; try { - cv_imgSeg = cv_bridge::toCvCopy(msgSegImage.segmentedImage, sensor_msgs::image_encodings::BGR8); + cvImgSeg = cv_bridge::toCvCopy( + std::make_shared(msgSegImage.segmented_image), + sensor_msgs::image_encodings::BGR8); } catch (cv_bridge::Exception &e) { - ROS_ERROR("cv_bridge exception: %s", e.what()); + RCLCPP_ERROR(this->get_logger(), "[Error] cv_bridge exception: %s", e.what()); return; } - // convert to PCL PointCloud2 from sensor_msgs PointCloud2 pcl::PCLPointCloud2::Ptr pclPc2SegPrb(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(msgSegImage.segmentedImageProbability, *pclPc2SegPrb); + pcl_conversions::toPCL(msgSegImage.segmented_image_probability, *pclPc2SegPrb); - // Create the tuple to be appended to the segmentedImageBuffer - std::tuple tuple(keyFrameId, cv_imgSeg->image, pclPc2SegPrb); + if (aux_depth_options_.use_aux_depth) + { + const double segTimestamp = rclcpp::Time(msgSegImage.header.stamp).seconds(); // Use msgSegImage timestamp (submsgs have empty timestamps) + SharedState::AuxDepthFrame auxDepthFrame; + double auxDepthDt = std::numeric_limits::max(); - // Add the segmented image to a buffer to be processed in the SemanticSegmentation thread + if (FindClosestAuxDepth(segTimestamp, auxDepthFrame, auxDepthDt)) + { + cv::Mat auxDepthForSeg; + if (ConvertAuxDepthImage(auxDepthFrame.msg, auxDepthForSeg)) + { + if (!auxDepthForSeg.empty() && auxDepthForSeg.size() != cvImgSeg->image.size()) + { + cv::Mat resizedDepth; + cv::resize(auxDepthForSeg, resizedDepth, cvImgSeg->image.size(), 0, 0, cv::INTER_NEAREST); + auxDepthForSeg = resizedDepth; + } + + RCLCPP_INFO(this->get_logger(), "AuxDepth: matched depth dt = %.6f for segmented keyframe ID %lu", + auxDepthDt, static_cast(keyFrameId)); + pSLAM->AttachAuxDepthToKeyFrame(keyFrameId, auxDepthForSeg, + auxDepthFrame.timestamp_sec, auxDepthFrame.frame_id, + aux_depth_options_.min_depth, aux_depth_options_.max_depth, + aux_depth_options_.stride, aux_depth_options_.scale_mode); + } + } + else + { + RCLCPP_WARN(this->get_logger(), "AuxDepth: no depth found for segmented frame timestamp %.9f keyframe ID %lu", + segTimestamp, static_cast(keyFrameId)); + } + } + + auto tuple = std::make_tuple(keyFrameId, cvImgSeg->image, pclPc2SegPrb); pSLAM->addSegmentedImage(&tuple); } -void ImageGrabber::GrabVoxbloxSkeletonGraph(const visualization_msgs::MarkerArray &msgSkeletonGraphs) +void ImageGrabber::GrabVoxbloxSkeletonGraph(const visualization_msgs::msg::MarkerArray &msgSkeletonGraphs) { - // Pass the skeleton graph to a buffer to be processed by the SemanticSegmentation thread setVoxbloxSkeletonCluster(msgSkeletonGraphs); -} \ No newline at end of file +} + +// --------------------------------------------------------------------------- +// main +// --------------------------------------------------------------------------- + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("vs_graphs"); + + if (argc > 1) + RCLCPP_WARN(node->get_logger(), "Arguments supplied via command line are ignored."); + + // --- Parameters --- + node->declare_parameter("yaw", 0.0); + node->declare_parameter("roll", 0.0); + node->declare_parameter("pitch", 0.0); + node->declare_parameter("enable_pangolin", true); + node->declare_parameter("static_transform", false); + node->declare_parameter("colored_pointcloud", true); + node->declare_parameter("publish_pointclouds", true); + node->declare_parameter("frame_imu", "imu"); + node->declare_parameter("frame_map", "map"); + node->declare_parameter("frame_world", "world"); + node->declare_parameter("frame_camera", "camera"); + node->declare_parameter("frame_structural_element", "struc_elem"); + node->declare_parameter("frame_building_component", "build_comp"); + node->declare_parameter("voc_file", "file_not_set"); + node->declare_parameter("settings_file", "file_not_set"); + node->declare_parameter("sys_params_file", "file_not_set"); + node->declare_parameter("use_aux_depth", true); + node->declare_parameter("aux_depth_topic", "/camera/depth_da3/image_rect"); + node->declare_parameter("aux_depth_encoding_is_metric", true); + node->declare_parameter("aux_depth_time_tolerance", 0.05); + node->declare_parameter("aux_depth_min", 0.2); + node->declare_parameter("aux_depth_max", 20.0); + node->declare_parameter("aux_depth_stride", 2); + node->declare_parameter("aux_depth_buffer_size", 600); + node->declare_parameter("aux_depth_scale_mode", "none"); + node->declare_parameter("instance_mask_topic", "/camera/color/image_instance_masks"); + node->declare_parameter("instance_mask_time_tolerance", kDefaultInstanceMaskTimeTolerance); + node->declare_parameter("instance_mask_buffer_size", 300); + + const std::string vocFile = node->get_parameter("voc_file").as_string(); + const std::string settingsFile = node->get_parameter("settings_file").as_string(); + const std::string sysParamsFile = node->get_parameter("sys_params_file").as_string(); + const std::string auxDepthTopic = node->get_parameter("aux_depth_topic").as_string(); + ImageGrabber::AuxDepthOptions auxDepthOptions; + auxDepthOptions.use_aux_depth = node->get_parameter("use_aux_depth").as_bool(); + auxDepthOptions.encoding_is_metric = node->get_parameter("aux_depth_encoding_is_metric").as_bool(); + auxDepthOptions.time_tolerance = node->get_parameter("aux_depth_time_tolerance").as_double(); + auxDepthOptions.min_depth = static_cast(node->get_parameter("aux_depth_min").as_double()); + auxDepthOptions.max_depth = static_cast(node->get_parameter("aux_depth_max").as_double()); + auxDepthOptions.stride = std::max(1, static_cast(node->get_parameter("aux_depth_stride").as_int())); + auxDepthOptions.scale_mode = node->get_parameter("aux_depth_scale_mode").as_string(); + + ImageGrabber::SegmentOptions segmentOptions; + segmentOptions.instance_mask_topic = node->get_parameter("instance_mask_topic").as_string(); + segmentOptions.instance_mask_time_tolerance = + std::max(0.0, node->get_parameter("instance_mask_time_tolerance").as_double()); + segmentOptions.max_instance_mask_buffer_size = + static_cast(std::max(1, static_cast(node->get_parameter("instance_mask_buffer_size").as_int()))); + if (auxDepthOptions.scale_mode != "none" && auxDepthOptions.scale_mode != "map_median") + { + RCLCPP_WARN(node->get_logger(), "AuxDepth: unsupported aux_depth_scale_mode '%s', using 'none'", + auxDepthOptions.scale_mode.c_str()); + auxDepthOptions.scale_mode = "none"; + } + if (auxDepthOptions.use_aux_depth && !auxDepthOptions.encoding_is_metric && auxDepthOptions.scale_mode == "none") + { + RCLCPP_WARN(node->get_logger(), + "AuxDepth: aux_depth_encoding_is_metric is false with scale_mode 'none'; depth will be interpreted as meters by segmentation"); + } + RCLCPP_INFO(node->get_logger(), "AuxDepth: use_aux_depth = %s", + auxDepthOptions.use_aux_depth ? "true" : "false"); + + if (vocFile == "file_not_set" || settingsFile == "file_not_set") + { + RCLCPP_ERROR(node->get_logger(), "[Error] 'voc_file' and 'settings_file' not set. Exiting."); + rclcpp::shutdown(); + return 1; + } + if (sysParamsFile == "file_not_set") + { + RCLCPP_ERROR(node->get_logger(), "[Error] 'sys_params_file' not set. Exiting."); + rclcpp::shutdown(); + return 1; + } + + // Populate globals required by common.h helpers (publishTopics, etc.) + yaw = node->get_parameter("yaw").as_double(); + roll = node->get_parameter("roll").as_double(); + pitch = node->get_parameter("pitch").as_double(); + frameImu = node->get_parameter("frame_imu").as_string(); + frameMap = node->get_parameter("frame_map").as_string(); + frameWorld = node->get_parameter("frame_world").as_string(); + frameCamera = node->get_parameter("frame_camera").as_string(); + colorPointcloud = node->get_parameter("colored_pointcloud").as_bool(); + pubPointClouds = node->get_parameter("publish_pointclouds").as_bool(); + frameBC = node->get_parameter("frame_building_component").as_string(); + frameSE = node->get_parameter("frame_structural_element").as_string(); + pubStaticTransform = node->get_parameter("static_transform").as_bool(); + const bool enablePangolin = node->get_parameter("enable_pangolin").as_bool(); + + // --- SLAM system --- + sensorType = ORB_SLAM3::System::IMU_MONOCULAR; + pSLAM = new ORB_SLAM3::System(vocFile, settingsFile, sysParamsFile, sensorType, enablePangolin); + + // --- Shared state --- + auto state = std::make_shared(); + state->max_aux_depth_buffer_size = + static_cast(std::max(1, static_cast(node->get_parameter("aux_depth_buffer_size").as_int()))); + + // --- TF broadcasters (one pair, shared) --- + tfBroadcaster = + std::make_shared(node); + staticTfBroadcaster = + std::make_shared(node); + + // --- Grabber nodes --- + auto imugb = std::make_shared(state); + auto igb = std::make_shared(state, auxDepthOptions, segmentOptions); + + // --- QoS (sensor data profile) --- + // rmw_qos_profile_sensor_data defaults to a keep-last depth of 5, which at ~400Hz is only a + // ~12.5ms buffer -- any delay servicing the IMU callback longer than that (e.g. the executor + // busy on a slower vision callback, see imuCallbackGroup below) causes DDS to silently drop + // the backlog before GrabImu ever runs, starving SyncWithImu's ready_imu_count even though + // the true publish rate is far higher than the ~30Hz camera rate would suggest is necessary. + // Widened to 0.5s worth of samples to absorb realistic scheduling jitter/bursts. + rclcpp::QoS sensorQos(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data)); + sensorQos.reliability(rclcpp::ReliabilityPolicy::BestEffort); + sensorQos.durability(rclcpp::DurabilityPolicy::Volatile); + sensorQos.keep_last(200); + + using sensor_msgs::msg::Image; + using sensor_msgs::msg::Imu; + + // automatically_add_to_executor_with_node = false: this group is serviced by its own + // dedicated executor/thread below instead of the shared 4-thread pool, so it must not also + // be picked up by executor.add_node(node) below (a callback group can only belong to one + // executor at a time). + auto imuCallbackGroup = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, /*automatically_add_to_executor_with_node=*/false); + auto imageCallbackGroup = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto auxDepthCallbackGroup = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto instanceMaskCallbackGroup = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto semanticCallbackGroup = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto voxbloxCallbackGroup = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + rclcpp::SubscriptionOptions imuOptions; + imuOptions.callback_group = imuCallbackGroup; + rclcpp::SubscriptionOptions imageOptions; + imageOptions.callback_group = imageCallbackGroup; + rclcpp::SubscriptionOptions auxDepthOptionsSub; + auxDepthOptionsSub.callback_group = auxDepthCallbackGroup; + rclcpp::SubscriptionOptions instanceMaskOptions; + instanceMaskOptions.callback_group = instanceMaskCallbackGroup; + rclcpp::SubscriptionOptions semanticOptions; + semanticOptions.callback_group = semanticCallbackGroup; + rclcpp::SubscriptionOptions voxbloxOptions; + voxbloxOptions.callback_group = voxbloxCallbackGroup; + + auto subImu = node->create_subscription( + "/imu", sensorQos, + [imugb](const Imu::ConstSharedPtr msg) { imugb->GrabImu(msg); }, + imuOptions); + + auto subImg = node->create_subscription( + "/camera/image_raw", 1, + [igb](const Image::ConstSharedPtr msg) { igb->GrabImage(msg); }, + imageOptions); + + rclcpp::Subscription::SharedPtr subAuxDepth; + if (auxDepthOptions.use_aux_depth) + { + subAuxDepth = node->create_subscription( + auxDepthTopic, 1, + [igb](const Image::ConstSharedPtr msg) { igb->GrabAuxDepth(msg); }, + auxDepthOptionsSub); + RCLCPP_INFO(node->get_logger(), "AuxDepth: subscribed to %s", auxDepthTopic.c_str()); + } + + auto subInstanceMask = node->create_subscription( + segmentOptions.instance_mask_topic, 10, + [igb](const Image::ConstSharedPtr msg) { igb->GrabInstanceMask(msg); }, + instanceMaskOptions); + RCLCPP_INFO(node->get_logger(), "Instance masks: subscribed to %s", segmentOptions.instance_mask_topic.c_str()); + + auto subSegmentedImage = node->create_subscription( + "/camera/color/image_segment", 50, + [igb](const segmenter_ros::msg::SegmenterDataMsg::SharedPtr msg) + { igb->GrabSegmentation(*msg); }, + semanticOptions); + + auto subVoxbloxSkeletonMesh = node->create_subscription( + "/voxblox_skeletonizer/sparse_graph", 1, + [igb](const visualization_msgs::msg::MarkerArray::SharedPtr msg) + { igb->GrabVoxbloxSkeletonGraph(*msg); }, + voxbloxOptions); + + static std::shared_ptr imageTransport = + std::make_shared(node); + setupPublishers(node, imageTransport, node->get_name()); + setupServices(node, node->get_name()); + + // --- Sync thread --- + std::thread syncThread(&ImageGrabber::SyncWithImu, igb); + + // Dedicated executor + thread for imuCallbackGroup: isolates high-rate (~400Hz) IMU + // servicing from the heavier per-frame vision callbacks (image/instanceMask/semantic/ + // voxblox), which share only 4 threads on the executor below and can otherwise starve the + // IMU callback long enough to overrun sensorQos's buffer and silently drop samples. + rclcpp::executors::SingleThreadedExecutor imuExecutor; + imuExecutor.add_callback_group(imuCallbackGroup, node->get_node_base_interface()); + std::thread imuThread([&imuExecutor]() { imuExecutor.spin(); }); + + rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 4); + executor.add_node(node); + executor.spin(); + + igb->mustStop = true; + state->image_ready_cv.notify_all(); // unblock SyncWithImu if it's waiting + syncThread.join(); + imuExecutor.cancel(); + imuThread.join(); + pSLAM->Shutdown(); + + rclcpp::shutdown(); + return 0; +}