feat: Visual odometry from RealSense stereo ORB (Issue #586) #593

sl-perception · 2026-03-14T12:22:28-04:00

sl-perception commented

2026-03-14 12:22:28 -04:00

Summary

Adds ORB stereo visual odometry to the existing saltybot_visual_odom package.

New modules

orb_stereo_matcher.py

OrbStereoMatcher class wrapping cv2.ORB_create()
update(curr_gray): detect ORB on infra1, BFMatcher NORM_HAMMING + Lowe ratio test → temporal matched pairs (prev→curr)
stereo_scale(left, right, fx, baseline_m): ORB match infra1↔infra2 with epipolar row constraint (|Δrow|≤2px), depth = baseline×fx/disparity → median metric depth

stereo_orb_node.py (stereo_orb_vo node)

Subscribes (ApproximateTimeSynchronizer): /camera/infra1/image_rect_raw, /camera/infra2/image_rect_raw, /camera/depth/image_rect_raw
Camera-info: intrinsics (infra1) + baseline from infra2 Tx field
Pipeline per frame: ORB temporal match → Essential matrix (5-point RANSAC) via StereoVO → scale from D435i depth (primary) or stereo disparity (fallback) → SE(3) accumulation
Publishes nav_msgs/Odometry on /saltybot/visual_odom
TF2 broadcast: odom → camera_link

config/stereo_orb_params.yaml, launch/stereo_orb.launch.py

setup.py: adds stereo_orb console script entry, installs launch+config dirs

Closes #586

## Summary Adds ORB stereo visual odometry to the existing `saltybot_visual_odom` package. ### New modules **`orb_stereo_matcher.py`** - `OrbStereoMatcher` class wrapping `cv2.ORB_create()` - `update(curr_gray)`: detect ORB on infra1, BFMatcher NORM_HAMMING + Lowe ratio test → temporal matched pairs (prev→curr) - `stereo_scale(left, right, fx, baseline_m)`: ORB match infra1↔infra2 with epipolar row constraint (|Δrow|≤2px), depth = baseline×fx/disparity → median metric depth **`stereo_orb_node.py`** (`stereo_orb_vo` node) - Subscribes (ApproximateTimeSynchronizer): `/camera/infra1/image_rect_raw`, `/camera/infra2/image_rect_raw`, `/camera/depth/image_rect_raw` - Camera-info: intrinsics (infra1) + baseline from infra2 Tx field - Pipeline per frame: ORB temporal match → Essential matrix (5-point RANSAC) via `StereoVO` → scale from D435i depth (primary) or stereo disparity (fallback) → SE(3) accumulation - Publishes `nav_msgs/Odometry` on `/saltybot/visual_odom` - TF2 broadcast: `odom → camera_link` **`config/stereo_orb_params.yaml`**, **`launch/stereo_orb.launch.py`** **`setup.py`**: adds `stereo_orb` console script entry, installs launch+config dirs Closes #586

sl-perception added 1 commit 2026-03-14 12:22:29 -04:00

feat: Visual odometry from RealSense stereo ORB (Issue #586 ) c1b82608d5

Adds stereo ORB-based visual odometry to saltybot_visual_odom package.

New modules:
- orb_stereo_matcher.py: ORB feature detection (cv2.ORB_create) with BFMatcher
  NORM_HAMMING + Lowe ratio test for temporal matching (infra1 prev→curr).
  Stereo scale method matches infra1↔infra2 under epipolar row constraint
  (|Δrow|≤2px), computes depth = baseline_m * fx / disparity.
- stereo_orb_node.py: StereoOrbNode subscribes to infra1+infra2+depth
  (ApproximateTimeSynchronizer 3-topic), detects/matches ORB temporally,
  estimates SE(3) via Essential matrix (5-point RANSAC) using StereoVO,
  recovers metric scale from D435i aligned depth (primary) or stereo
  baseline disparity (fallback). Publishes nav_msgs/Odometry on
  /saltybot/visual_odom and broadcasts TF2 odom→camera_link. Baseline
  auto-updated from infra2 camera_info Tx (overrides parameter).
- config/stereo_orb_params.yaml, launch/stereo_orb.launch.py
- setup.py: adds stereo_orb entrypoint, installs launch+config dirs

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

sl-jetson merged commit 15ff5acca7 into main

2026-03-14 13:32:57 -04:00

sl-jetson referenced this issue from a commit

2026-03-14 13:32:57 -04:00

Merge pull request 'feat: Visual odometry from RealSense stereo ORB (Issue #586)' (#593) from sl-perception/issue-586-visual-odom into main

Sign in to join this conversation.

1 Participants

Notifications

Due Date

No due date set.

Dependencies

No dependencies set.

Reference: seb/saltylab-firmware#593