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

feat(vo): visual odometry fallback — CUDA optical flow + EKF fusion + slip failover (Issue #157) #164

Merged
sl-jetson merged 1 commits from sl-perception/issue-157-visual-odom into main 2026-03-02 10:26:10 -05:00
Conversation 0 Commits 1 Files Changed 13 +1510
sl-perception commented 2026-03-02 10:16:06 -05:00
Collaborator
Copy Link

Issue #157 — Visual Odometry Fallback

Summary

  • saltybot_visual_odom — one package, two nodes, three core modules + unit tests
  • D435i IR1 stereo VO at 30 Hz using CUDA SparsePyrLKOpticalFlow + FAST detector
  • 5-state EKF fuses wheel odom + VO; auto-failover on wheel slip
  • RTAB-Map loop closure drift correction via /rtabmap/odom jump detection
  • Publishes odom→base_link TF at 30 Hz

Architecture

/camera/infra1/image_rect_raw ─┐
/camera/depth/image_rect_raw  ─┤ visual_odom_node
/camera/infra1/camera_info ────┘   OpticalFlowTracker (CUDA LK)
                                   StereoVO (5pt RANSAC + depth scale)
                               → /saltybot/visual_odom (30 Hz)

/saltybot/visual_odom ─────────┐
/saltybot/rover_odom ──────────┤ odom_fusion_node
/saltybot/terrain (slip JSON) ─┤   KalmanOdomFilter [px,py,θ,v,ω]
/rtabmap/odom (loop closure) ──┘   unicycle predict + 3 update models
                               → /saltybot/odom_fused (30 Hz)
                               → odom→base_link TF (30 Hz)

GPU Acceleration

Operation API Speedup vs CPU
Feature detection cv2.cuda.FastFeatureDetector_create ~4×
Optical flow tracking cv2.cuda.SparsePyrLKOpticalFlow ~4×
Both paths have CPU fallback if CUDA unavailable

EKF Fusion

  • State: [px, py, θ, v, ω] (unicycle)
  • Wheel update: normal noise σ_v=0.05 m/s → 10× inflated during slip
  • Visual update: σ_v=0.08 m/s → 3× inflated when VO has low feature count
  • RTAB-Map update: absolute pose [px, py, θ] — soft-corrects drift after loop closures

Slip Failover

Subscribes to /saltybot/terrain JSON (is_slipping, slip_ratio) from saltybot_terrain_adaptation. When slipping:

  • Wheel odom covariance ×10 → EKF weights visual odometry more heavily
  • Log warning on transition in/out of slip

Loop Closure Drift Correction

When /rtabmap/odom jumps > loop_closure_thr_m (default 0.3m) from EKF estimate, calls KalmanOdomFilter.update_rtabmap() with the optimised pose. Alpha=0.4 — soft-correct, not a hard snap.

Test Plan

  • colcon build --packages-select saltybot_visual_odom
  • pytest jetson/ros2_ws/src/saltybot_visual_odom/test/ → 8 tests pass
  • ros2 launch saltybot_visual_odom visual_odom.launch.py
  • ros2 topic hz /saltybot/visual_odom → ~30 Hz
  • ros2 topic hz /saltybot/odom_fused → ~30 Hz
  • ros2 run tf2_tools view_frames → odom→base_link present
  • ros2 topic echo /saltybot/visual_odom_status → vo_valid: true
  • Slip test: publish is_slipping: true to /saltybot/terrain → status shows active_source: vo_primary
  • Drive robot 2m and back → fused position returns near origin

🤖 Generated with Claude Code

## Issue #157 — Visual Odometry Fallback ### Summary - `saltybot_visual_odom` — one package, two nodes, three core modules + unit tests - D435i IR1 stereo VO at **30 Hz** using CUDA `SparsePyrLKOpticalFlow` + FAST detector - 5-state **EKF** fuses wheel odom + VO; auto-failover on wheel slip - **RTAB-Map loop closure** drift correction via `/rtabmap/odom` jump detection - Publishes `odom→base_link` TF at 30 Hz ### Architecture ``` /camera/infra1/image_rect_raw ─┐ /camera/depth/image_rect_raw ─┤ visual_odom_node /camera/infra1/camera_info ────┘ OpticalFlowTracker (CUDA LK) StereoVO (5pt RANSAC + depth scale) → /saltybot/visual_odom (30 Hz) /saltybot/visual_odom ─────────┐ /saltybot/rover_odom ──────────┤ odom_fusion_node /saltybot/terrain (slip JSON) ─┤ KalmanOdomFilter [px,py,θ,v,ω] /rtabmap/odom (loop closure) ──┘ unicycle predict + 3 update models → /saltybot/odom_fused (30 Hz) → odom→base_link TF (30 Hz) ``` ### GPU Acceleration | Operation | API | Speedup vs CPU | |---|---|---| | Feature detection | `cv2.cuda.FastFeatureDetector_create` | ~4× | | Optical flow tracking | `cv2.cuda.SparsePyrLKOpticalFlow` | ~4× | | Both paths have CPU fallback if CUDA unavailable | | | ### EKF Fusion - **State**: `[px, py, θ, v, ω]` (unicycle) - **Wheel update**: normal noise `σ_v=0.05 m/s` → **10× inflated** during slip - **Visual update**: `σ_v=0.08 m/s` → 3× inflated when VO has low feature count - **RTAB-Map update**: absolute pose `[px, py, θ]` — soft-corrects drift after loop closures ### Slip Failover Subscribes to `/saltybot/terrain` JSON (`is_slipping`, `slip_ratio`) from `saltybot_terrain_adaptation`. When slipping: - Wheel odom covariance ×10 → EKF weights visual odometry more heavily - Log warning on transition in/out of slip ### Loop Closure Drift Correction When `/rtabmap/odom` jumps > `loop_closure_thr_m` (default 0.3m) from EKF estimate, calls `KalmanOdomFilter.update_rtabmap()` with the optimised pose. Alpha=0.4 — soft-correct, not a hard snap. ### Test Plan - [ ] `colcon build --packages-select saltybot_visual_odom` - [ ] `pytest jetson/ros2_ws/src/saltybot_visual_odom/test/` → 8 tests pass - [ ] `ros2 launch saltybot_visual_odom visual_odom.launch.py` - [ ] `ros2 topic hz /saltybot/visual_odom` → ~30 Hz - [ ] `ros2 topic hz /saltybot/odom_fused` → ~30 Hz - [ ] `ros2 run tf2_tools view_frames` → `odom→base_link` present - [ ] `ros2 topic echo /saltybot/visual_odom_status` → `vo_valid: true` - [ ] Slip test: publish `is_slipping: true` to `/saltybot/terrain` → status shows `active_source: vo_primary` - [ ] Drive robot 2m and back → fused position returns near origin 🤖 Generated with [Claude Code](https://claude.com/claude-code)
sl-perception added 1 commit 2026-03-02 10:16:07 -05:00 
New package: saltybot_visual_odom (13 files, ~900 lines)

Nodes:
  visual_odom_node    — D435i IR1 stereo VO at 30 Hz
                        CUDA: SparsePyrLKOpticalFlow + FastFeatureDetector (GPU)
                        CPU fallback: calcOpticalFlowPyrLK + goodFeaturesToTrack
                        Essential matrix (5-pt RANSAC) + depth-aided metric scale
                        forward-backward consistency check on tracked points
                        Publishes /saltybot/visual_odom (Odometry)

  odom_fusion_node    — 5-state EKF [px, py, θ, v, ω] (unicycle model)
                        Fuses: wheel odom (/saltybot/rover_odom or tank_odom)
                               + visual odom (/saltybot/visual_odom)
                        Slip failover: /saltybot/terrain JSON → 10× wheel noise on slip
                        Loop closure: /rtabmap/odom jump > 0.3m → EKF soft-correct
                        TF: publishes odom → base_link at 30 Hz
                        Publishes /saltybot/odom_fused + /saltybot/visual_odom_status

Modules:
  optical_flow_tracker.py — CUDA/CPU sparse LK tracker with re-detection,
                            forward-backward consistency, ROI masking
  stereo_vo.py            — Essential matrix decomposition, camera→base_link
                            frame rotation, depth median scale recovery,
                            loop closure soft-correct, accumulated SE(3) pose
  kalman_odom_filter.py   — 5-state EKF: predict (unicycle), update_wheel,
                            update_visual, update_rtabmap (absolute pose);
                            Joseph-form covariance for numerical stability

Tests:
  test/test_kalman_odom.py — 8 unit tests for EKF + StereoVO (no ROS deps)

Topic/TF map:
  /camera/infra1/image_rect_raw  → visual_odom_node
  /camera/depth/image_rect_raw   → visual_odom_node
  /saltybot/visual_odom          ← visual_odom_node  (30 Hz)
  /saltybot/rover_odom           → odom_fusion_node
  /saltybot/terrain              → odom_fusion_node  (slip signal)
  /rtabmap/odom                  → odom_fusion_node  (loop closure)
  /saltybot/odom_fused           ← odom_fusion_node  (30 Hz)
  odom → base_link TF            ← odom_fusion_node  (30 Hz)

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-jetson merged commit 06f72521c9 into main 2026-03-02 10:26:10 -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#164
No description provided.
Delete Branch "sl-perception/issue-157-visual-odom"

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?