diff --git a/CHANGELOG.md b/CHANGELOG.md index d950d96..eaf8512 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/bench/opencv_comparison/manifest.json b/bench/opencv_comparison/manifest.json index ef44324..74c90b8 100644 --- a/bench/opencv_comparison/manifest.json +++ b/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"] }, diff --git a/bench/opencv_comparison/run.py b/bench/opencv_comparison/run.py index 7ebe11c..9db9d24 100644 --- a/bench/opencv_comparison/run.py +++ b/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", } diff --git a/bench/opencv_odometry_comparison/README.md b/bench/opencv_odometry_comparison/README.md new file mode 100644 index 0000000..dea642f --- /dev/null +++ b/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. diff --git a/bench/opencv_odometry_comparison/run.py b/bench/opencv_odometry_comparison/run.py new file mode 100644 index 0000000..9672ad3 --- /dev/null +++ b/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() diff --git a/crates/spatialrust-mapping/Cargo.toml b/crates/spatialrust-mapping/Cargo.toml index 32ebe44..cd6e241 100644 --- a/crates/spatialrust-mapping/Cargo.toml +++ b/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] diff --git a/crates/spatialrust-mapping/src/lib.rs b/crates/spatialrust-mapping/src/lib.rs index 67c8d90..59f888e 100644 --- a/crates/spatialrust-mapping/src/lib.rs +++ b/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}; diff --git a/crates/spatialrust-mapping/src/vision.rs b/crates/spatialrust-mapping/src/vision.rs new file mode 100644 index 0000000..5cbdefa --- /dev/null +++ b/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 { + 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, + translation: Vec3, + 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) -> Quat { + 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::::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::::identity()); + } +} diff --git a/crates/spatialrust-platform/src/stability.rs b/crates/spatialrust-platform/src/stability.rs index 6a2b8db..4309e94 100644 --- a/crates/spatialrust-platform/src/stability.rs +++ b/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", ]; diff --git a/crates/spatialrust-py/spatialrust.pyi b/crates/spatialrust-py/spatialrust.pyi index 2e9ad9a..b5ed469 100644 --- a/crates/spatialrust-py/spatialrust.pyi +++ b/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, diff --git a/crates/spatialrust-py/src/lib.rs b/crates/spatialrust-py/src/lib.rs index 3144a44..67675e9 100644 --- a/crates/spatialrust-py/src/lib.rs +++ b/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>, + Bound<'py, PyArray1>, + Bound<'py, PyArray1>, + usize, +)> { + let depth_view = depth.as_array(); + let (height, width) = (depth_view.shape()[0], depth_view.shape()[1]); + let depth_image = Image::::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)?)?; diff --git a/crates/spatialrust-vision/Cargo.toml b/crates/spatialrust-vision/Cargo.toml index 6c9a2be..06c997f 100644 --- a/crates/spatialrust-vision/Cargo.toml +++ b/crates/spatialrust-vision/Cargo.toml @@ -19,13 +19,14 @@ imgproc-analysis = [] imgproc-canny = ["imgproc-filter"] feature2d = ["imgproc-filter", "resize"] geometry = ["dep:spatialrust-camera"] +odometry = ["geometry", "feature2d"] detection = [] dense = ["detection"] spatial = ["dense", "dep:spatialrust-core", "dep:spatialrust-camera"] ai-adapters = ["preprocess", "dense", "detection", "dep:spatialrust-tensor", "dep:bytemuck", "spatialrust-tensor/image"] video = ["dense", "detection"] video-adapters = ["video"] -full = ["preprocess", "warp", "imgproc-filter", "imgproc-morphology", "imgproc-analysis", "imgproc-canny", "feature2d", "geometry", "detection", "dense", "spatial", "ai-adapters", "video-adapters"] +full = ["preprocess", "warp", "imgproc-filter", "imgproc-morphology", "imgproc-analysis", "imgproc-canny", "feature2d", "geometry", "odometry", "detection", "dense", "spatial", "ai-adapters", "video-adapters"] [dependencies] spatialrust-image.workspace = true @@ -84,3 +85,8 @@ required-features = ["geometry"] name = "video" harness = false required-features = ["video"] + +[[bench]] +name = "odometry" +harness = false +required-features = ["odometry"] diff --git a/crates/spatialrust-vision/benches/odometry.rs b/crates/spatialrust-vision/benches/odometry.rs new file mode 100644 index 0000000..14976e8 --- /dev/null +++ b/crates/spatialrust-vision/benches/odometry.rs @@ -0,0 +1,26 @@ +use criterion::{black_box, criterion_group, criterion_main, Criterion}; +use spatialrust_vision::{select_keypoints_grid, GridSelectionOptions, Keypoint2}; + +fn benchmark_odometry_frontend(c: &mut Criterion) { + let keypoints = (0..4096) + .map(|index| { + Keypoint2::try_new( + (index % 128) as f32 * 15.0, + (index / 128) as f32 * 15.0, + (index % 97) as f32, + ) + .unwrap() + }) + .collect::>(); + c.bench_function("grid_select_4096", |b| { + b.iter(|| { + black_box( + select_keypoints_grid(&keypoints, 1920, 1080, GridSelectionOptions::default()) + .unwrap(), + ) + }); + }); +} + +criterion_group!(benches, benchmark_odometry_frontend); +criterion_main!(benches); diff --git a/crates/spatialrust-vision/src/lib.rs b/crates/spatialrust-vision/src/lib.rs index f4ca603..9281eea 100644 --- a/crates/spatialrust-vision/src/lib.rs +++ b/crates/spatialrust-vision/src/lib.rs @@ -37,6 +37,8 @@ mod morphology; mod multiview; #[cfg(feature = "geometry")] mod optical_flow; +#[cfg(feature = "odometry")] +mod odometry; #[cfg(feature = "geometry")] mod pnp; #[cfg(feature = "ai-adapters")] @@ -87,6 +89,8 @@ pub use morphology::*; pub use multiview::*; #[cfg(feature = "geometry")] pub use optical_flow::*; +#[cfg(feature = "odometry")] +pub use odometry::*; #[cfg(feature = "geometry")] pub use pnp::*; #[cfg(feature = "feature2d")] diff --git a/crates/spatialrust-vision/src/odometry.rs b/crates/spatialrust-vision/src/odometry.rs new file mode 100644 index 0000000..dc1019d --- /dev/null +++ b/crates/spatialrust-vision/src/odometry.rs @@ -0,0 +1,355 @@ +//! Robust feature tracking and calibrated visual odometry building blocks. + +use spatialrust_image::ImageView; +use spatialrust_math::{Vec2, Vec3}; + +use crate::{ + estimate_essential_ransac, recover_relative_pose, solve_pnp_ransac, track_points_lucas_kanade, + AbsolutePose, CameraMatrix3, Keypoint2, LucasKanadeOptions, ObjectImageCorrespondence, + PointCorrespondence2, RelativePose, RobustEstimationOptions, VisionError, VisionResult, +}; + +/// Controls deterministic spatial distribution of candidate keypoints. +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub struct GridSelectionOptions { + /// Width of one grid cell in pixels. + pub cell_width: usize, + /// Height of one grid cell in pixels. + pub cell_height: usize, + /// Maximum retained candidates per cell. + pub max_per_cell: usize, +} + +impl Default for GridSelectionOptions { + fn default() -> Self { + Self { cell_width: 32, cell_height: 32, max_per_cell: 4 } + } +} + +/// Retains the strongest keypoints in each image cell. +/// +/// Output is ordered by cell scan order, then decreasing response, with the +/// input index as the deterministic tie breaker. +pub fn select_keypoints_grid( + keypoints: &[Keypoint2], + width: usize, + height: usize, + options: GridSelectionOptions, +) -> VisionResult> { + if width == 0 + || height == 0 + || options.cell_width == 0 + || options.cell_height == 0 + || options.max_per_cell == 0 + { + return Err(VisionError::InvalidParameter( + "grid selection dimensions and limits must be positive".into(), + )); + } + let columns = width.div_ceil(options.cell_width); + let rows = height.div_ceil(options.cell_height); + let mut cells = vec![Vec::<(usize, Keypoint2)>::new(); columns * rows]; + for (index, &keypoint) in keypoints.iter().enumerate() { + if keypoint.x() < 0.0 + || keypoint.y() < 0.0 + || keypoint.x() >= width as f32 + || keypoint.y() >= height as f32 + { + continue; + } + let column = keypoint.x() as usize / options.cell_width; + let row = keypoint.y() as usize / options.cell_height; + cells[row * columns + column].push((index, keypoint)); + } + let mut selected = Vec::new(); + for cell in &mut cells { + cell.sort_by(|(left_index, left), (right_index, right)| { + right.response().total_cmp(&left.response()).then(left_index.cmp(right_index)) + }); + selected.extend(cell.iter().take(options.max_per_cell).map(|(_, point)| *point)); + } + Ok(selected) +} + +/// Forward/backward consistency settings layered over pyramidal LK. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct RobustTrackOptions { + /// Underlying LK settings in both directions. + pub lucas_kanade: LucasKanadeOptions, + /// Maximum accepted round-trip distance in pixels. + pub max_forward_backward_error: f64, +} + +impl Default for RobustTrackOptions { + fn default() -> Self { + Self { lucas_kanade: LucasKanadeOptions::default(), max_forward_backward_error: 1.0 } + } +} + +/// One target coordinate, validity decision, and round-trip error per source point. +#[derive(Clone, Debug, PartialEq)] +pub struct RobustTracks { + next_points: Vec>, + status: Vec, + forward_backward_errors: Vec, +} + +impl RobustTracks { + /// Returns one next-frame coordinate per source point. + pub fn next_points(&self) -> &[Vec2] { + &self.next_points + } + /// Returns the combined forward, backward, and threshold decision. + pub fn status(&self) -> &[bool] { + &self.status + } + /// Returns round-trip pixel error, or infinity when either direction failed. + pub fn forward_backward_errors(&self) -> &[f64] { + &self.forward_backward_errors + } + /// Returns the accepted track count. + pub fn accepted_count(&self) -> usize { + self.status.iter().filter(|&&value| value).count() + } +} + +/// Tracks points in both directions and rejects inconsistent round trips. +pub fn track_points_forward_backward( + previous: ImageView<'_, T, 1>, + next: ImageView<'_, T, 1>, + points: &[Vec2], + options: RobustTrackOptions, +) -> VisionResult { + if !options.max_forward_backward_error.is_finite() || options.max_forward_backward_error < 0.0 { + return Err(VisionError::InvalidParameter( + "forward/backward threshold must be finite and non-negative".into(), + )); + } + let forward = track_points_lucas_kanade(previous, next, points, options.lucas_kanade)?; + let backward = + track_points_lucas_kanade(next, previous, forward.next_points(), options.lucas_kanade)?; + let mut status = Vec::with_capacity(points.len()); + let mut errors = Vec::with_capacity(points.len()); + for (index, point) in points.iter().enumerate() { + let valid = forward.status()[index] && backward.status()[index]; + let error = if valid { + let dx = backward.next_points()[index].x - point.x; + let dy = backward.next_points()[index].y - point.y; + dx.hypot(dy) + } else { + f64::INFINITY + }; + status.push(valid && error <= options.max_forward_backward_error); + errors.push(error); + } + Ok(RobustTracks { + next_points: forward.next_points().to_vec(), + status, + forward_backward_errors: errors, + }) +} + +/// Scale-ambiguous monocular odometry result. +#[derive(Clone, Debug, PartialEq)] +pub struct MonocularOdometryEstimate { + /// Source-to-target pose; translation has unit, arbitrary scale. + pub pose: RelativePose, + /// Essential-matrix RANSAC inliers. + pub inliers: Vec, + /// Correspondences triangulated in front of both cameras. + pub positive_depth_count: usize, +} + +/// Estimates calibrated monocular motion with essential RANSAC and cheirality. +pub fn estimate_monocular_odometry( + correspondences: &[PointCorrespondence2], + camera: CameraMatrix3, + options: RobustEstimationOptions, +) -> VisionResult { + let estimate = estimate_essential_ransac(correspondences, camera, camera, options)?; + let inlier_pairs = correspondences + .iter() + .zip(estimate.inliers()) + .filter_map(|(&pair, &inlier)| inlier.then_some(pair)) + .collect::>(); + if inlier_pairs.len() < 8 { + return Err(VisionError::InvalidParameter( + "monocular odometry requires at least eight essential inliers".into(), + )); + } + let recovered = recover_relative_pose(*estimate.model(), &inlier_pairs, camera, camera)?; + Ok(MonocularOdometryEstimate { + pose: recovered.pose(), + inliers: estimate.inliers().to_vec(), + positive_depth_count: recovered.positive_depth_count(), + }) +} + +/// Depth filtering and PnP RANSAC controls for metric RGB-D odometry. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct RgbdOdometryOptions { + /// Multiplier converting stored depth to metres. + pub depth_scale: f64, + /// Inclusive minimum accepted metric depth. + pub min_depth: f64, + /// Inclusive maximum accepted metric depth. + pub max_depth: f64, + /// PnP robust-estimation settings (threshold in target pixels). + pub robust: RobustEstimationOptions, +} + +impl Default for RgbdOdometryOptions { + fn default() -> Self { + Self { + depth_scale: 1.0, + min_depth: 0.1, + max_depth: 100.0, + robust: RobustEstimationOptions { threshold: 1.0, ..Default::default() }, + } + } +} + +/// Metric previous-camera to current-camera odometry result. +#[derive(Clone, Debug, PartialEq)] +pub struct RgbdOdometryEstimate { + /// Full metric source-to-target pose. + pub pose: AbsolutePose, + /// RANSAC decisions over correspondences that had valid source depth. + pub inliers: Vec, + /// Number of input rows discarded for missing/out-of-range source depth. + pub rejected_depth_count: usize, +} + +/// Estimates metric motion from source depth and source-to-target pixel tracks. +pub fn estimate_rgbd_odometry( + previous_depth: ImageView<'_, T, 1>, + correspondences: &[PointCorrespondence2], + camera: CameraMatrix3, + options: RgbdOdometryOptions, +) -> VisionResult { + if !options.depth_scale.is_finite() + || options.depth_scale <= 0.0 + || !options.min_depth.is_finite() + || options.min_depth <= 0.0 + || options.max_depth.is_nan() + || options.max_depth < options.min_depth + { + return Err(VisionError::InvalidParameter("invalid RGB-D odometry depth range".into())); + } + let mut pairs = Vec::new(); + let mut rejected = 0; + for &pair in correspondences { + let pixel = pair.source(); + let x = pixel.x.round() as isize; + let y = pixel.y.round() as isize; + let Some(sample) = + (x >= 0 && y >= 0).then(|| previous_depth.get(x as usize, y as usize)).flatten() + else { + rejected += 1; + continue; + }; + let depth = sample[0].to_f64() * options.depth_scale; + if !depth.is_finite() || depth < options.min_depth || depth > options.max_depth { + rejected += 1; + continue; + } + let ray = camera.normalize_pixel(pixel); + pairs.push(ObjectImageCorrespondence::try_new( + Vec3::new(ray.x * depth, ray.y * depth, depth), + pair.target(), + )?); + } + if pairs.len() < 6 { + return Err(VisionError::InvalidParameter( + "RGB-D odometry requires at least six tracks with valid depth".into(), + )); + } + let estimate = solve_pnp_ransac(&pairs, camera, options.robust)?; + Ok(RgbdOdometryEstimate { + pose: *estimate.model(), + inliers: estimate.inliers().to_vec(), + rejected_depth_count: rejected, + }) +} + +#[cfg(test)] +mod tests { + use super::{ + estimate_rgbd_odometry, select_keypoints_grid, GridSelectionOptions, RgbdOdometryOptions, + }; + use crate::{CameraMatrix3, Keypoint2, PointCorrespondence2, RobustEstimationOptions}; + use spatialrust_camera::CameraIntrinsics; + use spatialrust_image::Image; + use spatialrust_math::Vec2; + + #[test] + fn grid_selection_keeps_strongest_per_cell() { + let points = vec![ + Keypoint2::try_new(2.0, 2.0, 1.0).unwrap(), + Keypoint2::try_new(3.0, 3.0, 4.0).unwrap(), + Keypoint2::try_new(12.0, 2.0, 2.0).unwrap(), + ]; + let selected = select_keypoints_grid( + &points, + 20, + 10, + GridSelectionOptions { cell_width: 10, cell_height: 10, max_per_cell: 1 }, + ) + .unwrap(); + assert_eq!(selected.len(), 2); + assert_eq!(selected[0].response(), 4.0); + assert_eq!(selected[1].response(), 2.0); + } + + #[test] + fn rgbd_odometry_recovers_metric_translation() { + let camera = CameraMatrix3::from_intrinsics( + CameraIntrinsics::try_new(100.0, 100.0, 50.0, 40.0, 100, 80).unwrap(), + ); + let mut depth = Image::::from_pixel(100, 80, [f32::NAN]).unwrap(); + let mut pairs = Vec::new(); + for (index, (x, y)) in [ + (20, 20), + (35, 20), + (50, 20), + (65, 20), + (80, 20), + (20, 35), + (35, 35), + (50, 35), + (65, 35), + (80, 35), + (25, 55), + (45, 55), + (65, 55), + (80, 55), + ] + .into_iter() + .enumerate() + { + let z = 1.5 + index as f64 * 0.07; + depth.get_mut(x, y).unwrap()[0] = z as f32; + let source = Vec2 { x: x as f64, y: y as f64 }; + let target = Vec2 { x: source.x + 100.0 * 0.1 / z, y: source.y }; + pairs.push(PointCorrespondence2::try_new(source, target).unwrap()); + } + let estimate = estimate_rgbd_odometry( + depth.view(), + &pairs, + camera, + RgbdOdometryOptions { + robust: RobustEstimationOptions { + threshold: 0.05, + max_iterations: 500, + ..Default::default() + }, + ..Default::default() + }, + ) + .unwrap(); + assert!((estimate.pose.translation().x - 0.1).abs() < 1e-4); + assert!(estimate.pose.translation().y.abs() < 1e-4); + assert!(estimate.pose.translation().z.abs() < 1e-4); + assert_eq!(estimate.inliers.iter().filter(|&&value| value).count(), pairs.len()); + } +} diff --git a/crates/spatialrust/Cargo.toml b/crates/spatialrust/Cargo.toml index 6a73cbe..629e236 100644 --- a/crates/spatialrust/Cargo.toml +++ b/crates/spatialrust/Cargo.toml @@ -154,6 +154,8 @@ vision-imgproc-analysis = ["vision", "spatialrust-vision/imgproc-analysis"] vision-imgproc-canny = ["vision-imgproc-filter", "spatialrust-vision/imgproc-canny"] vision-feature2d = ["vision", "spatialrust-vision/feature2d"] vision-geometry = ["vision", "camera", "spatialrust-vision/geometry"] +vision-odometry = ["vision-geometry", "vision-feature2d", "spatialrust-vision/odometry"] +mapping-vision-odometry = ["vision-odometry", "mapping", "spatialrust-mapping/vision-odometry"] vision-detection = ["vision", "spatialrust-vision/detection"] vision-dense = ["vision-detection", "spatialrust-vision/dense"] vision-spatial = ["vision-dense", "camera-rgbd", "spatialrust-vision/spatial"] @@ -169,6 +171,7 @@ vision-full = [ "vision-imgproc-canny", "vision-feature2d", "vision-geometry", + "vision-odometry", "vision-detection", "vision-dense", "vision-spatial", diff --git a/docs/API_STABILITY.md b/docs/API_STABILITY.md index 954fbb7..26fa27e 100644 --- a/docs/API_STABILITY.md +++ b/docs/API_STABILITY.md @@ -63,12 +63,12 @@ until their individual 1.0 milestones. | Camera (`camera`, `camera-rgbd`) | Pinhole/Brown-Conrady models and explicit RGB-D conversion entry points are stable | | Image IO (`image-io-*`) | Bounded codecs, typed decoded pixels, and source metadata are provisional | | AI (`ai-*`) | Backend/session, named dynamic I/O, copy policy, I/O binding, mock backend, and ONNX Runtime adapter APIs are provisional | -| Vision (`vision-*`) | Base errors/borders, resize/filter entry points, detection/dense data contracts, and Feature2D data contracts are stable; geometry, stereo, optical flow, video, and AI adapters remain provisional | +| Vision (`vision-*`) | Base errors/borders, resize/filter entry points, detection/dense data contracts, and Feature2D data contracts are stable; geometry, stereo, optical flow, odometry, video, and AI adapters remain provisional | | Tensor (`tensor-*`) | Dtype/layout/device ownership, typed host storage, external host owner, and DLPack APIs are provisional | | Records (`records`) | Versioned `SpatialRecord`, schema compatibility/migration, and chunked record streams are provisional | | Arrow (`arrow-*`) | Arrow C Data/Stream/Device bridges for point clouds are provisional | | Sync (`sync`, `sync-mcap`) | Clock domains, frame graphs, stamped records, and deterministic episode replay are provisional | -| Mapping (`mapping`) | Trajectories, relative motion estimators, pose graphs, and loop-closure candidates are provisional | +| Mapping (`mapping`) | Trajectories, relative motion estimators, pose graphs, loop closure, and feature-gated vision-odometry bridges are provisional | | Scene (`scene`, `scene-gaussian`) | TSDF/surfel/mesh reconstruction and Gaussian scene containers are provisional | | Semantic (`semantic`) | Embeddings, open-vocab labels, fusion, and semantic search are provisional | | Episode (`episode`) | Embodied episode schemas, annotations, augmentation, eval, and provenance are provisional | @@ -96,7 +96,7 @@ spatialrust- / feature- | `spatialrust-records` | Provisional | Versioned records, schema evolution, chunked host streams; Arrow-free | | `spatialrust-arrow` | Provisional | Arrow C Data/Stream/Device adapters; optional features only | | `spatialrust-sync` | Provisional | Sensor clocks, frame graphs, stamped records, deterministic replay; MCAP file codecs gated | -| `spatialrust-mapping` | Provisional | Trajectories, synthetic odometry traits, pose graphs, loop-closure candidates | +| `spatialrust-mapping` | Provisional | Trajectories, odometry traits, pose graphs, loop closure, and explicit vision motion bridges | | `spatialrust-scene` | Provisional | TSDF, surfels, meshes; Gaussian containers + CPU soft-splat behind `gaussian` | | `spatialrust-semantic` | Provisional | Embeddings, entities, multimodal fusion/search | | `spatialrust-episode` | Provisional | Episode schema, annotation, augmentation, eval, provenance | diff --git a/docs/ARCHITECTURE.md b/docs/ARCHITECTURE.md index 90ea565..ac382ac 100644 --- a/docs/ARCHITECTURE.md +++ b/docs/ARCHITECTURE.md @@ -83,6 +83,10 @@ copies; future GPU/CUDA implementations belong behind explicit backend features. Video algorithms depend on dense/detection contracts, while timestamped pull sources are isolated behind `video-adapters`; native codec/camera runtimes stay in future dedicated adapter crates/features. +Visual and RGB-D odometry kernels remain in the additive `odometry` vision +feature. Their conversion into stamped trajectory motion is a one-way optional +bridge in `spatialrust-mapping`; monocular scale and invalid depth remain +explicit at that boundary. Its `imgproc-*` features share one border extrapolation contract; `filter2d` means correlation, while true convolution is an explicitly named operation. `spatialrust-tensor` is distinct from the point-cloud chunk iterator named diff --git a/docs/ROADMAP.md b/docs/ROADMAP.md index 1d8caeb..bfc054a 100644 --- a/docs/ROADMAP.md +++ b/docs/ROADMAP.md @@ -383,7 +383,7 @@ uses the standard completion gates above and lands as one reviewable PR. | 104 | Complete | 89, 101–103 | Texture-backed GPU Image v2 and device-resident resize/filter/edge/morphology chains | | 105 | Complete | 88, 101–102 | Mono/stereo/fisheye/hand-eye calibration and bundle-adjustment contracts | | 106 | Complete | 92, 101–105 | Dense flow, tracking, background modeling, and feature-gated video stream adapters | -| 107 | Planned | 93, 101–106 | Stronger local features, robust tracking, and visual/RGB-D odometry integration | +| 107 | Complete | 93, 101–106 | Stronger local features, robust tracking, and visual/RGB-D odometry integration | | 108 | Planned | 101–107 | Feature-gated computational photography and panorama stitching | | 109 | Planned | 97, 99, 101–108 | Bounded spatial execution graph with fusion, backpressure, and named transfer receipts | | 110 | Planned | 100, 101–109 | SpatialRust Vision 1.0 conformance, audits, performance budgets, examples, and migration policy | @@ -485,3 +485,18 @@ Core video algorithms stay runtime-free in `spatialrust-vision/video`. Codec, camera, and network integrations implement `VideoFrameSource` behind additive features; frames carry sequence/time explicitly and remain owned host images. No adapter may hide a GPU transfer. + +### Epic 107 delivery slices + +| Slice | Status | Scope | Evidence | +| --- | --- | --- | --- | +| 107A | Complete | Deterministic strongest-per-cell local-feature distribution | grid ordering and response test; 4096-keypoint Criterion | +| 107B | Complete | Bidirectional pyramidal-LK consistency filtering | per-track forward/backward errors and explicit threshold | +| 107C | Complete | Scale-ambiguous calibrated monocular odometry | essential RANSAC, cheirality recovery, explicit caller scale | +| 107D | Complete | Metric RGB-D odometry from source depth and pixel tracks | depth filtering, PnP RANSAC, synthetic metric translation test | +| 107E | Complete | Mapping/Python integration and OpenCV receipt | `mapping-vision-odometry`, Python binding, `solvePnPRansac` parity | + +The vision layer reports source-to-target motion and never invents monocular +scale. `spatialrust-mapping` accepts scale explicitly for monocular estimates +and preserves metric RGB-D translation. Invalid source depths are counted, +not silently filled or copied to another device. diff --git a/notes/2026-07-15_epic107_odometry.md b/notes/2026-07-15_epic107_odometry.md new file mode 100644 index 0000000..43bfa2f --- /dev/null +++ b/notes/2026-07-15_epic107_odometry.md @@ -0,0 +1,14 @@ +# Epic 107: robust visual and RGB-D odometry + +Epic 107 composes the calibrated geometry delivered by Epic 88 with mapping +contracts from Epic 93. It adds deterministic grid selection, forward/backward +LK diagnostics, scale-explicit monocular odometry, metric RGB-D odometry, and +optional mapping bridges. No runtime, codec, or device dependency enters the +portable algorithms. + +On a deterministic 48-track RGB-D scene with one invalid-depth row, +SpatialRust recovered translation with maximum error +`1.0408919004500916e-09` metres. Against OpenCV 4.10 `solvePnPRansac`, maximum +translation difference was `2.1377243378251087e-08` metres and maximum rotation +matrix difference was `3.2804083622441615e-10`; both accepted 48 inliers and +SpatialRust reported the one rejected depth explicitly.