Skip to main content

rust_robotics_slam/
bundle_adjustment.rs

1//! Pinhole reprojection factors and dense bundle adjustment.
2
3use nalgebra::{DMatrix, DVector, Matrix2, Matrix4, SMatrix, Vector2, Vector3};
4use rust_robotics_core::{se3_exp, se3_inverse, se3_log, skew, Vector6};
5use rust_robotics_optimization::{
6    solve, Factor, FactorEvaluation, LinearSolver, OptimizationError, OptimizationResult, Problem,
7    RobustKernel, SolverConfig, SolverSummary, Variable, VariableId,
8};
9
10/// Pinhole camera calibration without distortion.
11#[derive(Debug, Clone, Copy, PartialEq)]
12pub struct CameraIntrinsics {
13    pub fx: f64,
14    pub fy: f64,
15    pub cx: f64,
16    pub cy: f64,
17}
18
19impl CameraIntrinsics {
20    /// Projects a camera-frame point onto the image plane.
21    pub fn project(&self, point: &Vector3<f64>) -> OptimizationResult<Vector2<f64>> {
22        if point.z <= 1.0e-9 || !point.iter().all(|value| value.is_finite()) {
23            return Err(OptimizationError::InvalidFactor(
24                "reprojected point must be finite and in front of the camera".into(),
25            ));
26        }
27        Ok(Vector2::new(
28            self.fx * point.x / point.z + self.cx,
29            self.fy * point.y / point.z + self.cy,
30        ))
31    }
32
33    fn projection_jacobian(&self, point: &Vector3<f64>) -> SMatrix<f64, 2, 3> {
34        let inverse_z = 1.0 / point.z;
35        let inverse_z_squared = inverse_z * inverse_z;
36        SMatrix::<f64, 2, 3>::new(
37            self.fx * inverse_z,
38            0.0,
39            -self.fx * point.x * inverse_z_squared,
40            0.0,
41            self.fy * inverse_z,
42            -self.fy * point.y * inverse_z_squared,
43        )
44    }
45}
46
47/// One observed landmark pixel.
48#[derive(Debug, Clone, Copy, PartialEq)]
49pub struct ReprojectionObservation {
50    pub camera: usize,
51    pub point: usize,
52    pub pixel: Vector2<f64>,
53    pub information: Matrix2<f64>,
54}
55
56/// Inputs to a bundle-adjustment solve. Camera poses are world-from-camera.
57#[derive(Debug, Clone)]
58pub struct BundleAdjustmentProblem {
59    pub cameras: Vec<Matrix4<f64>>,
60    pub points: Vec<Vector3<f64>>,
61    pub observations: Vec<ReprojectionObservation>,
62    pub intrinsics: CameraIntrinsics,
63}
64
65/// Bundle-adjustment configuration.
66#[derive(Debug, Clone)]
67pub struct BundleAdjustmentConfig {
68    pub solver: SolverConfig,
69    /// Number of leading camera poses kept constant to remove gauge freedom.
70    pub fixed_cameras: usize,
71    /// Eliminate landmark blocks before solving the reduced camera system.
72    pub use_schur_complement: bool,
73    pub robust_kernel: RobustKernel,
74}
75
76impl Default for BundleAdjustmentConfig {
77    fn default() -> Self {
78        Self {
79            solver: SolverConfig::default(),
80            fixed_cameras: 1,
81            use_schur_complement: true,
82            robust_kernel: RobustKernel::Huber { delta: 2.0 },
83        }
84    }
85}
86
87/// Optimized camera poses and landmarks.
88#[derive(Debug, Clone)]
89pub struct BundleAdjustmentResult {
90    pub cameras: Vec<Matrix4<f64>>,
91    pub points: Vec<Vector3<f64>>,
92    pub summary: SolverSummary,
93}
94
95/// Reprojects a world point into a world-from-camera pose.
96pub fn reproject_world_point(
97    camera_world_from_camera: &Matrix4<f64>,
98    point_world: &Vector3<f64>,
99    intrinsics: &CameraIntrinsics,
100) -> OptimizationResult<Vector2<f64>> {
101    let inverse = se3_inverse(camera_world_from_camera);
102    let point_camera =
103        inverse.fixed_view::<3, 3>(0, 0) * point_world + inverse.fixed_view::<3, 1>(0, 3);
104    intrinsics.project(&point_camera)
105}
106
107/// Runs joint camera/landmark bundle adjustment.
108pub fn bundle_adjust(
109    input: &BundleAdjustmentProblem,
110    config: &BundleAdjustmentConfig,
111) -> OptimizationResult<BundleAdjustmentResult> {
112    if input.cameras.is_empty() || input.points.is_empty() {
113        return Err(OptimizationError::InvalidParameter(
114            "bundle adjustment needs cameras and points".into(),
115        ));
116    }
117    let mut problem = Problem::new();
118    let camera_ids = input
119        .cameras
120        .iter()
121        .map(|camera| {
122            problem.add_variable(
123                Variable::with_retraction(
124                    DVector::from_column_slice(se3_log(camera).as_slice()),
125                    6,
126                    retract_camera,
127                )
128                .expect("camera dimensions are valid"),
129            )
130        })
131        .collect::<Vec<_>>();
132    for id in camera_ids.iter().take(config.fixed_cameras) {
133        problem.variable_mut(*id)?.set_fixed(true);
134    }
135    let point_ids = input
136        .points
137        .iter()
138        .map(|point| {
139            problem.add_variable(Variable::euclidean(DVector::from_column_slice(
140                point.as_slice(),
141            )))
142        })
143        .collect::<Vec<_>>();
144
145    for observation in &input.observations {
146        let camera = *camera_ids.get(observation.camera).ok_or_else(|| {
147            OptimizationError::InvalidParameter("observation camera index is out of range".into())
148        })?;
149        let point = *point_ids.get(observation.point).ok_or_else(|| {
150            OptimizationError::InvalidParameter("observation point index is out of range".into())
151        })?;
152        problem.add_factor(ReprojectionFactor {
153            variables: [camera, point],
154            pixel: observation.pixel,
155            information: observation.information,
156            intrinsics: input.intrinsics,
157            kernel: config.robust_kernel,
158        })?;
159    }
160
161    let mut solver = config.solver;
162    if config.use_schur_complement {
163        solver.linear_solver = LinearSolver::SchurComplement {
164            retained_dimension: input.cameras.len().saturating_sub(config.fixed_cameras) * 6,
165        };
166    }
167    let summary = solve(&mut problem, &solver)?;
168    let cameras = camera_ids
169        .iter()
170        .map(|id| {
171            let value = problem.variable(*id).expect("camera exists").value();
172            se3_exp(&Vector6::from_column_slice(value.as_slice()))
173        })
174        .collect();
175    let points = point_ids
176        .iter()
177        .map(|id| {
178            let value = problem.variable(*id).expect("point exists").value();
179            Vector3::new(value[0], value[1], value[2])
180        })
181        .collect();
182    Ok(BundleAdjustmentResult {
183        cameras,
184        points,
185        summary,
186    })
187}
188
189struct ReprojectionFactor {
190    variables: [VariableId; 2],
191    pixel: Vector2<f64>,
192    information: Matrix2<f64>,
193    intrinsics: CameraIntrinsics,
194    kernel: RobustKernel,
195}
196
197impl Factor for ReprojectionFactor {
198    fn variable_ids(&self) -> &[VariableId] {
199        &self.variables
200    }
201
202    fn robust_kernel(&self) -> RobustKernel {
203        self.kernel
204    }
205
206    fn evaluate(&self, values: &[&DVector<f64>]) -> OptimizationResult<FactorEvaluation> {
207        let camera = se3_exp(&Vector6::from_column_slice(values[0].as_slice()));
208        let point_world = Vector3::new(values[1][0], values[1][1], values[1][2]);
209        let inverse = se3_inverse(&camera);
210        let rotation_camera_from_world = inverse.fixed_view::<3, 3>(0, 0).into_owned();
211        let point_camera =
212            rotation_camera_from_world * point_world + inverse.fixed_view::<3, 1>(0, 3);
213        let projection = self.intrinsics.project(&point_camera)?;
214        let projection_jacobian = self.intrinsics.projection_jacobian(&point_camera);
215
216        // Right perturbation of T_wc gives p_c' ~= p_c - v + [p_c]x w.
217        let camera_point_jacobian = SMatrix::<f64, 3, 6>::from_columns(&[
218            -Vector3::x(),
219            -Vector3::y(),
220            -Vector3::z(),
221            skew(&point_camera).column(0).into_owned(),
222            skew(&point_camera).column(1).into_owned(),
223            skew(&point_camera).column(2).into_owned(),
224        ]);
225        let camera_jacobian = projection_jacobian * camera_point_jacobian;
226        let point_jacobian = projection_jacobian * rotation_camera_from_world;
227        Ok(FactorEvaluation {
228            residual: DVector::from_column_slice((projection - self.pixel).as_slice()),
229            information: DMatrix::from_column_slice(2, 2, self.information.as_slice()),
230            jacobians: vec![
231                DMatrix::from_column_slice(2, 6, camera_jacobian.as_slice()),
232                DMatrix::from_column_slice(2, 3, point_jacobian.as_slice()),
233            ],
234        })
235    }
236}
237
238fn retract_camera(
239    value: &DVector<f64>,
240    increment: &DVector<f64>,
241) -> OptimizationResult<DVector<f64>> {
242    let camera = se3_exp(&Vector6::from_column_slice(value.as_slice()));
243    let delta = se3_exp(&Vector6::from_column_slice(increment.as_slice()));
244    Ok(DVector::from_column_slice(
245        se3_log(&(camera * delta)).as_slice(),
246    ))
247}
248
249#[cfg(test)]
250mod tests {
251    use super::*;
252
253    #[test]
254    fn reprojection_matches_pinhole_geometry() {
255        let intrinsics = CameraIntrinsics {
256            fx: 400.0,
257            fy: 400.0,
258            cx: 320.0,
259            cy: 240.0,
260        };
261        let pixel = reproject_world_point(
262            &Matrix4::identity(),
263            &Vector3::new(1.0, 0.5, 2.0),
264            &intrinsics,
265        )
266        .unwrap();
267        assert!((pixel.x - 520.0).abs() < 1.0e-12);
268        assert!((pixel.y - 340.0).abs() < 1.0e-12);
269    }
270
271    #[test]
272    fn reprojection_jacobians_match_finite_differences() {
273        let intrinsics = CameraIntrinsics {
274            fx: 400.0,
275            fy: 380.0,
276            cx: 320.0,
277            cy: 240.0,
278        };
279        let camera =
280            DVector::from_column_slice(Vector6::new(0.2, -0.1, 0.3, 0.05, -0.08, 0.03).as_slice());
281        let point = DVector::from_vec(vec![0.5, -0.2, 4.0]);
282        let factor = ReprojectionFactor {
283            variables: [VariableId(0), VariableId(1)],
284            pixel: Vector2::new(330.0, 220.0),
285            information: Matrix2::identity(),
286            intrinsics,
287            kernel: RobustKernel::L2,
288        };
289        let analytic = factor.evaluate(&[&camera, &point]).unwrap();
290        let epsilon = 1.0e-6;
291
292        for variable in 0..2 {
293            let dimension = if variable == 0 { 6 } else { 3 };
294            for column in 0..dimension {
295                let mut delta = DVector::zeros(dimension);
296                delta[column] = epsilon;
297                let (plus, minus) = if variable == 0 {
298                    (
299                        retract_camera(&camera, &delta).unwrap(),
300                        retract_camera(&camera, &(-&delta)).unwrap(),
301                    )
302                } else {
303                    (&point + &delta, &point - &delta)
304                };
305                let (plus_values, minus_values) = if variable == 0 {
306                    ([&plus, &point], [&minus, &point])
307                } else {
308                    ([&camera, &plus], [&camera, &minus])
309                };
310                let plus_residual = factor.evaluate(&plus_values).unwrap().residual;
311                let minus_residual = factor.evaluate(&minus_values).unwrap().residual;
312                let numerical = (plus_residual - minus_residual) / (2.0 * epsilon);
313                assert!((analytic.jacobians[variable].column(column) - numerical).norm() < 1.0e-5);
314            }
315        }
316    }
317
318    #[test]
319    fn bundle_adjustment_recovers_points_with_fixed_cameras() {
320        let intrinsics = CameraIntrinsics {
321            fx: 300.0,
322            fy: 300.0,
323            cx: 320.0,
324            cy: 240.0,
325        };
326        let cameras = vec![
327            Matrix4::identity(),
328            se3_exp(&Vector6::new(1.0, 0.0, 0.0, 0.0, 0.0, 0.0)),
329        ];
330        let truth = [
331            Vector3::new(0.2, 0.1, 4.0),
332            Vector3::new(-0.5, 0.3, 5.0),
333            Vector3::new(0.7, -0.4, 6.0),
334        ];
335        let observations = cameras
336            .iter()
337            .enumerate()
338            .flat_map(|(camera, pose)| {
339                truth
340                    .iter()
341                    .enumerate()
342                    .map(move |(point, landmark)| ReprojectionObservation {
343                        camera,
344                        point,
345                        pixel: reproject_world_point(pose, landmark, &intrinsics).unwrap(),
346                        information: Matrix2::identity(),
347                    })
348            })
349            .collect();
350        let problem = BundleAdjustmentProblem {
351            cameras,
352            points: truth
353                .iter()
354                .map(|point| point + Vector3::new(0.15, -0.1, 0.2))
355                .collect(),
356            observations,
357            intrinsics,
358        };
359        let result = bundle_adjust(
360            &problem,
361            &BundleAdjustmentConfig {
362                fixed_cameras: 2,
363                ..BundleAdjustmentConfig::default()
364            },
365        )
366        .unwrap();
367        for (actual, expected) in result.points.iter().zip(truth) {
368            assert!((actual - expected).norm() < 1.0e-6);
369        }
370        assert!(result.summary.final_cost < result.summary.initial_cost);
371    }
372}