1use nalgebra::{Matrix2, Matrix4, Vector3};
4use rust_robotics_core::{se3_inverse, se3_log, Matrix6};
5use rust_robotics_optimization::{LinearSolver, OptimizationError, OptimizationResult};
6
7use crate::bundle_adjustment::{
8 bundle_adjust, BundleAdjustmentConfig, BundleAdjustmentProblem, CameraIntrinsics,
9 ReprojectionObservation,
10};
11use crate::dataset::{EurocDataset, EurocFeatureTracks, ImuSample};
12use crate::imu_preintegration::{
13 optimize_imu_trajectory, ImuBias, ImuExtrinsics, ImuMeasurement, ImuNoise, ImuTrajectoryConfig,
14 Matrix9, NavState, PreintegratedImuMeasurement,
15};
16use crate::pose_graph_optimization::PoseGraphConfig;
17use crate::pose_graph_optimization_3d::{
18 optimize_pose_graph_3d, Edge3D, Pose3DNode, PoseGraphResult3D,
19};
20
21#[derive(Debug, Clone)]
23pub struct VioPipelineInput {
24 pub keyframe_timestamps_ns: Vec<i64>,
25 pub imu_samples: Vec<ImuSample>,
26 pub initial_state: NavState,
27 pub bias: ImuBias,
28 pub gravity: Vector3<f64>,
29 pub body_from_imu: Matrix4<f64>,
31 pub body_from_camera: Matrix4<f64>,
33 pub landmarks: Vec<Vector3<f64>>,
34 pub observations: Vec<ReprojectionObservation>,
35 pub intrinsics: CameraIntrinsics,
36}
37
38#[derive(Debug, Clone)]
40pub struct VioPipelineConfig {
41 pub imu_noise: ImuNoise,
42 pub imu_trajectory: ImuTrajectoryConfig,
43 pub bundle_adjustment: BundleAdjustmentConfig,
44 pub pose_graph: PoseGraphConfig,
45 pub imu_information: Matrix6,
46 pub visual_information: Matrix6,
47}
48
49impl Default for VioPipelineConfig {
50 fn default() -> Self {
51 let mut nav_measurement_information = Matrix9::zeros();
52 nav_measurement_information
53 .fixed_view_mut::<6, 6>(0, 0)
54 .copy_from(&(nalgebra::SMatrix::<f64, 6, 6>::identity() * 50.0));
55 Self {
56 imu_noise: ImuNoise::default(),
57 imu_trajectory: ImuTrajectoryConfig {
58 nav_measurement_information: Some(nav_measurement_information),
59 ..ImuTrajectoryConfig::default()
60 },
61 bundle_adjustment: BundleAdjustmentConfig::default(),
62 pose_graph: PoseGraphConfig {
63 linear_solver: LinearSolver::BlockSparsePcg {
64 max_iterations: 1_000,
65 tolerance: 1.0e-10,
66 },
67 ..PoseGraphConfig::default()
68 },
69 imu_information: Matrix6::identity() * 100.0,
70 visual_information: Matrix6::identity() * 50.0,
71 }
72 }
73}
74
75#[derive(Debug, Clone)]
77pub struct VioPipelineResult {
78 pub imu_states: Vec<NavState>,
79 pub imu_biases: Vec<ImuBias>,
80 pub bundle_adjusted_cameras: Vec<Matrix4<f64>>,
81 pub landmarks: Vec<Vector3<f64>>,
82 pub trajectory: Vec<Matrix4<f64>>,
83 pub pose_graph_iterations: usize,
84 pub pose_graph_converged: bool,
85 pub bundle_adjustment_iterations: usize,
86 pub imu_optimization_iterations: usize,
87}
88
89pub fn euroc_vio_input(
93 dataset: &EurocDataset,
94 tracks: &EurocFeatureTracks,
95 gravity: Vector3<f64>,
96) -> OptimizationResult<VioPipelineInput> {
97 let (initial_state, bias) = euroc_initial_conditions(dataset)?;
98 let observations = tracks
99 .observations
100 .iter()
101 .map(|observation| {
102 let camera = dataset
103 .camera_frames
104 .binary_search_by_key(&observation.timestamp_ns, |frame| frame.timestamp_ns)
105 .map_err(|_| {
106 OptimizationError::InvalidParameter(
107 "feature timestamp has no matching camera frame".into(),
108 )
109 })?;
110 Ok(ReprojectionObservation {
111 camera,
112 point: observation.landmark_id,
113 pixel: observation.pixel,
114 information: Matrix2::identity(),
115 })
116 })
117 .collect::<OptimizationResult<Vec<_>>>()?;
118 Ok(VioPipelineInput {
119 keyframe_timestamps_ns: dataset
120 .camera_frames
121 .iter()
122 .map(|frame| frame.timestamp_ns)
123 .collect(),
124 imu_samples: dataset.imu.clone(),
125 initial_state,
126 bias,
127 gravity,
128 body_from_imu: dataset.imu_body_from_sensor,
129 body_from_camera: dataset.camera.body_from_sensor,
130 landmarks: tracks
131 .landmarks
132 .iter()
133 .map(|landmark| landmark.position)
134 .collect(),
135 observations,
136 intrinsics: CameraIntrinsics {
137 fx: dataset.camera.intrinsics[0],
138 fy: dataset.camera.intrinsics[1],
139 cx: dataset.camera.intrinsics[2],
140 cy: dataset.camera.intrinsics[3],
141 },
142 })
143}
144
145pub fn euroc_imu_camera_trajectory(
150 dataset: &EurocDataset,
151 gravity: Vector3<f64>,
152 noise: ImuNoise,
153) -> OptimizationResult<Vec<Matrix4<f64>>> {
154 let (initial_state, bias) = euroc_initial_conditions(dataset)?;
155 let timestamps = dataset
156 .camera_frames
157 .iter()
158 .map(|frame| frame.timestamp_ns)
159 .collect::<Vec<_>>();
160 let (states, _) = initialize_imu_sequence(
161 ×tamps,
162 &dataset.imu,
163 initial_state,
164 bias,
165 gravity,
166 &dataset.imu_body_from_sensor,
167 noise,
168 )?;
169 Ok(states
170 .iter()
171 .map(|state| nav_state_pose(state) * dataset.camera.body_from_sensor)
172 .collect())
173}
174
175pub fn run_vio_pipeline(
177 input: &VioPipelineInput,
178 config: &VioPipelineConfig,
179) -> OptimizationResult<VioPipelineResult> {
180 validate_input(input)?;
181 let (predicted_imu_states, preintegrated) = initialize_from_imu(input, config)?;
182 let imu_cameras = predicted_imu_states
183 .iter()
184 .map(|state| nav_state_pose(state) * input.body_from_camera)
185 .collect::<Vec<_>>();
186 let bundle_adjustment = bundle_adjust(
187 &BundleAdjustmentProblem {
188 cameras: imu_cameras,
189 points: input.landmarks.clone(),
190 observations: input.observations.clone(),
191 intrinsics: input.intrinsics,
192 },
193 &config.bundle_adjustment,
194 )?;
195 let camera_from_body = se3_inverse(&input.body_from_camera);
196 let visual_nav_states = bundle_adjustment
197 .cameras
198 .iter()
199 .zip(&predicted_imu_states)
200 .map(|(world_from_camera, predicted)| {
201 let world_from_body = world_from_camera * camera_from_body;
202 NavState {
203 rotation: world_from_body.fixed_view::<3, 3>(0, 0).into_owned(),
204 position: world_from_body.fixed_view::<3, 1>(0, 3).into_owned(),
205 velocity: predicted.velocity,
206 }
207 })
208 .collect::<Vec<_>>();
209 let inertial = optimize_imu_trajectory(
210 &visual_nav_states,
211 input.bias,
212 &preintegrated,
213 input.gravity,
214 &config.imu_trajectory,
215 )?;
216 let imu_states = inertial.states;
217
218 let graph = fuse_pose_graph(
219 &bundle_adjustment.cameras,
220 &imu_states,
221 &preintegrated,
222 &input.body_from_camera,
223 config,
224 );
225 Ok(VioPipelineResult {
226 imu_states,
227 imu_biases: inertial.biases,
228 bundle_adjusted_cameras: bundle_adjustment.cameras,
229 landmarks: bundle_adjustment.points,
230 trajectory: graph.poses.iter().map(|pose| pose.transform).collect(),
231 pose_graph_iterations: graph.iterations,
232 pose_graph_converged: graph.converged,
233 bundle_adjustment_iterations: bundle_adjustment.summary.iterations,
234 imu_optimization_iterations: inertial.summary.iterations,
235 })
236}
237
238fn validate_input(input: &VioPipelineInput) -> OptimizationResult<()> {
239 if input.keyframe_timestamps_ns.len() < 2 {
240 return Err(OptimizationError::InvalidParameter(
241 "VIO requires at least two keyframes".into(),
242 ));
243 }
244 if input
245 .keyframe_timestamps_ns
246 .windows(2)
247 .any(|window| window[1] <= window[0])
248 {
249 return Err(OptimizationError::InvalidParameter(
250 "VIO keyframe timestamps must be strictly increasing".into(),
251 ));
252 }
253 if input
254 .imu_samples
255 .windows(2)
256 .any(|window| window[1].timestamp_ns <= window[0].timestamp_ns)
257 {
258 return Err(OptimizationError::InvalidParameter(
259 "VIO IMU timestamps must be strictly increasing".into(),
260 ));
261 }
262 if input.imu_samples.is_empty() || input.landmarks.is_empty() {
263 return Err(OptimizationError::InvalidParameter(
264 "VIO requires IMU samples and landmarks".into(),
265 ));
266 }
267 if input.observations.iter().any(|observation| {
268 observation.camera >= input.keyframe_timestamps_ns.len()
269 || observation.point >= input.landmarks.len()
270 }) {
271 return Err(OptimizationError::InvalidParameter(
272 "VIO observation index is out of range".into(),
273 ));
274 }
275 Ok(())
276}
277
278fn initialize_from_imu(
279 input: &VioPipelineInput,
280 config: &VioPipelineConfig,
281) -> OptimizationResult<(Vec<NavState>, Vec<PreintegratedImuMeasurement>)> {
282 initialize_imu_sequence(
283 &input.keyframe_timestamps_ns,
284 &input.imu_samples,
285 input.initial_state.clone(),
286 input.bias,
287 input.gravity,
288 &input.body_from_imu,
289 config.imu_noise,
290 )
291}
292
293fn initialize_imu_sequence(
294 timestamps_ns: &[i64],
295 samples: &[ImuSample],
296 initial_state: NavState,
297 bias: ImuBias,
298 gravity: Vector3<f64>,
299 body_from_imu: &Matrix4<f64>,
300 noise: ImuNoise,
301) -> OptimizationResult<(Vec<NavState>, Vec<PreintegratedImuMeasurement>)> {
302 if timestamps_ns.len() < 2 {
303 return Err(OptimizationError::InvalidParameter(
304 "IMU initialization requires at least two keyframes".into(),
305 ));
306 }
307 let mut states = vec![initial_state];
308 let mut measurements = Vec::with_capacity(timestamps_ns.len() - 1);
309 let extrinsics = ImuExtrinsics::from_homogeneous(body_from_imu)?;
310 for interval in timestamps_ns.windows(2) {
311 let measurement =
312 preintegrate_interval(samples, interval[0], interval[1], bias, noise, &extrinsics)?;
313 let next =
314 measurement.predict(states.last().expect("initial state exists"), &bias, gravity);
315 states.push(next);
316 measurements.push(measurement);
317 }
318 Ok((states, measurements))
319}
320
321fn euroc_initial_conditions(dataset: &EurocDataset) -> OptimizationResult<(NavState, ImuBias)> {
322 let first_truth = dataset.ground_truth.first().ok_or_else(|| {
323 OptimizationError::InvalidParameter("EuRoC VIO initialization needs ground truth".into())
324 })?;
325 Ok((
326 NavState {
327 rotation: first_truth
328 .world_from_body
329 .fixed_view::<3, 3>(0, 0)
330 .into_owned(),
331 position: first_truth
332 .world_from_body
333 .fixed_view::<3, 1>(0, 3)
334 .into_owned(),
335 velocity: first_truth.velocity,
336 },
337 ImuBias {
338 accelerometer: first_truth.accelerometer_bias,
339 gyroscope: first_truth.gyroscope_bias,
340 },
341 ))
342}
343
344fn preintegrate_interval(
345 samples: &[ImuSample],
346 start_ns: i64,
347 end_ns: i64,
348 bias: ImuBias,
349 noise: ImuNoise,
350 extrinsics: &ImuExtrinsics,
351) -> OptimizationResult<PreintegratedImuMeasurement> {
352 let mut measurement = PreintegratedImuMeasurement::new(bias, noise);
353 let mut previous_ns = start_ns;
354 let mut current = samples
355 .iter()
356 .rev()
357 .find(|sample| sample.timestamp_ns <= start_ns)
358 .or_else(|| samples.iter().find(|sample| sample.timestamp_ns < end_ns))
359 .ok_or_else(|| {
360 OptimizationError::InvalidParameter("no IMU sample covers keyframe interval".into())
361 })?;
362 for sample in samples
363 .iter()
364 .filter(|sample| sample.timestamp_ns > start_ns && sample.timestamp_ns < end_ns)
365 {
366 let dt = (sample.timestamp_ns - previous_ns) as f64 * 1.0e-9;
367 let angular_acceleration = (sample.angular_velocity - current.angular_velocity) / dt;
368 measurement.integrate_sensor_measurement(
369 ImuMeasurement {
370 acceleration: current.acceleration,
371 angular_velocity: current.angular_velocity,
372 },
373 angular_acceleration,
374 extrinsics,
375 dt,
376 )?;
377 previous_ns = sample.timestamp_ns;
378 current = sample;
379 }
380 let final_dt = (end_ns - previous_ns) as f64 * 1.0e-9;
381 let angular_acceleration = samples
382 .iter()
383 .find(|sample| sample.timestamp_ns >= end_ns && sample.timestamp_ns > current.timestamp_ns)
384 .map(|next| {
385 (next.angular_velocity - current.angular_velocity)
386 / ((next.timestamp_ns - current.timestamp_ns) as f64 * 1.0e-9)
387 })
388 .unwrap_or_else(Vector3::zeros);
389 measurement.integrate_sensor_measurement(
390 ImuMeasurement {
391 acceleration: current.acceleration,
392 angular_velocity: current.angular_velocity,
393 },
394 angular_acceleration,
395 extrinsics,
396 final_dt,
397 )?;
398 Ok(measurement)
399}
400
401fn nav_state_pose(state: &NavState) -> Matrix4<f64> {
402 let mut pose = Matrix4::identity();
403 pose.fixed_view_mut::<3, 3>(0, 0).copy_from(&state.rotation);
404 pose.fixed_view_mut::<3, 1>(0, 3).copy_from(&state.position);
405 pose
406}
407
408fn fuse_pose_graph(
409 cameras: &[Matrix4<f64>],
410 imu_states: &[NavState],
411 preintegrated: &[PreintegratedImuMeasurement],
412 body_from_camera: &Matrix4<f64>,
413 config: &VioPipelineConfig,
414) -> PoseGraphResult3D {
415 let initial = cameras
416 .iter()
417 .map(|transform| Pose3DNode {
418 transform: *transform,
419 })
420 .collect::<Vec<_>>();
421 let mut edges = preintegrated
422 .iter()
423 .enumerate()
424 .map(|(index, _)| {
425 let from = nav_state_pose(&imu_states[index]) * body_from_camera;
426 let to = nav_state_pose(&imu_states[index + 1]) * body_from_camera;
427 Edge3D {
428 from: index,
429 to: index + 1,
430 measurement: Pose3DNode {
431 transform: se3_inverse(&from) * to,
432 },
433 information: config.imu_information,
434 }
435 })
436 .collect::<Vec<_>>();
437 let last = cameras.len() - 1;
438 edges.push(Edge3D {
439 from: 0,
440 to: last,
441 measurement: Pose3DNode {
442 transform: se3_inverse(&cameras[0]) * cameras[last],
443 },
444 information: config.visual_information,
445 });
446 optimize_pose_graph_3d(&initial, &edges, &config.pose_graph)
447}
448
449pub fn pose_error(actual: &Matrix4<f64>, expected: &Matrix4<f64>) -> f64 {
451 se3_log(&(se3_inverse(expected) * actual)).norm()
452}
453
454#[cfg(test)]
455mod tests {
456 use nalgebra::Matrix2;
457
458 use super::*;
459 use crate::bundle_adjustment::reproject_world_point;
460
461 #[test]
462 fn connected_vio_pipeline_recovers_synthetic_sequence() {
463 let timestamps = (0..5).map(|index| index * 200_000_000).collect::<Vec<_>>();
464 let imu_samples = (0..=80)
465 .map(|index| ImuSample {
466 timestamp_ns: index * 10_000_000,
467 angular_velocity: Vector3::zeros(),
468 acceleration: Vector3::new(0.0, 0.0, 9.81),
469 })
470 .collect::<Vec<_>>();
471 let intrinsics = CameraIntrinsics {
472 fx: 320.0,
473 fy: 320.0,
474 cx: 320.0,
475 cy: 240.0,
476 };
477 let truth_cameras = (0..5)
478 .map(|index| {
479 let mut pose = Matrix4::identity();
480 pose[(0, 3)] = index as f64 * 0.2;
481 pose
482 })
483 .collect::<Vec<_>>();
484 let truth_points = vec![
485 Vector3::new(-0.4, -0.2, 4.0),
486 Vector3::new(0.2, 0.3, 4.5),
487 Vector3::new(0.8, -0.1, 5.0),
488 Vector3::new(-0.6, 0.4, 5.5),
489 ];
490 let observations = truth_cameras
491 .iter()
492 .enumerate()
493 .flat_map(|(camera, pose)| {
494 truth_points
495 .iter()
496 .enumerate()
497 .map(move |(point, landmark)| ReprojectionObservation {
498 camera,
499 point,
500 pixel: reproject_world_point(pose, landmark, &intrinsics).unwrap(),
501 information: Matrix2::identity(),
502 })
503 })
504 .collect();
505 let input = VioPipelineInput {
506 keyframe_timestamps_ns: timestamps,
507 imu_samples,
508 initial_state: NavState {
509 velocity: Vector3::x(),
510 ..NavState::identity()
511 },
512 bias: ImuBias::zero(),
513 gravity: Vector3::new(0.0, 0.0, -9.81),
514 body_from_imu: Matrix4::identity(),
515 body_from_camera: Matrix4::identity(),
516 landmarks: truth_points
517 .iter()
518 .map(|point| point + Vector3::new(0.08, -0.05, 0.12))
519 .collect(),
520 observations,
521 intrinsics,
522 };
523 let result = run_vio_pipeline(&input, &VioPipelineConfig::default()).unwrap();
524 assert!(result.pose_graph_converged);
525 assert_eq!(result.trajectory.len(), truth_cameras.len());
526 assert_eq!(result.imu_biases.len(), truth_cameras.len());
527 let terminal_error = pose_error(&result.trajectory[4], &truth_cameras[4]);
528 assert!(terminal_error < 2.0e-2, "terminal error: {terminal_error}");
529 let landmark_error = result
530 .landmarks
531 .iter()
532 .zip(&truth_points)
533 .map(|(actual, expected)| (actual - expected).norm())
534 .sum::<f64>()
535 / truth_points.len() as f64;
536 assert!(landmark_error < 0.2, "landmark error: {landmark_error}");
537 }
538}