|
|
c1b82608d5
|
feat: Visual odometry from RealSense stereo ORB (Issue #586)
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>
|
2026-03-14 12:21:58 -04:00 |
|
|
|
572e578069
|
feat(vo): visual odometry fallback — CUDA optical flow + EKF fusion + slip failover (Issue #157)
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>
|
2026-03-02 10:15:32 -05:00 |
|
