spatialrust_vision/
odometry.rs

1//! Robust feature tracking and calibrated visual odometry building blocks.
2
3use spatialrust_image::ImageView;
4use spatialrust_math::{Vec2, Vec3};
5
6use crate::{
7    estimate_essential_ransac, recover_relative_pose, solve_pnp_ransac, track_points_lucas_kanade,
8    AbsolutePose, CameraMatrix3, Keypoint2, LucasKanadeOptions, ObjectImageCorrespondence,
9    PointCorrespondence2, RelativePose, RobustEstimationOptions, VisionError, VisionResult,
10};
11
12/// Controls deterministic spatial distribution of candidate keypoints.
13#[derive(Clone, Copy, Debug, PartialEq, Eq)]
14pub struct GridSelectionOptions {
15    /// Width of one grid cell in pixels.
16    pub cell_width: usize,
17    /// Height of one grid cell in pixels.
18    pub cell_height: usize,
19    /// Maximum retained candidates per cell.
20    pub max_per_cell: usize,
21}
22
23impl Default for GridSelectionOptions {
24    fn default() -> Self {
25        Self { cell_width: 32, cell_height: 32, max_per_cell: 4 }
26    }
27}
28
29/// Retains the strongest keypoints in each image cell.
30///
31/// Output is ordered by cell scan order, then decreasing response, with the
32/// input index as the deterministic tie breaker.
33pub fn select_keypoints_grid(
34    keypoints: &[Keypoint2],
35    width: usize,
36    height: usize,
37    options: GridSelectionOptions,
38) -> VisionResult<Vec<Keypoint2>> {
39    if width == 0
40        || height == 0
41        || options.cell_width == 0
42        || options.cell_height == 0
43        || options.max_per_cell == 0
44    {
45        return Err(VisionError::InvalidParameter(
46            "grid selection dimensions and limits must be positive".into(),
47        ));
48    }
49    let columns = width.div_ceil(options.cell_width);
50    let rows = height.div_ceil(options.cell_height);
51    let mut cells = vec![Vec::<(usize, Keypoint2)>::new(); columns * rows];
52    for (index, &keypoint) in keypoints.iter().enumerate() {
53        if keypoint.x() < 0.0
54            || keypoint.y() < 0.0
55            || keypoint.x() >= width as f32
56            || keypoint.y() >= height as f32
57        {
58            continue;
59        }
60        let column = keypoint.x() as usize / options.cell_width;
61        let row = keypoint.y() as usize / options.cell_height;
62        cells[row * columns + column].push((index, keypoint));
63    }
64    let mut selected = Vec::new();
65    for cell in &mut cells {
66        cell.sort_by(|(left_index, left), (right_index, right)| {
67            right.response().total_cmp(&left.response()).then(left_index.cmp(right_index))
68        });
69        selected.extend(cell.iter().take(options.max_per_cell).map(|(_, point)| *point));
70    }
71    Ok(selected)
72}
73
74/// Forward/backward consistency settings layered over pyramidal LK.
75#[derive(Clone, Copy, Debug, PartialEq)]
76pub struct RobustTrackOptions {
77    /// Underlying LK settings in both directions.
78    pub lucas_kanade: LucasKanadeOptions,
79    /// Maximum accepted round-trip distance in pixels.
80    pub max_forward_backward_error: f64,
81}
82
83impl Default for RobustTrackOptions {
84    fn default() -> Self {
85        Self { lucas_kanade: LucasKanadeOptions::default(), max_forward_backward_error: 1.0 }
86    }
87}
88
89/// One target coordinate, validity decision, and round-trip error per source point.
90#[derive(Clone, Debug, PartialEq)]
91pub struct RobustTracks {
92    next_points: Vec<Vec2<f64>>,
93    status: Vec<bool>,
94    forward_backward_errors: Vec<f64>,
95}
96
97impl RobustTracks {
98    /// Returns one next-frame coordinate per source point.
99    pub fn next_points(&self) -> &[Vec2<f64>] {
100        &self.next_points
101    }
102    /// Returns the combined forward, backward, and threshold decision.
103    pub fn status(&self) -> &[bool] {
104        &self.status
105    }
106    /// Returns round-trip pixel error, or infinity when either direction failed.
107    pub fn forward_backward_errors(&self) -> &[f64] {
108        &self.forward_backward_errors
109    }
110    /// Returns the accepted track count.
111    pub fn accepted_count(&self) -> usize {
112        self.status.iter().filter(|&&value| value).count()
113    }
114}
115
116/// Tracks points in both directions and rejects inconsistent round trips.
117pub fn track_points_forward_backward<T: crate::PixelComponent>(
118    previous: ImageView<'_, T, 1>,
119    next: ImageView<'_, T, 1>,
120    points: &[Vec2<f64>],
121    options: RobustTrackOptions,
122) -> VisionResult<RobustTracks> {
123    if !options.max_forward_backward_error.is_finite() || options.max_forward_backward_error < 0.0 {
124        return Err(VisionError::InvalidParameter(
125            "forward/backward threshold must be finite and non-negative".into(),
126        ));
127    }
128    let forward = track_points_lucas_kanade(previous, next, points, options.lucas_kanade)?;
129    let backward =
130        track_points_lucas_kanade(next, previous, forward.next_points(), options.lucas_kanade)?;
131    let mut status = Vec::with_capacity(points.len());
132    let mut errors = Vec::with_capacity(points.len());
133    for (index, point) in points.iter().enumerate() {
134        let valid = forward.status()[index] && backward.status()[index];
135        let error = if valid {
136            let dx = backward.next_points()[index].x - point.x;
137            let dy = backward.next_points()[index].y - point.y;
138            dx.hypot(dy)
139        } else {
140            f64::INFINITY
141        };
142        status.push(valid && error <= options.max_forward_backward_error);
143        errors.push(error);
144    }
145    Ok(RobustTracks {
146        next_points: forward.next_points().to_vec(),
147        status,
148        forward_backward_errors: errors,
149    })
150}
151
152/// Scale-ambiguous monocular odometry result.
153#[derive(Clone, Debug, PartialEq)]
154pub struct MonocularOdometryEstimate {
155    /// Source-to-target pose; translation has unit, arbitrary scale.
156    pub pose: RelativePose,
157    /// Essential-matrix RANSAC inliers.
158    pub inliers: Vec<bool>,
159    /// Correspondences triangulated in front of both cameras.
160    pub positive_depth_count: usize,
161}
162
163/// Estimates calibrated monocular motion with essential RANSAC and cheirality.
164pub fn estimate_monocular_odometry(
165    correspondences: &[PointCorrespondence2],
166    camera: CameraMatrix3,
167    options: RobustEstimationOptions,
168) -> VisionResult<MonocularOdometryEstimate> {
169    let estimate = estimate_essential_ransac(correspondences, camera, camera, options)?;
170    let inlier_pairs = correspondences
171        .iter()
172        .zip(estimate.inliers())
173        .filter_map(|(&pair, &inlier)| inlier.then_some(pair))
174        .collect::<Vec<_>>();
175    if inlier_pairs.len() < 8 {
176        return Err(VisionError::InvalidParameter(
177            "monocular odometry requires at least eight essential inliers".into(),
178        ));
179    }
180    let recovered = recover_relative_pose(*estimate.model(), &inlier_pairs, camera, camera)?;
181    Ok(MonocularOdometryEstimate {
182        pose: recovered.pose(),
183        inliers: estimate.inliers().to_vec(),
184        positive_depth_count: recovered.positive_depth_count(),
185    })
186}
187
188/// Depth filtering and PnP RANSAC controls for metric RGB-D odometry.
189#[derive(Clone, Copy, Debug, PartialEq)]
190pub struct RgbdOdometryOptions {
191    /// Multiplier converting stored depth to metres.
192    pub depth_scale: f64,
193    /// Inclusive minimum accepted metric depth.
194    pub min_depth: f64,
195    /// Inclusive maximum accepted metric depth.
196    pub max_depth: f64,
197    /// PnP robust-estimation settings (threshold in target pixels).
198    pub robust: RobustEstimationOptions,
199}
200
201impl Default for RgbdOdometryOptions {
202    fn default() -> Self {
203        Self {
204            depth_scale: 1.0,
205            min_depth: 0.1,
206            max_depth: 100.0,
207            robust: RobustEstimationOptions { threshold: 1.0, ..Default::default() },
208        }
209    }
210}
211
212/// Metric previous-camera to current-camera odometry result.
213#[derive(Clone, Debug, PartialEq)]
214pub struct RgbdOdometryEstimate {
215    /// Full metric source-to-target pose.
216    pub pose: AbsolutePose,
217    /// RANSAC decisions over correspondences that had valid source depth.
218    pub inliers: Vec<bool>,
219    /// Number of input rows discarded for missing/out-of-range source depth.
220    pub rejected_depth_count: usize,
221}
222
223/// Estimates metric motion from source depth and source-to-target pixel tracks.
224pub fn estimate_rgbd_odometry<T: crate::PixelComponent>(
225    previous_depth: ImageView<'_, T, 1>,
226    correspondences: &[PointCorrespondence2],
227    camera: CameraMatrix3,
228    options: RgbdOdometryOptions,
229) -> VisionResult<RgbdOdometryEstimate> {
230    if !options.depth_scale.is_finite()
231        || options.depth_scale <= 0.0
232        || !options.min_depth.is_finite()
233        || options.min_depth <= 0.0
234        || options.max_depth.is_nan()
235        || options.max_depth < options.min_depth
236    {
237        return Err(VisionError::InvalidParameter("invalid RGB-D odometry depth range".into()));
238    }
239    let mut pairs = Vec::new();
240    let mut rejected = 0;
241    for &pair in correspondences {
242        let pixel = pair.source();
243        let x = pixel.x.round() as isize;
244        let y = pixel.y.round() as isize;
245        let Some(sample) =
246            (x >= 0 && y >= 0).then(|| previous_depth.get(x as usize, y as usize)).flatten()
247        else {
248            rejected += 1;
249            continue;
250        };
251        let depth = sample[0].to_f64() * options.depth_scale;
252        if !depth.is_finite() || depth < options.min_depth || depth > options.max_depth {
253            rejected += 1;
254            continue;
255        }
256        let ray = camera.normalize_pixel(pixel);
257        pairs.push(ObjectImageCorrespondence::try_new(
258            Vec3::new(ray.x * depth, ray.y * depth, depth),
259            pair.target(),
260        )?);
261    }
262    if pairs.len() < 6 {
263        return Err(VisionError::InvalidParameter(
264            "RGB-D odometry requires at least six tracks with valid depth".into(),
265        ));
266    }
267    let estimate = solve_pnp_ransac(&pairs, camera, options.robust)?;
268    Ok(RgbdOdometryEstimate {
269        pose: *estimate.model(),
270        inliers: estimate.inliers().to_vec(),
271        rejected_depth_count: rejected,
272    })
273}
274
275#[cfg(test)]
276mod tests {
277    use super::{
278        estimate_rgbd_odometry, select_keypoints_grid, GridSelectionOptions, RgbdOdometryOptions,
279    };
280    use crate::{CameraMatrix3, Keypoint2, PointCorrespondence2, RobustEstimationOptions};
281    use spatialrust_camera::CameraIntrinsics;
282    use spatialrust_image::Image;
283    use spatialrust_math::Vec2;
284
285    #[test]
286    fn grid_selection_keeps_strongest_per_cell() {
287        let points = vec![
288            Keypoint2::try_new(2.0, 2.0, 1.0).unwrap(),
289            Keypoint2::try_new(3.0, 3.0, 4.0).unwrap(),
290            Keypoint2::try_new(12.0, 2.0, 2.0).unwrap(),
291        ];
292        let selected = select_keypoints_grid(
293            &points,
294            20,
295            10,
296            GridSelectionOptions { cell_width: 10, cell_height: 10, max_per_cell: 1 },
297        )
298        .unwrap();
299        assert_eq!(selected.len(), 2);
300        assert_eq!(selected[0].response(), 4.0);
301        assert_eq!(selected[1].response(), 2.0);
302    }
303
304    #[test]
305    fn rgbd_odometry_recovers_metric_translation() {
306        let camera = CameraMatrix3::from_intrinsics(
307            CameraIntrinsics::try_new(100.0, 100.0, 50.0, 40.0, 100, 80).unwrap(),
308        );
309        let mut depth = Image::<f32, 1>::from_pixel(100, 80, [f32::NAN]).unwrap();
310        let mut pairs = Vec::new();
311        for (index, (x, y)) in [
312            (20, 20),
313            (35, 20),
314            (50, 20),
315            (65, 20),
316            (80, 20),
317            (20, 35),
318            (35, 35),
319            (50, 35),
320            (65, 35),
321            (80, 35),
322            (25, 55),
323            (45, 55),
324            (65, 55),
325            (80, 55),
326        ]
327        .into_iter()
328        .enumerate()
329        {
330            let z = 1.5 + index as f64 * 0.07;
331            depth.get_mut(x, y).unwrap()[0] = z as f32;
332            let source = Vec2 { x: x as f64, y: y as f64 };
333            let target = Vec2 { x: source.x + 100.0 * 0.1 / z, y: source.y };
334            pairs.push(PointCorrespondence2::try_new(source, target).unwrap());
335        }
336        let estimate = estimate_rgbd_odometry(
337            depth.view(),
338            &pairs,
339            camera,
340            RgbdOdometryOptions {
341                robust: RobustEstimationOptions {
342                    threshold: 0.05,
343                    max_iterations: 500,
344                    ..Default::default()
345                },
346                ..Default::default()
347            },
348        )
349        .unwrap();
350        assert!((estimate.pose.translation().x - 0.1).abs() < 1e-4);
351        assert!(estimate.pose.translation().y.abs() < 1e-4);
352        assert!(estimate.pose.translation().z.abs() < 1e-4);
353        assert_eq!(estimate.inliers.iter().filter(|&&value| value).count(), pairs.len());
354    }
355}
spatialrust_vision/odometry.rs

spatialrust_vision/
odometry.rs
