slamkit_rs/mapping/
keyframe.rs

1use nalgebra as na;
2
3/// Keyframe selection criteria
4#[derive(Debug, Clone)]
5pub struct KeyframeConfig {
6    /// Minimum translation distance to consider new keyframe (meters)
7    pub min_translation: f64,
8    /// Minimum rotation angle to consider new keyframe (radians)
9    pub min_rotation: f64,
10    /// Minimum ratio of matches compared to last keyframe
11    pub min_match_ratio: f64,
12    /// Maximum number of frames since last keyframe
13    pub max_frames: usize,
14}
15
16impl Default for KeyframeConfig {
17    fn default() -> Self {
18        Self {
19            min_translation: 0.1, // 10cm
20            min_rotation: 0.1,    // ~5.7 degrees
21            min_match_ratio: 0.8, // 80% of previous matches
22            max_frames: 10,       // Force keyframe every 10 frames
23        }
24    }
25}
26
27/// Keyframe selector for visual odometry
28pub struct KeyframeSelector {
29    config: KeyframeConfig,
30    frames_since_last: usize,
31    last_keyframe_matches: usize,
32}
33
34impl KeyframeSelector {
35    /// Create new keyframe selector with default config
36    pub fn new() -> Self {
37        Self::with_config(KeyframeConfig::default())
38    }
39
40    /// Create new keyframe selector with custom config
41    pub fn with_config(config: KeyframeConfig) -> Self {
42        Self {
43            config,
44            frames_since_last: 0,
45            last_keyframe_matches: 0,
46        }
47    }
48
49    /// Check if current frame should be a keyframe
50    pub fn should_be_keyframe(
51        &mut self,
52        rotation: &na::Matrix3<f64>,
53        translation: &na::Vector3<f64>,
54        num_matches: usize,
55    ) -> bool {
56        self.frames_since_last += 1;
57
58        // Force keyframe if too many frames passed
59        if self.frames_since_last >= self.config.max_frames {
60            self.mark_as_keyframe(num_matches);
61            return true;
62        }
63
64        // Check translation
65        let trans_norm = translation.norm();
66        if trans_norm >= self.config.min_translation {
67            self.mark_as_keyframe(num_matches);
68            return true;
69        }
70
71        // Check rotation (convert to angle)
72        let rotation_angle = rotation_matrix_to_angle(rotation);
73        if rotation_angle >= self.config.min_rotation {
74            self.mark_as_keyframe(num_matches);
75            return true;
76        }
77
78        // Check match quality degradation
79        if self.last_keyframe_matches > 0 {
80            let match_ratio = num_matches as f64 / self.last_keyframe_matches as f64;
81            if match_ratio < self.config.min_match_ratio {
82                self.mark_as_keyframe(num_matches);
83                return true;
84            }
85        }
86
87        false
88    }
89
90    /// Reset selector state
91    pub fn reset(&mut self) {
92        self.frames_since_last = 0;
93        self.last_keyframe_matches = 0;
94    }
95
96    /// Mark current frame as keyframe
97    fn mark_as_keyframe(&mut self, num_matches: usize) {
98        self.frames_since_last = 0;
99        self.last_keyframe_matches = num_matches;
100    }
101
102    /// Get frames since last keyframe
103    pub fn frames_since_last(&self) -> usize {
104        self.frames_since_last
105    }
106}
107
108/// Convert rotation matrix to rotation angle (radians)
109fn rotation_matrix_to_angle(rotation: &na::Matrix3<f64>) -> f64 {
110    // trace(R) = 1 + 2*cos(theta)
111    let trace = rotation.trace();
112    let cos_angle = (trace - 1.0) / 2.0;
113    let cos_angle = cos_angle.clamp(-1.0, 1.0); // Numerical stability
114    cos_angle.acos()
115}
116
117#[cfg(test)]
118mod tests {
119    use super::*;
120
121    #[test]
122    fn test_keyframe_selector_creation() {
123        let selector = KeyframeSelector::new();
124        assert_eq!(selector.frames_since_last(), 0);
125    }
126
127    #[test]
128    fn test_force_keyframe_after_max_frames() {
129        let config = KeyframeConfig {
130            max_frames: 5,
131            ..Default::default()
132        };
133        let mut selector = KeyframeSelector::with_config(config);
134
135        let r = na::Matrix3::identity();
136        let t = na::Vector3::zeros();
137
138        for i in 0..4 {
139            assert!(!selector.should_be_keyframe(&r, &t, 100), "Frame {}", i);
140        }
141        assert!(
142            selector.should_be_keyframe(&r, &t, 100),
143            "Frame 5 should be keyframe"
144        );
145    }
146
147    #[test]
148    fn test_keyframe_on_large_translation() {
149        let mut selector = KeyframeSelector::new();
150        let r = na::Matrix3::identity();
151        let t = na::Vector3::new(0.2, 0.0, 0.0); // 20cm movement
152
153        assert!(selector.should_be_keyframe(&r, &t, 100));
154    }
155
156    #[test]
157    fn test_keyframe_on_large_rotation() {
158        let mut selector = KeyframeSelector::new();
159        let angle: f64 = 0.15; // > 0.1 rad threshold
160        let r = na::Matrix3::new(
161            angle.cos(),
162            -angle.sin(),
163            0.0,
164            angle.sin(),
165            angle.cos(),
166            0.0,
167            0.0,
168            0.0,
169            1.0,
170        );
171        let t = na::Vector3::zeros();
172
173        assert!(selector.should_be_keyframe(&r, &t, 100));
174    }
175
176    #[test]
177    fn test_no_keyframe_small_motion() {
178        let mut selector = KeyframeSelector::new();
179        selector.mark_as_keyframe(100);
180
181        let r = na::Matrix3::identity();
182        let t = na::Vector3::new(0.01, 0.0, 0.0); // 1cm, below threshold
183
184        assert!(!selector.should_be_keyframe(&r, &t, 95));
185    }
186}
slamkit_rs/mapping/keyframe.rs

slamkit_rs/mapping/
keyframe.rs
