1use nalgebra::{Matrix3, Matrix4, Vector2, Vector3};
8use rust_robotics_optimization::{OptimizationError, OptimizationResult};
9
10use crate::dataset::{EurocFeatureTracks, FeatureTrackObservation, TrackedLandmark};
11
12#[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#[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#[derive(Debug, Clone, Copy, PartialEq)]
123pub struct ImageFeatureObservation {
124 pub frame: usize,
125 pub pixel: Vector2<f64>,
126}
127
128#[derive(Debug, Clone, PartialEq)]
130pub struct ImageFeatureTrack {
131 pub id: usize,
132 pub observations: Vec<ImageFeatureObservation>,
133}
134
135#[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 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#[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
259pub 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}