seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

feat(perception): depth-based obstacle size estimator (Issue #348) #351

Merged
sl-jetson merged 1 commits from sl-perception/issue-348-obstacle-size into main 2026-03-03 13:44:33 -05:00
Conversation 0 Commits 1 Files Changed 7 +1023
sl-perception commented 2026-03-03 13:33:10 -05:00
Collaborator
Copy Link

Summary

Fuses 2-D LIDAR clusters with the D435i depth image to estimate full 3-D obstacle width and height in metres.

Algorithm

  1. Gap-segment LIDAR scan → clusters (reuses _lidar_clustering)
  2. Transform each cluster centroid: LIDAR frame → camera frame (translation-only co-mount extrinsic)
  3. Project to depth image pixel via pinhole model
  4. Sample median depth in a configurable window (robust to noise)
  5. Width — taken directly from LIDAR bbox (horizontal, reliable)
  6. Height — scan vertical strip in depth image, collect pixels within z_tol of sampled depth, back-project row extent via fy
  7. Confidence from depth window fill ratio + LIDAR/depth Z agreement

New files

  • saltybot_scene_msgs/msg/ObstacleSize.msg — obstacle_id, centroid_x/y, depth_z, width_m, height_m, pixel_u/v, lidar_range, confidence
  • saltybot_scene_msgs/msg/ObstacleSizeArray.msg
  • saltybot_bringup/_obstacle_size.py — pure Python, no ROS2 deps
    • CameraParams — pinhole intrinsics + LIDAR→camera extrinsics
    • lidar_to_camera() — LIDAR (+X fwd, +Y left) → camera (+X right, +Y down, +Z fwd)
    • project_to_pixel() — pinhole projection + image bounds check
    • sample_depth_median() — uint16 depth window → median metres
    • estimate_height() — vertical strip back-projection
    • estimate_cluster_size() — full pipeline
  • saltybot_bringup/obstacle_size_node.py — ROS2 node
    • Subscribes: /scan, /camera/depth/image_rect_raw, /camera/depth/camera_info
    • Publishes: /saltybot/obstacle_sizes
    • Intrinsics latched from CameraInfo on first arrival
    • Graceful fallback (width-only, confidence=0) when depth unavailable

Modified files

  • saltybot_scene_msgs/CMakeLists.txt — register new msgs
  • saltybot_bringup/setup.py — add obstacle_size console script

Test plan

  • CameraParams defaults and custom construction
  • lidar_to_camera: forward axis, lateral→x, extrinsic offset, origin
  • project_to_pixel: principal point, negative/zero Z, off-axis, out-of-bounds, exact value
  • sample_depth_median: all-zero, uniform, scale, edge clip, sparse, mixed median
  • estimate_height: blank image, zero z_ref, known span, single row, depth outside tolerance
  • estimate_cluster_size: returns NamedTuple, width from LIDAR, centroid, range, blank depth, obstacle patch, behind-camera, height >0, obstacle_id, confidence bounds
  • 33 tests, 33 passing

Closes #348

🤖 Generated with Claude Code

## Summary Fuses 2-D LIDAR clusters with the D435i depth image to estimate full 3-D obstacle width and height in metres. ### Algorithm 1. Gap-segment LIDAR scan → clusters (reuses `_lidar_clustering`) 2. Transform each cluster centroid: LIDAR frame → camera frame (translation-only co-mount extrinsic) 3. Project to depth image pixel via pinhole model 4. Sample median depth in a configurable window (robust to noise) 5. **Width** — taken directly from LIDAR bbox (horizontal, reliable) 6. **Height** — scan vertical strip in depth image, collect pixels within `z_tol` of sampled depth, back-project row extent via `fy` 7. Confidence from depth window fill ratio + LIDAR/depth Z agreement ### New files - `saltybot_scene_msgs/msg/ObstacleSize.msg` — `obstacle_id`, `centroid_x/y`, `depth_z`, `width_m`, `height_m`, `pixel_u/v`, `lidar_range`, `confidence` - `saltybot_scene_msgs/msg/ObstacleSizeArray.msg` - `saltybot_bringup/_obstacle_size.py` — pure Python, no ROS2 deps - `CameraParams` — pinhole intrinsics + LIDAR→camera extrinsics - `lidar_to_camera()` — LIDAR (+X fwd, +Y left) → camera (+X right, +Y down, +Z fwd) - `project_to_pixel()` — pinhole projection + image bounds check - `sample_depth_median()` — uint16 depth window → median metres - `estimate_height()` — vertical strip back-projection - `estimate_cluster_size()` — full pipeline - `saltybot_bringup/obstacle_size_node.py` — ROS2 node - Subscribes: `/scan`, `/camera/depth/image_rect_raw`, `/camera/depth/camera_info` - Publishes: `/saltybot/obstacle_sizes` - Intrinsics latched from `CameraInfo` on first arrival - Graceful fallback (width-only, confidence=0) when depth unavailable ### Modified files - `saltybot_scene_msgs/CMakeLists.txt` — register new msgs - `saltybot_bringup/setup.py` — add `obstacle_size` console script ## Test plan - [x] `CameraParams` defaults and custom construction - [x] `lidar_to_camera`: forward axis, lateral→x, extrinsic offset, origin - [x] `project_to_pixel`: principal point, negative/zero Z, off-axis, out-of-bounds, exact value - [x] `sample_depth_median`: all-zero, uniform, scale, edge clip, sparse, mixed median - [x] `estimate_height`: blank image, zero z_ref, known span, single row, depth outside tolerance - [x] `estimate_cluster_size`: returns NamedTuple, width from LIDAR, centroid, range, blank depth, obstacle patch, behind-camera, height >0, obstacle_id, confidence bounds - **33 tests, 33 passing** Closes #348 🤖 Generated with [Claude Code](https://claude.com/claude-code)
sl-perception added 1 commit 2026-03-03 13:33:11 -05:00 
Projects LIDAR clusters into the D435i depth image to estimate 3-D
obstacle width and height in metres.

- saltybot_scene_msgs/msg/ObstacleSize.msg      — new message
- saltybot_scene_msgs/msg/ObstacleSizeArray.msg — array wrapper
- saltybot_scene_msgs/CMakeLists.txt            — register new msgs
- saltybot_bringup/_obstacle_size.py            — pure-Python helper:
    CameraParams (intrinsics + LIDAR→camera extrinsics)
    ObstacleSizeEstimate (NamedTuple)
    lidar_to_camera()         LIDAR frame → camera frame transform
    project_to_pixel()        pinhole projection + bounds check
    sample_depth_median()     uint16 depth image window → median metres
    estimate_height()         vertical strip scan for row extent → height_m
    estimate_cluster_size()   full pipeline: cluster → size estimate
- saltybot_bringup/obstacle_size_node.py        — ROS2 node
    sub: /scan, /camera/depth/image_rect_raw, /camera/depth/camera_info
    pub: /saltybot/obstacle_sizes (ObstacleSizeArray)
    width from LIDAR bbox; height from depth strip back-projection;
    graceful fallback (LIDAR-only) when depth image unavailable;
    intrinsics latched from CameraInfo on first arrival
- test/test_obstacle_size.py                    — 33 tests, 33 passing
- setup.py                                      — add obstacle_size entry

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-jetson merged commit 7966eb5187 into main 2026-03-03 13:44:33 -05:00
Sign in to join this conversation.
Reviewers
No Reviewers
Labels
No items
No Label
Milestone
No items
No Milestone
Projects
Clear projects
No project
Assignees
Clear assignees
shaytac sl-controls sl-firmware sl-jetson sl-mechanical sl-perception sl-uwb sl-webui seb
No Assignees
1 Participants
Notifications
Due Date
No due date set.
Dependencies

No dependencies set.

Reference: seb/saltylab-firmware#351
No description provided.
Delete Branch "sl-perception/issue-348-obstacle-size"

Deleting a branch is permanent. Although the deleted branch may continue to exist for a short time before it actually gets removed, it CANNOT be undone in most cases. Continue?