Skip to main content

rust_robotics_slam/
visual_frontend.rs

1//! Decoder-independent sparse visual frontend for EuRoC-style sequences.
2//!
3//! Corners are distributed with non-maximum suppression, tracked with
4//! pyramidal Lucas-Kanade optical flow, checked in both directions, and
5//! triangulated from an IMU-predicted camera trajectory.
6
7use nalgebra::{Matrix3, Matrix4, Vector2, Vector3};
8use rust_robotics_optimization::{OptimizationError, OptimizationResult};
9
10use crate::dataset::{EurocFeatureTracks, FeatureTrackObservation, TrackedLandmark};
11
12/// Owned eight-bit grayscale image.
13#[derive(Debug, Clone, PartialEq, Eq)]
14pub struct GrayImage {
15    width: usize,
16    height: usize,
17    pixels: Vec<u8>,
18}
19
20impl GrayImage {
21    pub fn new(width: usize, height: usize, pixels: Vec<u8>) -> OptimizationResult<Self> {
22        if width < 8 || height < 8 || pixels.len() != width * height {
23            return Err(OptimizationError::InvalidParameter(
24                "gray image dimensions or pixel count are invalid".into(),
25            ));
26        }
27        Ok(Self {
28            width,
29            height,
30            pixels,
31        })
32    }
33
34    pub fn width(&self) -> usize {
35        self.width
36    }
37
38    pub fn height(&self) -> usize {
39        self.height
40    }
41
42    pub fn pixels(&self) -> &[u8] {
43        &self.pixels
44    }
45
46    fn sample(&self, point: Vector2<f64>) -> Option<f64> {
47        if point.x < 0.0
48            || point.y < 0.0
49            || point.x >= (self.width - 1) as f64
50            || point.y >= (self.height - 1) as f64
51        {
52            return None;
53        }
54        let x = point.x.floor() as usize;
55        let y = point.y.floor() as usize;
56        let dx = point.x - x as f64;
57        let dy = point.y - y as f64;
58        let at = |column: usize, row: usize| self.pixels[row * self.width + column] as f64;
59        Some(
60            at(x, y) * (1.0 - dx) * (1.0 - dy)
61                + at(x + 1, y) * dx * (1.0 - dy)
62                + at(x, y + 1) * (1.0 - dx) * dy
63                + at(x + 1, y + 1) * dx * dy,
64        )
65    }
66
67    fn half_size(&self) -> Self {
68        let width = self.width / 2;
69        let height = self.height / 2;
70        let mut pixels = vec![0; width * height];
71        for y in 0..height {
72            for x in 0..width {
73                let source = 2 * y * self.width + 2 * x;
74                let sum = self.pixels[source] as u16
75                    + self.pixels[source + 1] as u16
76                    + self.pixels[source + self.width] as u16
77                    + self.pixels[source + self.width + 1] as u16;
78                pixels[y * width + x] = (sum / 4) as u8;
79            }
80        }
81        Self {
82            width,
83            height,
84            pixels,
85        }
86    }
87}
88
89/// Sparse tracking parameters.
90#[derive(Debug, Clone, Copy)]
91pub struct FeatureFrontendConfig {
92    pub max_features: usize,
93    pub quality_level: f64,
94    pub min_distance: f64,
95    pub pyramid_levels: usize,
96    pub window_radius: usize,
97    pub max_iterations: usize,
98    pub convergence_epsilon: f64,
99    pub forward_backward_tolerance: f64,
100    pub max_patch_rms: f64,
101    pub min_track_length: usize,
102}
103
104impl Default for FeatureFrontendConfig {
105    fn default() -> Self {
106        Self {
107            max_features: 240,
108            quality_level: 0.01,
109            min_distance: 12.0,
110            pyramid_levels: 3,
111            window_radius: 3,
112            max_iterations: 12,
113            convergence_epsilon: 0.02,
114            forward_backward_tolerance: 1.0,
115            max_patch_rms: 30.0,
116            min_track_length: 3,
117        }
118    }
119}
120
121/// One 2D observation retained by the frontend.
122#[derive(Debug, Clone, Copy, PartialEq)]
123pub struct ImageFeatureObservation {
124    pub frame: usize,
125    pub pixel: Vector2<f64>,
126}
127
128/// A feature identity followed across consecutive frames.
129#[derive(Debug, Clone, PartialEq)]
130pub struct ImageFeatureTrack {
131    pub id: usize,
132    pub observations: Vec<ImageFeatureObservation>,
133}
134
135/// Stateful streaming tracker. Only the previous image pyramid is retained.
136#[derive(Debug)]
137pub struct FeatureTracker {
138    config: FeatureFrontendConfig,
139    previous: Option<Vec<GrayImage>>,
140    tracks: Vec<ImageFeatureTrack>,
141    active: Vec<usize>,
142    frame_count: usize,
143    accepted_matches: usize,
144}
145
146impl FeatureTracker {
147    pub fn new(config: FeatureFrontendConfig) -> OptimizationResult<Self> {
148        validate_config(&config)?;
149        Ok(Self {
150            config,
151            previous: None,
152            tracks: Vec::new(),
153            active: Vec::new(),
154            frame_count: 0,
155            accepted_matches: 0,
156        })
157    }
158
159    /// Adds one image in timestamp order.
160    pub fn process(&mut self, image: GrayImage) -> OptimizationResult<()> {
161        let pyramid = image_pyramid(image, self.config.pyramid_levels);
162        if let Some(previous) = &self.previous {
163            let mut next_active = Vec::new();
164            for &track_index in &self.active {
165                let previous_pixel = self.tracks[track_index]
166                    .observations
167                    .last()
168                    .expect("active track has an observation")
169                    .pixel;
170                let Some(current_pixel) =
171                    track_pyramid(previous, &pyramid, previous_pixel, &self.config)
172                else {
173                    continue;
174                };
175                let Some(backward_pixel) =
176                    track_pyramid(&pyramid, previous, current_pixel, &self.config)
177                else {
178                    continue;
179                };
180                if (backward_pixel - previous_pixel).norm() > self.config.forward_backward_tolerance
181                {
182                    continue;
183                }
184                self.tracks[track_index]
185                    .observations
186                    .push(ImageFeatureObservation {
187                        frame: self.frame_count,
188                        pixel: current_pixel,
189                    });
190                next_active.push(track_index);
191                self.accepted_matches += 1;
192            }
193            self.active = next_active;
194        }
195
196        let occupied = self
197            .active
198            .iter()
199            .map(|&index| {
200                self.tracks[index]
201                    .observations
202                    .last()
203                    .expect("active track has an observation")
204                    .pixel
205            })
206            .collect::<Vec<_>>();
207        let capacity = self.config.max_features.saturating_sub(self.active.len());
208        let new_points = detect_corners(&pyramid[0], &occupied, capacity, &self.config);
209        for pixel in new_points {
210            let index = self.tracks.len();
211            self.tracks.push(ImageFeatureTrack {
212                id: index,
213                observations: vec![ImageFeatureObservation {
214                    frame: self.frame_count,
215                    pixel,
216                }],
217            });
218            self.active.push(index);
219        }
220        self.previous = Some(pyramid);
221        self.frame_count += 1;
222        Ok(())
223    }
224
225    pub fn frame_count(&self) -> usize {
226        self.frame_count
227    }
228
229    pub fn accepted_matches(&self) -> usize {
230        self.accepted_matches
231    }
232
233    pub fn finish(self) -> Vec<ImageFeatureTrack> {
234        self.tracks
235            .into_iter()
236            .filter(|track| track.observations.len() >= self.config.min_track_length)
237            .collect()
238    }
239}
240
241/// Landmark initialization parameters.
242#[derive(Debug, Clone, Copy)]
243pub struct TriangulationConfig {
244    pub min_parallax_radians: f64,
245    pub min_depth: f64,
246    pub max_reprojection_error: f64,
247}
248
249impl Default for TriangulationConfig {
250    fn default() -> Self {
251        Self {
252            min_parallax_radians: 0.005,
253            min_depth: 0.1,
254            max_reprojection_error: 8.0,
255        }
256    }
257}
258
259/// Triangulates 2D tracks and converts them to the VIO sidecar representation.
260pub fn triangulate_tracks(
261    tracks: &[ImageFeatureTrack],
262    timestamps_ns: &[i64],
263    world_from_camera: &[Matrix4<f64>],
264    intrinsics: [f64; 4],
265    config: TriangulationConfig,
266) -> OptimizationResult<EurocFeatureTracks> {
267    if timestamps_ns.len() != world_from_camera.len() || timestamps_ns.is_empty() {
268        return Err(OptimizationError::InvalidParameter(
269            "timestamps and camera trajectory must have the same nonzero length".into(),
270        ));
271    }
272    if config.min_parallax_radians <= 0.0
273        || config.min_depth <= 0.0
274        || config.max_reprojection_error <= 0.0
275    {
276        return Err(OptimizationError::InvalidParameter(
277            "triangulation thresholds must be positive".into(),
278        ));
279    }
280
281    let mut landmarks = Vec::new();
282    let mut observations = Vec::new();
283    for track in tracks {
284        if track
285            .observations
286            .iter()
287            .any(|observation| observation.frame >= world_from_camera.len())
288        {
289            return Err(OptimizationError::InvalidParameter(
290                "feature observation frame is out of range".into(),
291            ));
292        }
293        let Some(position) = triangulate_one(track, world_from_camera, intrinsics, config)? else {
294            continue;
295        };
296        let id = landmarks.len();
297        landmarks.push(TrackedLandmark { id, position });
298        observations.extend(
299            track
300                .observations
301                .iter()
302                .map(|observation| FeatureTrackObservation {
303                    timestamp_ns: timestamps_ns[observation.frame],
304                    landmark_id: id,
305                    pixel: observation.pixel,
306                }),
307        );
308    }
309    if landmarks.is_empty() {
310        return Err(OptimizationError::InvalidParameter(
311            "no feature track passed triangulation".into(),
312        ));
313    }
314    Ok(EurocFeatureTracks {
315        landmarks,
316        observations,
317    })
318}
319
320fn validate_config(config: &FeatureFrontendConfig) -> OptimizationResult<()> {
321    if config.max_features == 0
322        || !(0.0..=1.0).contains(&config.quality_level)
323        || config.quality_level == 0.0
324        || config.min_distance < 1.0
325        || config.pyramid_levels == 0
326        || config.window_radius < 2
327        || config.max_iterations == 0
328        || config.convergence_epsilon <= 0.0
329        || config.forward_backward_tolerance <= 0.0
330        || config.max_patch_rms <= 0.0
331        || config.min_track_length < 2
332    {
333        return Err(OptimizationError::InvalidParameter(
334            "invalid visual frontend configuration".into(),
335        ));
336    }
337    Ok(())
338}
339
340fn image_pyramid(image: GrayImage, levels: usize) -> Vec<GrayImage> {
341    let mut pyramid = vec![image];
342    while pyramid.len() < levels {
343        let previous = pyramid.last().expect("base image exists");
344        if previous.width < 16 || previous.height < 16 {
345            break;
346        }
347        pyramid.push(previous.half_size());
348    }
349    pyramid
350}
351
352fn detect_corners(
353    image: &GrayImage,
354    occupied: &[Vector2<f64>],
355    capacity: usize,
356    config: &FeatureFrontendConfig,
357) -> Vec<Vector2<f64>> {
358    if capacity == 0 {
359        return Vec::new();
360    }
361    let margin = config.window_radius + 2;
362    let mut scores = vec![0.0; image.width * image.height];
363    let mut maximum: f64 = 0.0;
364    for y in margin..image.height - margin {
365        for x in margin..image.width - margin {
366            let mut xx = 0.0;
367            let mut xy = 0.0;
368            let mut yy = 0.0;
369            for row in y - 1..=y + 1 {
370                for column in x - 1..=x + 1 {
371                    let gx = image.pixels[row * image.width + column + 1] as f64
372                        - image.pixels[row * image.width + column - 1] as f64;
373                    let gy = image.pixels[(row + 1) * image.width + column] as f64
374                        - image.pixels[(row - 1) * image.width + column] as f64;
375                    xx += gx * gx;
376                    xy += gx * gy;
377                    yy += gy * gy;
378                }
379            }
380            let trace = xx + yy;
381            let score = 0.5 * (trace - ((xx - yy).powi(2) + 4.0 * xy * xy).sqrt());
382            maximum = maximum.max(score);
383            scores[y * image.width + x] = score;
384        }
385    }
386    let threshold = maximum * config.quality_level;
387    let mut candidates = Vec::new();
388    for y in margin + 1..image.height - margin - 1 {
389        for x in margin + 1..image.width - margin - 1 {
390            let score = scores[y * image.width + x];
391            if score < threshold || score <= 0.0 {
392                continue;
393            }
394            let is_local_maximum = (y - 1..=y + 1).all(|row| {
395                (x - 1..=x + 1).all(|column| {
396                    (row == y && column == x) || scores[row * image.width + column] <= score
397                })
398            });
399            if is_local_maximum {
400                candidates.push((score, Vector2::new(x as f64, y as f64)));
401            }
402        }
403    }
404    candidates.sort_by(|left, right| right.0.total_cmp(&left.0));
405
406    let minimum_squared = config.min_distance * config.min_distance;
407    let mut selected = occupied.to_vec();
408    let existing = selected.len();
409    for (_, point) in candidates {
410        if selected
411            .iter()
412            .all(|other| (point - other).norm_squared() >= minimum_squared)
413        {
414            selected.push(point);
415            if selected.len() - existing == capacity {
416                break;
417            }
418        }
419    }
420    selected.into_iter().skip(existing).collect()
421}
422
423fn track_pyramid(
424    source: &[GrayImage],
425    destination: &[GrayImage],
426    source_point: Vector2<f64>,
427    config: &FeatureFrontendConfig,
428) -> Option<Vector2<f64>> {
429    let levels = source.len().min(destination.len());
430    let mut destination_point = Vector2::zeros();
431    for level in (0..levels).rev() {
432        let scale = (1_usize << level) as f64;
433        let point = source_point / scale;
434        if level == levels - 1 {
435            destination_point = point;
436        } else {
437            destination_point *= 2.0;
438        }
439        destination_point = track_level(
440            &source[level],
441            &destination[level],
442            point,
443            destination_point,
444            config,
445        )?;
446    }
447    Some(destination_point)
448}
449
450fn track_level(
451    source: &GrayImage,
452    destination: &GrayImage,
453    source_point: Vector2<f64>,
454    mut destination_point: Vector2<f64>,
455    config: &FeatureFrontendConfig,
456) -> Option<Vector2<f64>> {
457    let radius = config.window_radius as isize;
458    let mut final_squared_error = 0.0;
459    let mut count = 0;
460    for _ in 0..config.max_iterations {
461        let mut hessian = Matrix3::<f64>::zeros();
462        let mut gradient = Vector2::<f64>::zeros();
463        final_squared_error = 0.0;
464        count = 0;
465        for dy in -radius..=radius {
466            for dx in -radius..=radius {
467                let offset = Vector2::new(dx as f64, dy as f64);
468                let reference = source.sample(source_point + offset)?;
469                let target_point = destination_point + offset;
470                let target = destination.sample(target_point)?;
471                let gx = 0.5
472                    * (destination.sample(target_point + Vector2::new(1.0, 0.0))?
473                        - destination.sample(target_point - Vector2::new(1.0, 0.0))?);
474                let gy = 0.5
475                    * (destination.sample(target_point + Vector2::new(0.0, 1.0))?
476                        - destination.sample(target_point - Vector2::new(0.0, 1.0))?);
477                let residual = target - reference;
478                hessian[(0, 0)] += gx * gx;
479                hessian[(0, 1)] += gx * gy;
480                hessian[(1, 0)] += gx * gy;
481                hessian[(1, 1)] += gy * gy;
482                gradient.x += gx * residual;
483                gradient.y += gy * residual;
484                final_squared_error += residual * residual;
485                count += 1;
486            }
487        }
488        let determinant = hessian[(0, 0)] * hessian[(1, 1)] - hessian[(0, 1)].powi(2);
489        if determinant < 1.0e-6 {
490            return None;
491        }
492        let delta = Vector2::<f64>::new(
493            (-hessian[(1, 1)] * gradient.x + hessian[(0, 1)] * gradient.y) / determinant,
494            (hessian[(1, 0)] * gradient.x - hessian[(0, 0)] * gradient.y) / determinant,
495        );
496        if !delta.iter().all(|value| value.is_finite()) || delta.norm() > 3.0 {
497            return None;
498        }
499        destination_point += delta;
500        if delta.norm() < config.convergence_epsilon {
501            break;
502        }
503    }
504    let rms = (final_squared_error / count as f64).sqrt();
505    (rms <= config.max_patch_rms).then_some(destination_point)
506}
507
508fn triangulate_one(
509    track: &ImageFeatureTrack,
510    poses: &[Matrix4<f64>],
511    intrinsics: [f64; 4],
512    config: TriangulationConfig,
513) -> OptimizationResult<Option<Vector3<f64>>> {
514    if track.observations.len() < 2 {
515        return Ok(None);
516    }
517    let [fx, fy, cx, cy] = intrinsics;
518    if fx <= 0.0 || fy <= 0.0 {
519        return Err(OptimizationError::InvalidParameter(
520            "camera focal lengths must be positive".into(),
521        ));
522    }
523    let mut rays = Vec::with_capacity(track.observations.len());
524    for observation in &track.observations {
525        let pose = &poses[observation.frame];
526        let origin = pose.fixed_view::<3, 1>(0, 3).into_owned();
527        let camera_ray = Vector3::new(
528            (observation.pixel.x - cx) / fx,
529            (observation.pixel.y - cy) / fy,
530            1.0,
531        )
532        .normalize();
533        let direction = pose.fixed_view::<3, 3>(0, 0) * camera_ray;
534        rays.push((origin, direction));
535    }
536    let maximum_parallax = rays
537        .iter()
538        .enumerate()
539        .flat_map(|(index, left)| rays[index + 1..].iter().map(move |right| (left, right)))
540        .map(|(left, right)| left.1.dot(&right.1).clamp(-1.0, 1.0).acos())
541        .fold(0.0_f64, f64::max);
542    if maximum_parallax < config.min_parallax_radians {
543        return Ok(None);
544    }
545
546    let mut normal = Matrix3::zeros();
547    let mut right_hand_side = Vector3::zeros();
548    for (origin, direction) in &rays {
549        let projection = Matrix3::identity() - direction * direction.transpose();
550        normal += projection;
551        right_hand_side += projection * origin;
552    }
553    let Some(position) = normal.lu().solve(&right_hand_side) else {
554        return Ok(None);
555    };
556    for (observation, pose) in track.observations.iter().zip(
557        track
558            .observations
559            .iter()
560            .map(|observation| &poses[observation.frame]),
561    ) {
562        let camera_from_world_rotation = pose.fixed_view::<3, 3>(0, 0).transpose();
563        let camera_point =
564            camera_from_world_rotation * (position - pose.fixed_view::<3, 1>(0, 3).into_owned());
565        if camera_point.z <= config.min_depth {
566            return Ok(None);
567        }
568        let projected = Vector2::new(
569            fx * camera_point.x / camera_point.z + cx,
570            fy * camera_point.y / camera_point.z + cy,
571        );
572        if (projected - observation.pixel).norm() > config.max_reprojection_error {
573            return Ok(None);
574        }
575    }
576    Ok(Some(position))
577}
578
579#[cfg(test)]
580mod tests {
581    use super::*;
582
583    fn synthetic_image(shift_x: usize) -> GrayImage {
584        let width = 96;
585        let height = 72;
586        let mut pixels = vec![20; width * height];
587        for &(x, y) in &[(18, 18), (42, 20), (70, 22), (24, 48), (58, 50)] {
588            let x = x + shift_x;
589            for row in y - 3..=y + 3 {
590                for column in x - 3..=x + 3 {
591                    pixels[row * width + column] = if (row + column) % 2 == 0 { 240 } else { 80 };
592                }
593            }
594        }
595        GrayImage::new(width, height, pixels).unwrap()
596    }
597
598    #[test]
599    fn pyramidal_tracker_follows_translation() {
600        let config = FeatureFrontendConfig {
601            min_distance: 8.0,
602            min_track_length: 3,
603            ..FeatureFrontendConfig::default()
604        };
605        let mut tracker = FeatureTracker::new(config).unwrap();
606        tracker.process(synthetic_image(0)).unwrap();
607        tracker.process(synthetic_image(2)).unwrap();
608        tracker.process(synthetic_image(4)).unwrap();
609        assert!(tracker.accepted_matches() >= 6);
610        let tracks = tracker.finish();
611        assert!(tracks.len() >= 3);
612        for track in tracks {
613            let displacement =
614                track.observations.last().unwrap().pixel - track.observations[0].pixel;
615            assert!((displacement.x - 4.0).abs() < 0.25);
616            assert!(displacement.y.abs() < 0.25);
617        }
618    }
619
620    #[test]
621    fn triangulation_recovers_known_point() {
622        let intrinsics = [200.0, 200.0, 48.0, 36.0];
623        let point = Vector3::new(0.2, -0.1, 4.0);
624        let mut poses = vec![Matrix4::identity(); 3];
625        poses[1][(0, 3)] = 0.2;
626        poses[2][(0, 3)] = 0.4;
627        let observations = poses
628            .iter()
629            .enumerate()
630            .map(|(frame, pose)| {
631                let camera_point = point - pose.fixed_view::<3, 1>(0, 3).into_owned();
632                ImageFeatureObservation {
633                    frame,
634                    pixel: Vector2::new(
635                        intrinsics[0] * camera_point.x / camera_point.z + intrinsics[2],
636                        intrinsics[1] * camera_point.y / camera_point.z + intrinsics[3],
637                    ),
638                }
639            })
640            .collect();
641        let result = triangulate_tracks(
642            &[ImageFeatureTrack {
643                id: 7,
644                observations,
645            }],
646            &[1, 2, 3],
647            &poses,
648            intrinsics,
649            TriangulationConfig::default(),
650        )
651        .unwrap();
652        assert_eq!(result.landmarks[0].id, 0);
653        assert!((result.landmarks[0].position - point).norm() < 1.0e-10);
654        assert_eq!(result.observations.len(), 3);
655    }
656}