align3d/
trajectory.rs

1use std::ops::Index;
2
3use crate::transform::Transform;
4
5/// Trajectory of camera poses. Use it to store or create trajectories while aligning scans.
6#[derive(Clone, Debug)]
7pub struct Trajectory {
8    /// Camera poses, transforms points from camera to world.
9    pub camera_to_world: Vec<Transform>,
10    /// Timestamps of each pose.
11    pub times: Vec<f32>,
12}
13
14impl Default for Trajectory {
15    /// Empty trajectory.
16    fn default() -> Self {
17        Self {
18            camera_to_world: Vec::new(),
19            times: Vec::new(),
20        }
21    }
22}
23
24impl Trajectory {
25    /// Adds a new pose to the trajectory.
26    ///
27    /// # Arguments
28    ///
29    /// * `camera_to_world` - Transform from camera to world.
30    /// * `time` - Timestamp of the pose.
31    pub fn push(&mut self, camera_to_world: Transform, time: f32) {
32        self.camera_to_world.push(camera_to_world);
33        self.times.push(time);
34    }
35
36    /// Returns the number of poses in the trajectory.
37    pub fn len(&self) -> usize {
38        self.camera_to_world.len()
39    }
40
41    /// Returns true if the trajectory is empty.
42    pub fn is_empty(&self) -> bool {
43        self.camera_to_world.is_empty()
44    }
45
46    /// Returns the relative transform between two poses.
47    pub fn get_relative_transform(
48        &self,
49        from_index: usize,
50        dest_index: usize,
51    ) -> Option<Transform> {
52        Some(&self.camera_to_world[dest_index].inverse() * &self.camera_to_world[from_index])
53    }
54
55    /// Returns the iterator over poses and timestamps.
56    pub fn iter(&self) -> impl Iterator<Item = (Transform, f32)> + '_ {
57        self.camera_to_world
58            .iter()
59            .zip(self.times.iter())
60            .map(|(camera_to_world, time)| (camera_to_world.clone(), *time))
61    }
62
63    /// Creates a new trajectory with the poses transformed in such a way that the first pose is at origin.
64    pub fn first_frame_at_origin(&self) -> Self {
65        if self.camera_to_world.is_empty() {
66            return self.clone();
67        }
68
69        let first_inv = self.camera_to_world[0].inverse();
70        Self {
71            camera_to_world: self
72                .camera_to_world
73                .iter()
74                .map(|transform| &first_inv * transform)
75                .collect::<Vec<Transform>>(),
76            times: self.times.clone(),
77        }
78    }
79
80    /// Creates a new trajectory with the given range.
81    ///
82    /// # Arguments
83    ///
84    /// * `start` - Inclusive start index of the range.
85    /// * `end` - Exclusive end index of the range.
86    ///
87    /// # Returns
88    ///
89    /// New trajectory with the poses in the given range.
90    pub fn slice(&self, start: usize, end: usize) -> Self {
91        Self {
92            camera_to_world: self.camera_to_world[start..end].to_vec(),
93            times: self.times[start..end].to_vec(),
94        }
95    }
96
97    /// Gets the last pose and timestamp.
98    /// If the trajectory is empty, it returns `None`.
99    pub fn last(&self) -> Option<(Transform, f32)> {
100        if self.is_empty() {
101            None
102        } else {
103            Some((
104                self.camera_to_world[self.len() - 1].clone(),
105                self.times[self.len() - 1],
106            ))
107        }
108    }
109}
110
111impl FromIterator<(Transform, f32)> for Trajectory {
112    /// Creates a new trajectory from the `(Transform, f32)` iterator.
113    /// Use with the `collect::<Trajectory>` method.
114    fn from_iter<T: IntoIterator<Item = (Transform, f32)>>(iter: T) -> Self {
115        let mut trajectory = Trajectory::default();
116        for (transform, time) in iter {
117            trajectory.push(transform, time);
118        }
119        trajectory
120    }
121}
122
123impl Index<usize> for Trajectory {
124    type Output = Transform;
125    /// Returns the pose at the given index.
126    fn index(&self, index: usize) -> &Self::Output {
127        &self.camera_to_world[index]
128    }
129}
130
131/// Accumulates transforms and builds a trajectory.
132#[derive(Clone, Debug)]
133pub struct TrajectoryBuilder {
134    trajectory: Trajectory,
135    last: Transform,
136    last_time: f32,
137}
138
139impl Default for TrajectoryBuilder {
140    /// Creates a new `TrajectoryBuilder`.
141    /// It'll contain a single pose at the origin.
142    fn default() -> Self {
143        Self {
144            trajectory: Trajectory::default(),
145            last: Transform::eye(),
146            last_time: 0.0,
147        }
148    }
149}
150
151impl TrajectoryBuilder {
152    pub fn with_start(start_transform: Transform, start_time: f32) -> Self {
153        let mut trajectory = Trajectory::default();
154        trajectory.push(start_transform.clone(), start_time);
155        Self {
156            trajectory,
157            last: start_transform,
158            last_time: start_time,
159        }
160    }
161
162    /// Accumulates the given transform and timestamp into the previous ones and adds
163    /// it to the trajectory being build.
164    pub fn accumulate(&mut self, now_to_previous: &Transform, timestamp: Option<f32>) {
165        self.last = now_to_previous * &self.last;
166        self.last_time = timestamp.unwrap_or(self.last_time + 1.0);
167        self.trajectory.push(self.last.clone(), self.last_time);
168    }
169
170    /// Creates the trajectory at its current state.
171    pub fn build(self) -> Trajectory {
172        self.trajectory
173    }
174
175    /// Returns the current camera pose in the world frame.
176    /// If the trajectory is empty, it returns `None`.
177    pub fn current_camera_to_world(&self) -> Option<Transform> {
178        if self.trajectory.is_empty() {
179            None
180        } else {
181            Some(self.trajectory.last().unwrap().0.clone())
182        }
183    }
184}
align3d/trajectory.rs

align3d/
trajectory.rs
