1use 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#[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 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#[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#[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#[derive(Debug, Clone)]
67pub struct BundleAdjustmentConfig {
68 pub solver: SolverConfig,
69 pub fixed_cameras: usize,
71 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#[derive(Debug, Clone)]
89pub struct BundleAdjustmentResult {
90 pub cameras: Vec<Matrix4<f64>>,
91 pub points: Vec<Vector3<f64>>,
92 pub summary: SolverSummary,
93}
94
95pub 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
107pub 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 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}