Epic 107: add robust visual and RGB-D odometry

CHANGELOG.md

@@ -21,6 +21,11 @@ removed no sooner than the next major (see `docs/API_STABILITY.md`).
 
 ### Added
 
+- **Robust visual/RGB-D odometry integration (Epic 107)**: spatially balanced
+  keypoint selection, forward/backward LK validation, scale-explicit monocular
+  motion, metric depth-backed PnP, mapping `DeltaMotion` bridges, Python RGB-D
+  odometry, and an OpenCV pose receipt.
+
 - **Video recognition substrate (Epic 106)**: dense integer block flow,
   adaptive Gaussian foreground segmentation, deterministic same-class IoU
   tracking, and timestamped pull-based video source contracts. Codec/camera

bench/opencv_comparison/manifest.json

@@ -28,6 +28,8 @@
       { "id": "dense_optical_flow", "domain": "video", "modes": ["allocate"] },
       { "id": "background_model", "domain": "video", "modes": ["streaming"] },
       { "id": "multi_object_tracking", "domain": "video", "modes": ["streaming"] },
+      { "id": "visual_odometry", "domain": "localization", "modes": ["correctness"] },
+      { "id": "rgbd_odometry", "domain": "localization", "modes": ["correctness"] },
     { "id": "depth_to_xyz", "domain": "rgbd", "modes": ["allocate", "reuse"] },
     { "id": "rgbd_to_point_cloud", "domain": "spatial-e2e", "modes": ["allocate"] },
     { "id": "ai_preprocess", "domain": "dnn-adapter", "modes": ["allocate", "reuse"] },

bench/opencv_comparison/run.py

@@ -20,6 +20,7 @@
     / "performance.py",
     "rgbd": ROOT / "bench" / "opencv_rgbd_comparison" / "run.py",
     "video": ROOT / "bench" / "opencv_video_comparison" / "run.py",
+    "odometry": ROOT / "bench" / "opencv_odometry_comparison" / "run.py",
 }
 
 

bench/opencv_odometry_comparison/README.md

@@ -0,0 +1,5 @@
+# OpenCV odometry comparison
+
+Runs deterministic metric RGB-D odometry on shared synthetic tracks and
+compares SpatialRust with OpenCV `solvePnPRansac`. The receipt gates pose error,
+inlier count, and the explicit invalid-depth count.

bench/opencv_odometry_comparison/run.py

@@ -0,0 +1,78 @@
+"""OpenCV correctness receipt for Epic 107 metric RGB-D odometry."""
+
+from __future__ import annotations
+
+import argparse
+import sys
+from pathlib import Path
+
+import cv2
+import numpy as np
+import spatialrust as sr
+
+sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
+from opencv_comparison.report import emit_report, environment, make_report
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--output", type=Path)
+    args = parser.parse_args()
+    width, height = 160, 120
+    fx, fy, cx, cy = 180.0, 175.0, 80.0, 60.0
+    camera = np.array([[fx, 0.0, cx], [0.0, fy, cy], [0.0, 0.0, 1.0]])
+    depth = np.full((height, width), np.nan, dtype=np.float32)
+    source, target, objects = [], [], []
+    expected = np.array([0.08, -0.025, 0.04], dtype=np.float64)
+    for index, (x, y) in enumerate(
+        (x, y) for y in range(20, 101, 16) for x in range(24, 137, 16)
+    ):
+        z = 1.3 + 0.025 * index
+        point = np.array([(x - cx) * z / fx, (y - cy) * z / fy, z])
+        moved = point + expected
+        source.append((x, y))
+        target.append((fx * moved[0] / moved[2] + cx, fy * moved[1] / moved[2] + cy))
+        objects.append(point)
+        depth[y, x] = z
+    source = np.asarray(source, dtype=np.float64)
+    target = np.asarray(target, dtype=np.float64)
+    objects = np.asarray(objects, dtype=np.float64)
+    source = np.vstack((source, [[5.0, 5.0]]))
+    target = np.vstack((target, [[5.0, 5.0]]))
+
+    sr_rotation, sr_translation, sr_inliers, rejected = sr.estimate_rgbd_odometry(
+        depth, source, target, fx, fy, cx, cy, threshold=0.1
+    )
+    ok, rvec, cv_translation, cv_inliers = cv2.solvePnPRansac(
+        objects, target[:-1], camera, None, iterationsCount=2000,
+        reprojectionError=0.1, confidence=0.99, flags=cv2.SOLVEPNP_EPNP,
+    )
+    if not ok:
+        raise RuntimeError("OpenCV solvePnPRansac failed")
+    cv_rotation, _ = cv2.Rodrigues(rvec)
+    translation_error = float(np.max(np.abs(sr_translation - expected)))
+    cross_translation_error = float(np.max(np.abs(sr_translation - cv_translation[:, 0])))
+    cross_rotation_error = float(np.max(np.abs(sr_rotation - cv_rotation)))
+    status = "pass" if (
+        translation_error <= 1e-5 and cross_translation_error <= 1e-5
+        and cross_rotation_error <= 1e-5 and rejected == 1
+        and int(np.count_nonzero(sr_inliers)) == len(objects)
+    ) else "fail"
+    emit_report(make_report(
+        suite="opencv-odometry", kind="correctness", status=status,
+        environment_receipt=environment(
+            opencv_version=cv2.__version__, spatialrust_version=sr.__version__),
+        results={
+            "translation_max_error": translation_error,
+            "cross_translation_max_error": cross_translation_error,
+            "cross_rotation_max_error": cross_rotation_error,
+            "spatialrust_inliers": int(np.count_nonzero(sr_inliers)),
+            "opencv_inliers": int(len(cv_inliers)),
+            "rejected_depth_count": rejected,
+            "thresholds": {"pose_max_error": 1e-5},
+        },
+    ), args.output)
+
+
+if __name__ == "__main__":
+    main()

crates/spatialrust-mapping/Cargo.toml

@@ -10,11 +10,13 @@ description = "Trajectories, pose graphs, and localization primitives for Spatia
 
 [features]
 default = []
+vision-odometry = ["dep:spatialrust-vision"]
 
 [dependencies]
 spatialrust-core.workspace = true
 spatialrust-math.workspace = true
 spatialrust-sync.workspace = true
+spatialrust-vision = { workspace = true, optional = true, features = ["odometry"] }
 thiserror.workspace = true
 
 [dev-dependencies]

crates/spatialrust-mapping/src/lib.rs

@@ -7,8 +7,12 @@ mod error;
 mod motion;
 mod pose_graph;
 mod trajectory;
+#[cfg(feature = "vision-odometry")]
+mod vision;
 
 pub use error::{MappingError, MappingResult};
 pub use motion::{DeltaMotion, RelativeMotionEstimator, SyntheticOdometry};
 pub use pose_graph::{PoseGraph, PoseGraphEdge, PoseNodeId};
 pub use trajectory::{StampedPose, Trajectory};
+#[cfg(feature = "vision-odometry")]
+pub use vision::{delta_from_monocular_odometry, delta_from_rgbd_odometry};

crates/spatialrust-mapping/src/vision.rs

@@ -0,0 +1,116 @@
+//! Explicit conversion from vision odometry estimates into mapping motion.
+
+use spatialrust_math::{Isometry3, Mat3, Quat, Vec3};
+use spatialrust_sync::StampedTime;
+use spatialrust_vision::{MonocularOdometryEstimate, RgbdOdometryEstimate};
+
+use crate::{DeltaMotion, MappingError, MappingResult};
+
+/// Converts scale-ambiguous monocular motion after the caller supplies scale.
+pub fn delta_from_monocular_odometry(
+    from: StampedTime,
+    to: StampedTime,
+    estimate: &MonocularOdometryEstimate,
+    translation_scale: f32,
+) -> MappingResult<DeltaMotion> {
+    if !translation_scale.is_finite() || translation_scale <= 0.0 {
+        return Err(MappingError::InvalidConfiguration(
+            "monocular translation scale must be finite and positive".into(),
+        ));
+    }
+    let pose = estimate.pose;
+    Ok(delta(from, to, pose.rotation(), pose.translation(), translation_scale))
+}
+
+/// Converts a metric RGB-D source-to-target pose into mapping motion.
+pub fn delta_from_rgbd_odometry(
+    from: StampedTime,
+    to: StampedTime,
+    estimate: &RgbdOdometryEstimate,
+) -> DeltaMotion {
+    let pose = estimate.pose;
+    delta(from, to, pose.rotation(), pose.translation(), 1.0)
+}
+
+fn delta(
+    from: StampedTime,
+    to: StampedTime,
+    rotation: Mat3<f64>,
+    translation: Vec3<f64>,
+    scale: f32,
+) -> DeltaMotion {
+    let matrix = Mat3::from_rows(
+        [rotation.m[0][0] as f32, rotation.m[0][1] as f32, rotation.m[0][2] as f32],
+        [rotation.m[1][0] as f32, rotation.m[1][1] as f32, rotation.m[1][2] as f32],
+        [rotation.m[2][0] as f32, rotation.m[2][1] as f32, rotation.m[2][2] as f32],
+    );
+    let value = matrix_to_quaternion(matrix);
+    let translation = Vec3::new(
+        translation.x as f32 * scale,
+        translation.y as f32 * scale,
+        translation.z as f32 * scale,
+    );
+    DeltaMotion { from, to, to_t_from: Isometry3::new(value, translation) }
+}
+
+fn matrix_to_quaternion(matrix: Mat3<f32>) -> Quat<f32> {
+    let trace = matrix.m[0][0] + matrix.m[1][1] + matrix.m[2][2];
+    let quaternion = if trace > 0.0 {
+        let s = (trace + 1.0).sqrt() * 2.0;
+        Quat::new(
+            (matrix.m[2][1] - matrix.m[1][2]) / s,
+            (matrix.m[0][2] - matrix.m[2][0]) / s,
+            (matrix.m[1][0] - matrix.m[0][1]) / s,
+            0.25 * s,
+        )
+    } else if matrix.m[0][0] > matrix.m[1][1] && matrix.m[0][0] > matrix.m[2][2] {
+        let s = (1.0 + matrix.m[0][0] - matrix.m[1][1] - matrix.m[2][2]).sqrt() * 2.0;
+        Quat::new(
+            0.25 * s,
+            (matrix.m[0][1] + matrix.m[1][0]) / s,
+            (matrix.m[0][2] + matrix.m[2][0]) / s,
+            (matrix.m[2][1] - matrix.m[1][2]) / s,
+        )
+    } else if matrix.m[1][1] > matrix.m[2][2] {
+        let s = (1.0 + matrix.m[1][1] - matrix.m[0][0] - matrix.m[2][2]).sqrt() * 2.0;
+        Quat::new(
+            (matrix.m[0][1] + matrix.m[1][0]) / s,
+            0.25 * s,
+            (matrix.m[1][2] + matrix.m[2][1]) / s,
+            (matrix.m[0][2] - matrix.m[2][0]) / s,
+        )
+    } else {
+        let s = (1.0 + matrix.m[2][2] - matrix.m[0][0] - matrix.m[1][1]).sqrt() * 2.0;
+        Quat::new(
+            (matrix.m[0][2] + matrix.m[2][0]) / s,
+            (matrix.m[1][2] + matrix.m[2][1]) / s,
+            0.25 * s,
+            (matrix.m[1][0] - matrix.m[0][1]) / s,
+        )
+    };
+    quaternion.normalize()
+}
+
+#[cfg(test)]
+mod tests {
+    use super::delta_from_monocular_odometry;
+    use spatialrust_core::Timestamp;
+    use spatialrust_math::{Mat3, Vec3};
+    use spatialrust_sync::{ClockDomain, StampedTime};
+    use spatialrust_vision::{MonocularOdometryEstimate, RelativePose};
+
+    #[test]
+    fn monocular_bridge_applies_caller_scale() {
+        let estimate = MonocularOdometryEstimate {
+            pose: RelativePose::try_new(Mat3::<f64>::identity(), Vec3::new(1.0, 0.0, 0.0)).unwrap(),
+            inliers: vec![true; 8],
+            positive_depth_count: 8,
+        };
+        let stamp = |nanos| {
+            StampedTime::exact("camera", ClockDomain::HostSteady, Timestamp::from_nanos(nanos))
+        };
+        let motion = delta_from_monocular_odometry(stamp(1), stamp(2), &estimate, 0.25).unwrap();
+        assert!((motion.to_t_from.translation().x - 0.25).abs() < 1e-6);
+        assert_eq!(motion.to_t_from.rotation(), spatialrust_math::Quat::<f32>::identity());
+    }
+}

crates/spatialrust-platform/src/stability.rs

@@ -119,6 +119,7 @@ impl StabilityRegistry {
             "spatialrust-vision::stereo",
             "spatialrust-vision::optical-flow",
             "spatialrust-vision::video",
+            "spatialrust-vision::odometry",
             "spatialrust-vision::ai-adapters",
             "spatialrust-gpu::GpuImage",
         ];

crates/spatialrust-py/spatialrust.pyi

@@ -18,7 +18,7 @@ __all__: list[str] = [
     "CylinderResult", "RegistrationResult", "read_image",
     "tensor_copy_from_numpy", "tensor_view_from_dlpack", "harris_keypoints",
     "shi_tomasi_keypoints", "fast_keypoints", "orb_features",
-    "estimate_homography_ransac", "solve_pnp", "stereo_block_match",
+    "estimate_homography_ransac", "solve_pnp", "estimate_rgbd_odometry", "stereo_block_match",
     "match_binary_descriptors", "match_float_descriptors", "write_image", "read",
     "write", "voxel_downsample", "crop_box", "pass_through", "iss_keypoints",
     "orient_normals", "detect_boundary", "mls_smooth", "farthest_point_sampling",
@@ -694,6 +694,17 @@ def solve_pnp(
     width: int = ...,
     height: int = ...,
 ) -> tuple[_F64Array, _F64Array]: ...
+def estimate_rgbd_odometry(
+    depth: _F32Array,
+    source: _F64Array,
+    target: _F64Array,
+    fx: float,
+    fy: float,
+    cx: float,
+    cy: float,
+    depth_scale: float = ...,
+    threshold: float = ...,
+) -> tuple[_F64Array, _F64Array, _BoolArray, int]: ...
 def stereo_block_match(
     left: _U8Array,
     right: _U8Array,

crates/spatialrust-py/src/lib.rs

@@ -76,7 +76,8 @@ use spatialrust::vision::{
     detect_and_describe_orb as detect_and_describe_orb_op, detect_fast as detect_fast_op,
     detect_harris as detect_harris_op, detect_shi_tomasi as detect_shi_tomasi_op,
     encode_rle as encode_mask_runs, equalize_histogram as equalize_histogram_op,
-    estimate_homography_ransac as estimate_homography_ransac_op, filter2d as filter2d_op,
+    estimate_homography_ransac as estimate_homography_ransac_op,
+    estimate_rgbd_odometry as estimate_rgbd_odometry_op, filter2d as filter2d_op,
     find_contours as trace_contours, gaussian_blur as gaussian_blur_op,
     histogram_u8 as histogram_u8_op, integral_image as integral_image_op,
     laplacian as laplacian_op, letterbox as letterbox_op,
@@ -92,8 +93,9 @@ use spatialrust::vision::{
     BoundingBox2, CameraMatrix3, CannyOptions, ConfidenceMap, Connectivity, CornerSelectionOptions,
     DescriptorBuffer, FastOptions, HarrisOptions, Interpolation, Kernel2D, Keypoint2, MaskRle,
     MatchOptions, MorphologyOperation, MorphologyShape, ObjectImageCorrespondence, OrbOptions,
-    OrbScoreType, PointCorrespondence2, PointMap, RleOrder, RobustEstimationOptions,
-    ShiTomasiOptions, SoftNmsMethod, StereoBmOptions, StructuringElement, ThresholdType,
+    OrbScoreType, PointCorrespondence2, PointMap, RgbdOdometryOptions, RleOrder,
+    RobustEstimationOptions, ShiTomasiOptions, SoftNmsMethod, StereoBmOptions, StructuringElement,
+    ThresholdType,
 };
 use spatialrust::vision::{dense_flow_block_match as dense_flow_native, DenseFlowOptions};
 use spatialrust::voxelize::{
@@ -2607,6 +2609,61 @@ fn solve_pnp<'py>(
     Ok((mat3_to_numpy(py, pose.rotation()), translation))
 }
 
+/// Estimates metric RGB-D odometry from source depth and pixel tracks.
+#[pyfunction]
+#[pyo3(signature = (depth, source, target, fx, fy, cx, cy, depth_scale=1.0, threshold=1.0))]
+#[allow(clippy::too_many_arguments)]
+fn estimate_rgbd_odometry<'py>(
+    py: Python<'py>,
+    depth: PyReadonlyArray2<'_, f32>,
+    source: PyReadonlyArray2<'_, f64>,
+    target: PyReadonlyArray2<'_, f64>,
+    fx: f64,
+    fy: f64,
+    cx: f64,
+    cy: f64,
+    depth_scale: f64,
+    threshold: f64,
+) -> PyResult<(
+    Bound<'py, PyArray2<f64>>,
+    Bound<'py, PyArray1<f64>>,
+    Bound<'py, PyArray1<bool>>,
+    usize,
+)> {
+    let depth_view = depth.as_array();
+    let (height, width) = (depth_view.shape()[0], depth_view.shape()[1]);
+    let depth_image = Image::<f32, 1>::try_new(width, height, depth_view.iter().copied().collect())
+        .map_err(to_py_err)?;
+    let pairs = correspondences_from_numpy(source, target)?;
+    let camera = CameraMatrix3::from_intrinsics(
+        CameraIntrinsics::try_new(fx, fy, cx, cy, width, height).map_err(to_py_err)?,
+    );
+    let estimate = estimate_rgbd_odometry_op(
+        depth_image.view(),
+        &pairs,
+        camera,
+        RgbdOdometryOptions {
+            depth_scale,
+            robust: RobustEstimationOptions { threshold, ..Default::default() },
+            ..Default::default()
+        },
+    )
+    .map_err(to_py_err)?;
+    Ok((
+        mat3_to_numpy(py, estimate.pose.rotation()),
+        numpy::PyArray1::from_vec_bound(
+            py,
+            vec![
+                estimate.pose.translation().x,
+                estimate.pose.translation().y,
+                estimate.pose.translation().z,
+            ],
+        ),
+        numpy::PyArray1::from_vec_bound(py, estimate.inliers),
+        estimate.rejected_depth_count,
+    ))
+}
+
 /// Dense SAD stereo block matching on rectified grayscale images.
 #[pyfunction]
 #[pyo3(signature = (left, right, window_size=15, min_disparity=0, num_disparities=64, uniqueness_ratio=15.0))]
@@ -3130,6 +3187,7 @@ fn spatialrust_module(m: &Bound<'_, PyModule>) -> PyResult<()> {
     m.add_function(wrap_pyfunction!(orb_features, m)?)?;
     m.add_function(wrap_pyfunction!(estimate_homography_ransac, m)?)?;
     m.add_function(wrap_pyfunction!(solve_pnp, m)?)?;
+    m.add_function(wrap_pyfunction!(estimate_rgbd_odometry, m)?)?;
     m.add_function(wrap_pyfunction!(stereo_block_match, m)?)?;
     m.add_function(wrap_pyfunction!(match_binary_descriptors, m)?)?;
     m.add_function(wrap_pyfunction!(match_float_descriptors, m)?)?;