Skip to main content

rust_robotics_slam/
vio_pipeline.rs

1//! Offline visual-inertial pipeline built from the shared estimation modules.
2
3use 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/// Inputs for an offline visual-inertial solve.
22#[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    /// Body-from-IMU extrinsic transform.
30    pub body_from_imu: Matrix4<f64>,
31    /// Body-from-camera extrinsic transform.
32    pub body_from_camera: Matrix4<f64>,
33    pub landmarks: Vec<Vector3<f64>>,
34    pub observations: Vec<ReprojectionObservation>,
35    pub intrinsics: CameraIntrinsics,
36}
37
38/// Configuration for the visual-inertial stages.
39#[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/// Output and stage diagnostics from [`run_vio_pipeline`].
76#[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
89/// Converts a loaded EuRoC sequence and pre-extracted track sidecar into VIO
90/// inputs. Ground truth is used only for the initial state and acceptance
91/// evaluation; later states are not passed to the optimizer.
92pub 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
145/// Produces the metric camera trajectory used to initialize visual tracks.
146///
147/// Only the first EuRoC ground-truth state supplies pose, velocity and biases;
148/// every later camera pose is predicted from the IMU.
149pub 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        &timestamps,
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
175/// Runs IMU initialization, joint camera/landmark BA, then SE(3) graph fusion.
176pub 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
449/// Translation plus rotation-vector error between two poses.
450pub 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}