slamkit_rs/odometry/
trajectory.rs

1use nalgebra as na;
2use serde::{Deserialize, Serialize};
3
4/// Single trajectory point with metadata
5#[derive(Debug, Clone, Serialize, Deserialize)]
6pub struct TrajectoryPoint {
7    pub frame: usize,
8    pub position: [f64; 3],
9    pub timestamp: f64,
10}
11
12/// Trajectory tracker for visual odometry
13pub struct Trajectory {
14    points: Vec<TrajectoryPoint>,
15    global_pose: na::Matrix4<f64>,
16}
17
18impl Trajectory {
19    /// Create a new trajectory starting at origin
20    pub fn new() -> Self {
21        Self {
22            points: vec![TrajectoryPoint {
23                frame: 0,
24                position: [0.0, 0.0, 0.0],
25                timestamp: 0.0,
26            }],
27            global_pose: na::Matrix4::identity(),
28        }
29    }
30
31    /// Update pose with relative rotation and translation
32    pub fn update(
33        &mut self,
34        rotation: &na::Matrix3<f64>,
35        translation: &na::Vector3<f64>,
36        frame: usize,
37        timestamp: f64,
38    ) {
39        // Build relative transformation matrix
40        let mut relative_transform = na::Matrix4::identity();
41
42        // rotation
43        for i in 0..3 {
44            for j in 0..3 {
45                relative_transform[(i, j)] = rotation[(i, j)];
46            }
47        }
48
49        // translation
50        relative_transform[(0, 3)] = translation[0];
51        relative_transform[(1, 3)] = translation[1];
52        relative_transform[(2, 3)] = translation[2];
53
54        // Compose: T_global = T_global * T_relative
55        self.global_pose = self.global_pose * relative_transform;
56
57        // Extract current position
58        let position = [
59            self.global_pose[(0, 3)],
60            self.global_pose[(1, 3)],
61            self.global_pose[(2, 3)],
62        ];
63
64        self.points.push(TrajectoryPoint {
65            frame,
66            position,
67            timestamp,
68        });
69    }
70
71    /// Get current global pose
72    pub fn current_pose(&self) -> &na::Matrix4<f64> {
73        &self.global_pose
74    }
75
76    /// Get current rotation and translation as a tuple
77    pub fn current_pose_rt(&self) -> (na::Matrix3<f64>, na::Vector3<f64>) {
78        let mut rotation = na::Matrix3::zeros();
79        for i in 0..3 {
80            for j in 0..3 {
81                rotation[(i, j)] = self.global_pose[(i, j)];
82            }
83        }
84
85        let translation = na::Vector3::new(
86            self.global_pose[(0, 3)],
87            self.global_pose[(1, 3)],
88            self.global_pose[(2, 3)],
89        );
90
91        (rotation, translation)
92    }
93
94    /// Get all trajectory points
95    pub fn points(&self) -> &[TrajectoryPoint] {
96        &self.points
97    }
98
99    /// Calculate total distance traveled
100    pub fn total_distance(&self) -> f64 {
101        let mut distance = 0.0;
102        for i in 1..self.points.len() {
103            let p1 = &self.points[i - 1].position;
104            let p2 = &self.points[i].position;
105
106            let dx = p2[0] - p1[0];
107            let dy = p2[1] - p1[1];
108            let dz = p2[2] - p1[2];
109
110            distance += (dx * dx + dy * dy + dz * dz).sqrt();
111        }
112        distance
113    }
114
115    /// Get number of trajectory points
116    pub fn len(&self) -> usize {
117        self.points.len()
118    }
119
120    /// Check if trajectory is empty
121    pub fn is_empty(&self) -> bool {
122        self.points.is_empty()
123    }
124
125    /// Export trajectory to JSON
126    pub fn to_json(&self) -> Result<String, Box<dyn std::error::Error>> {
127        let json = serde_json::to_string_pretty(&self.points)?;
128        Ok(json)
129    }
130
131    /// Save trajectory to file
132    pub fn save_to_file(&self, path: &str) -> Result<(), Box<dyn std::error::Error>> {
133        let json = self.to_json()?;
134        std::fs::write(path, json)?;
135        Ok(())
136    }
137}
138
139#[cfg(test)]
140mod tests {
141    use super::*;
142
143    #[test]
144    fn test_trajectory_creation() {
145        let traj = Trajectory::new();
146        assert_eq!(traj.len(), 1);
147        assert_eq!(traj.points()[0].frame, 0);
148    }
149
150    #[test]
151    fn test_trajectory_update() {
152        let mut traj = Trajectory::new();
153        let r = na::Matrix3::identity();
154        let t = na::Vector3::new(1.0, 0.0, 0.0);
155
156        traj.update(&r, &t, 1, 0.1);
157
158        assert_eq!(traj.len(), 2);
159        assert_eq!(traj.points()[1].frame, 1);
160        assert!((traj.points()[1].position[0] - 1.0).abs() < 1e-6);
161    }
162
163    #[test]
164    fn test_total_distance() {
165        let mut traj = Trajectory::new();
166        let r = na::Matrix3::identity();
167
168        // Move 3 units in x direction
169        let t1 = na::Vector3::new(3.0, 0.0, 0.0);
170        traj.update(&r, &t1, 1, 0.1);
171
172        // Move 4 units in y direction
173        let t2 = na::Vector3::new(0.0, 4.0, 0.0);
174        traj.update(&r, &t2, 2, 0.2);
175
176        let distance = traj.total_distance();
177        assert!((distance - 7.0).abs() < 1e-6);
178    }
179
180    #[test]
181    fn test_json_export() {
182        let mut traj = Trajectory::new();
183        let r = na::Matrix3::identity();
184        let t = na::Vector3::new(1.0, 2.0, 3.0);
185        traj.update(&r, &t, 1, 0.1);
186
187        let json = traj.to_json();
188        assert!(json.is_ok());
189        assert!(json.unwrap().contains("position"));
190    }
191}
slamkit_rs/odometry/trajectory.rs

slamkit_rs/odometry/
trajectory.rs
