apex_solver/factors/
projection_factor.rs

1//! Generic projection factor for bundle adjustment and SfM.
2
3use nalgebra::{DMatrix, DVector, Matrix2xX, Matrix3xX};
4use std::marker::PhantomData;
5use tracing::warn;
6
7use crate::factors::{Factor, OptimizeParams};
8use apex_camera_models::CameraModel;
9use apex_manifolds::LieGroup;
10use apex_manifolds::se3::SE3;
11
12/// Trait for optimization configuration.
13///
14/// This trait allows accessing the compile-time boolean flags for
15/// parameter optimization (pose, landmarks, intrinsics).
16pub trait OptimizationConfig: Send + Sync + 'static + Clone + Copy + Default {
17    const POSE: bool;
18    const LANDMARK: bool;
19    const INTRINSIC: bool;
20}
21
22impl<const P: bool, const L: bool, const I: bool> OptimizationConfig for OptimizeParams<P, L, I> {
23    const POSE: bool = P;
24    const LANDMARK: bool = L;
25    const INTRINSIC: bool = I;
26}
27
28/// Generic projection factor for bundle adjustment and structure from motion.
29///
30/// This factor computes reprojection errors between observed 2D image points
31/// and projected 3D landmarks. It supports flexible optimization configurations
32/// via generic types implementing `OptimizationConfig`.
33///
34/// # Type Parameters
35///
36/// - `CAM`: Camera model implementing [`CameraModel`] trait
37/// - `OP`: Optimization configuration (e.g., [`BundleAdjustment`](apex_camera_models::BundleAdjustment))
38///
39/// # Examples
40///
41/// ```
42/// # fn main() -> Result<(), Box<dyn std::error::Error>> {
43/// use apex_solver::factors::projection_factor::ProjectionFactor;
44/// use apex_solver::factors::BundleAdjustment;
45/// use apex_camera_models::PinholeCamera;
46/// use nalgebra::{Matrix2xX, Vector2};
47///
48/// let camera = PinholeCamera::from([500.0, 500.0, 320.0, 240.0]);
49/// let observations = Matrix2xX::from_columns(&[
50///     Vector2::new(100.0, 150.0),
51///     Vector2::new(200.0, 250.0),
52/// ]);
53///
54/// // Bundle adjustment: optimize pose + landmarks (intrinsics fixed)
55/// let factor: ProjectionFactor<PinholeCamera, BundleAdjustment> =
56///     ProjectionFactor::new(observations, camera);
57/// # Ok(())
58/// # }
59/// ```
60#[derive(Clone)]
61pub struct ProjectionFactor<CAM, OP>
62where
63    CAM: CameraModel,
64    OP: OptimizationConfig,
65{
66    /// 2D observations in image coordinates (2×N for N observations)
67    pub observations: Matrix2xX<f64>,
68
69    /// Camera model with intrinsic parameters
70    pub camera: CAM,
71
72    /// Fixed pose (required when POSE = false)
73    pub fixed_pose: Option<SE3>,
74
75    /// Fixed landmarks (required when LANDMARK = false), 3×N matrix
76    pub fixed_landmarks: Option<Matrix3xX<f64>>,
77
78    /// Log warnings for cheirality exceptions (points behind camera)
79    pub verbose_cheirality: bool,
80
81    /// Phantom data for optimization type
82    _phantom: PhantomData<OP>,
83}
84
85impl<CAM, OP> ProjectionFactor<CAM, OP>
86where
87    CAM: CameraModel,
88    OP: OptimizationConfig,
89{
90    /// Create a new projection factor.
91    ///
92    /// # Arguments
93    ///
94    /// * `observations` - 2D image measurements (2×N matrix)
95    /// * `camera` - Camera model with intrinsics
96    ///
97    /// # Example
98    ///
99    /// ```
100    /// # fn main() -> Result<(), Box<dyn std::error::Error>> {
101    /// # use apex_solver::factors::projection_factor::ProjectionFactor;
102    /// # use apex_solver::factors::BundleAdjustment;
103    /// # use apex_camera_models::PinholeCamera;
104    /// # use nalgebra::{Matrix2xX, Vector2};
105    /// # let camera = PinholeCamera::from([500.0, 500.0, 320.0, 240.0]);
106    /// # let observations = Matrix2xX::from_columns(&[Vector2::new(100.0, 150.0)]);
107    /// let factor: ProjectionFactor<PinholeCamera, BundleAdjustment> =
108    ///     ProjectionFactor::new(observations, camera);
109    /// # Ok(())
110    /// # }
111    /// ```
112    pub fn new(observations: Matrix2xX<f64>, camera: CAM) -> Self {
113        Self {
114            observations,
115            camera,
116            fixed_pose: None,
117            fixed_landmarks: None,
118            verbose_cheirality: false,
119            _phantom: PhantomData,
120        }
121    }
122
123    /// Set fixed pose (required when POSE = false).
124    ///
125    /// # Example
126    ///
127    /// ```
128    /// # fn main() -> Result<(), Box<dyn std::error::Error>> {
129    /// # use apex_solver::factors::projection_factor::ProjectionFactor;
130    /// # use apex_solver::factors::BundleAdjustment;
131    /// # use apex_camera_models::PinholeCamera;
132    /// # use apex_solver::manifold::se3::SE3;
133    /// # use nalgebra::{Matrix2xX, Vector2};
134    /// # let camera = PinholeCamera::from([500.0, 500.0, 320.0, 240.0]);
135    /// # let observations = Matrix2xX::from_columns(&[Vector2::new(100.0, 150.0)]);
136    /// # let factor: ProjectionFactor<PinholeCamera, BundleAdjustment> = ProjectionFactor::new(observations, camera);
137    /// let factor = factor.with_fixed_pose(SE3::identity());
138    /// # Ok(())
139    /// # }
140    /// ```
141    pub fn with_fixed_pose(mut self, pose: SE3) -> Self {
142        self.fixed_pose = Some(pose);
143        self
144    }
145
146    /// Set fixed landmarks (required when LANDMARK = false).
147    ///
148    /// # Example
149    ///
150    /// ```
151    /// # fn main() -> Result<(), Box<dyn std::error::Error>> {
152    /// # use apex_solver::factors::projection_factor::ProjectionFactor;
153    /// # use apex_solver::factors::BundleAdjustment;
154    /// # use apex_camera_models::PinholeCamera;
155    /// # use nalgebra::{Matrix2xX, Matrix3xX, Vector2, Vector3};
156    /// # let camera = PinholeCamera::from([500.0, 500.0, 320.0, 240.0]);
157    /// # let observations = Matrix2xX::from_columns(&[Vector2::new(100.0, 150.0)]);
158    /// # let factor: ProjectionFactor<PinholeCamera, BundleAdjustment> = ProjectionFactor::new(observations, camera);
159    /// # let landmarks = Matrix3xX::from_columns(&[Vector3::new(0.1, 0.2, 1.0)]);
160    /// let factor = factor.with_fixed_landmarks(landmarks);
161    /// # Ok(())
162    /// # }
163    /// ```
164    pub fn with_fixed_landmarks(mut self, landmarks: Matrix3xX<f64>) -> Self {
165        self.fixed_landmarks = Some(landmarks);
166        self
167    }
168
169    /// Enable verbose cheirality warnings.
170    ///
171    /// When enabled, logs warnings when landmarks project behind the camera.
172    pub fn with_verbose_cheirality(mut self) -> Self {
173        self.verbose_cheirality = true;
174        self
175    }
176
177    /// Get number of observations.
178    pub fn num_observations(&self) -> usize {
179        self.observations.ncols()
180    }
181
182    /// Internal evaluation function that computes residuals and Jacobians.
183    fn evaluate_internal(
184        &self,
185        pose: &SE3,
186        landmarks: &Matrix3xX<f64>,
187        camera: &CAM,
188        compute_jacobian: bool,
189    ) -> (DVector<f64>, Option<DMatrix<f64>>) {
190        let n = self.observations.ncols();
191        let residual_dim = n * 2;
192
193        // Allocate residuals
194        let mut residuals = DVector::zeros(residual_dim);
195
196        // Calculate total Jacobian dimension
197        let mut jacobian_cols = 0;
198        if OP::POSE {
199            jacobian_cols += 6; // SE3 tangent space
200        }
201        if OP::LANDMARK {
202            jacobian_cols += n * 3; // 3D landmarks
203        }
204        if OP::INTRINSIC {
205            jacobian_cols += CAM::INTRINSIC_DIM;
206        }
207
208        let mut jacobian_matrix = if compute_jacobian {
209            Some(DMatrix::zeros(residual_dim, jacobian_cols))
210        } else {
211            None
212        };
213
214        // Process each observation
215        for i in 0..n {
216            let observation = self.observations.column(i);
217            let p_world = landmarks.column(i).into_owned();
218
219            // Transform point to camera frame
220            // World-to-camera convention: pose is T_wc where p_cam = R * p_world + t
221            // This matches BAL dataset format and ReprojectionFactor
222            // pose.act() computes exactly: R * p_world + t = p_cam
223            let p_cam = pose.act(&p_world, None, None);
224
225            // Project point (includes all validity checks)
226            let uv = match camera.project(&p_cam) {
227                Ok(proj) => proj,
228                Err(_) => {
229                    if self.verbose_cheirality {
230                        warn!(
231                            "Point {} behind camera or invalid: p_cam = ({}, {}, {})",
232                            i, p_cam.x, p_cam.y, p_cam.z
233                        );
234                    }
235                    // Invalid projection: use zero residual (matches Ceres convention)
236                    residuals[i * 2] = 0.0;
237                    residuals[i * 2 + 1] = 0.0;
238                    // Jacobian rows remain zero
239                    continue;
240                }
241            };
242
243            // Compute residual
244            residuals[i * 2] = uv.x - observation.x;
245            residuals[i * 2 + 1] = uv.y - observation.y;
246
247            // Compute Jacobians if requested
248            if let Some(ref mut jac) = jacobian_matrix {
249                let mut col_offset = 0;
250
251                // Jacobian w.r.t. pose (world-to-camera convention)
252                if OP::POSE {
253                    let (d_uv_d_pcam, d_pcam_d_pose) = camera.jacobian_pose(&p_world, pose);
254                    let d_uv_d_pose = d_uv_d_pcam * d_pcam_d_pose;
255                    for r in 0..2 {
256                        for c in 0..6 {
257                            jac[(i * 2 + r, col_offset + c)] = d_uv_d_pose[(r, c)];
258                        }
259                    }
260                    col_offset += 6;
261                }
262
263                // Jacobian w.r.t. landmarks (world-to-camera convention)
264                if OP::LANDMARK {
265                    // For this landmark (3 DOF)
266                    let d_uv_d_pcam = camera.jacobian_point(&p_cam);
267                    // p_cam = R * p_world + t
268                    // ∂p_cam/∂p_world = R
269                    // ∂uv/∂p_world = ∂uv/∂p_cam * R
270                    let rotation = pose.rotation_so3().rotation_matrix();
271                    let d_uv_d_landmark = d_uv_d_pcam * rotation;
272
273                    for r in 0..2 {
274                        for c in 0..3 {
275                            jac[(i * 2 + r, col_offset + i * 3 + c)] = d_uv_d_landmark[(r, c)];
276                        }
277                    }
278                }
279
280                // Update column offset for intrinsics (if landmarks are optimized)
281                if OP::LANDMARK {
282                    col_offset += n * 3;
283                }
284
285                // Jacobian w.r.t. intrinsics (shared across all observations)
286                if OP::INTRINSIC {
287                    let d_uv_d_intrinsics = camera.jacobian_intrinsics(&p_cam);
288                    for r in 0..2 {
289                        for c in 0..CAM::INTRINSIC_DIM {
290                            jac[(i * 2 + r, col_offset + c)] = d_uv_d_intrinsics[(r, c)];
291                        }
292                    }
293                }
294            }
295        }
296
297        (residuals, jacobian_matrix)
298    }
299}
300
301// Factor trait implementation with generic dispatch
302impl<CAM, OP> Factor for ProjectionFactor<CAM, OP>
303where
304    CAM: CameraModel,
305    for<'a> CAM: From<&'a [f64]>,
306    OP: OptimizationConfig,
307{
308    fn linearize(
309        &self,
310        params: &[DVector<f64>],
311        compute_jacobian: bool,
312    ) -> (DVector<f64>, Option<DMatrix<f64>>) {
313        let mut param_idx = 0;
314
315        // Get pose (from params if optimized, else from fixed_pose)
316        let pose: SE3 = if OP::POSE {
317            params
318                .get(param_idx)
319                .map(|p| {
320                    param_idx += 1;
321                    SE3::from(p.clone())
322                })
323                .unwrap_or_else(SE3::identity)
324        } else {
325            self.fixed_pose.clone().unwrap_or_else(SE3::identity)
326        };
327
328        // Get landmarks (3×N)
329        let landmarks: Matrix3xX<f64> = if OP::LANDMARK {
330            params
331                .get(param_idx)
332                .map(|flat| {
333                    let n = flat.len() / 3;
334                    param_idx += 1;
335                    Matrix3xX::from_fn(n, |r, c| flat[c * 3 + r])
336                })
337                .unwrap_or_else(|| Matrix3xX::zeros(0))
338        } else {
339            self.fixed_landmarks
340                .clone()
341                .unwrap_or_else(|| Matrix3xX::zeros(0))
342        };
343
344        // Get camera intrinsics
345        let camera: CAM = if OP::INTRINSIC {
346            params
347                .get(param_idx)
348                .map(|p| <CAM as From<&[f64]>>::from(p.as_slice()))
349                .unwrap_or_else(|| self.camera.clone())
350        } else {
351            self.camera.clone()
352        };
353
354        // Verify dimensions
355        let n = self.observations.ncols();
356        assert_eq!(
357            landmarks.ncols(),
358            n,
359            "Number of landmarks ({}) must match observations ({})",
360            landmarks.ncols(),
361            n
362        );
363
364        // Compute residuals and Jacobians
365        self.evaluate_internal(&pose, &landmarks, &camera, compute_jacobian)
366    }
367
368    fn get_dimension(&self) -> usize {
369        self.observations.ncols() * 2
370    }
371}
372
373#[cfg(test)]
374mod tests {
375    use super::*;
376    use crate::factors::{BundleAdjustment, OnlyIntrinsics, SelfCalibration};
377    use apex_camera_models::PinholeCamera;
378    use nalgebra::{Vector2, Vector3};
379
380    type TestResult = Result<(), Box<dyn std::error::Error>>;
381
382    #[test]
383    fn test_projection_factor_creation() -> TestResult {
384        let camera = PinholeCamera::from([500.0, 500.0, 320.0, 240.0]);
385        let observations = Matrix2xX::from_columns(&[Vector2::new(100.0, 150.0)]);
386
387        // Bundle adjustment: optimize pose + landmarks (intrinsics fixed)
388        let factor: ProjectionFactor<PinholeCamera, BundleAdjustment> =
389            ProjectionFactor::new(observations, camera);
390
391        assert_eq!(factor.num_observations(), 1);
392        assert_eq!(factor.get_dimension(), 2);
393
394        Ok(())
395    }
396
397    #[test]
398    fn test_bundle_adjustment_factor() -> TestResult {
399        let camera = PinholeCamera::from([500.0, 500.0, 320.0, 240.0]);
400
401        // Create known landmark and observation
402        // World-to-camera convention: p_cam = R * p_world + t
403        // With identity pose: p_cam = p_world
404        let p_world = Vector3::new(0.1, 0.2, 1.0);
405        let pose = SE3::identity();
406
407        // Project to get observation using world-to-camera convention
408        let p_cam = pose.act(&p_world, None, None);
409        let uv = camera.project(&p_cam)?;
410
411        let observations = Matrix2xX::from_columns(&[uv]);
412
413        // Bundle adjustment: optimize pose + landmarks (intrinsics fixed)
414        let factor: ProjectionFactor<PinholeCamera, BundleAdjustment> =
415            ProjectionFactor::new(observations, camera);
416
417        // Linearize
418        let pose_vec: DVector<f64> = pose.clone().into();
419        let landmarks_vec = DVector::from_vec(vec![p_world.x, p_world.y, p_world.z]);
420        let params = vec![pose_vec, landmarks_vec];
421
422        let (residual, jacobian) = factor.linearize(&params, true);
423
424        // Residual should be near zero
425        assert!(residual.norm() < 1e-10, "Residual: {:?}", residual);
426
427        // Jacobian dimensions: 2×(6+3) = 2×9
428        let jac = jacobian.ok_or("Jacobian should be Some")?;
429        assert_eq!(jac.nrows(), 2);
430        assert_eq!(jac.ncols(), 9);
431
432        Ok(())
433    }
434
435    #[test]
436    fn test_self_calibration_factor() -> TestResult {
437        let camera = PinholeCamera::from([500.0, 500.0, 320.0, 240.0]);
438        let p_world = Vector3::new(0.1, 0.2, 1.0);
439        let pose = SE3::identity();
440
441        // Get observation using world-to-camera convention
442        let p_cam = pose.act(&p_world, None, None);
443        let uv = camera.project(&p_cam)?;
444
445        let observations = Matrix2xX::from_columns(&[uv]);
446        let factor: ProjectionFactor<PinholeCamera, SelfCalibration> =
447            ProjectionFactor::new(observations, camera);
448
449        // Linearize with all parameters
450        let pose_vec: DVector<f64> = pose.into();
451        let landmarks_vec = DVector::from_vec(vec![p_world.x, p_world.y, p_world.z]);
452        let intrinsics_vec = DVector::from_vec(vec![500.0, 500.0, 320.0, 240.0]);
453        let params = vec![pose_vec, landmarks_vec, intrinsics_vec];
454
455        let (residual, jacobian) = factor.linearize(&params, true);
456
457        assert!(residual.norm() < 1e-10);
458
459        // Jacobian dimensions: 2×(6+3+4) = 2×13
460        let jac = jacobian.ok_or("Jacobian should be Some")?;
461        assert_eq!(jac.nrows(), 2);
462        assert_eq!(jac.ncols(), 13);
463
464        Ok(())
465    }
466
467    #[test]
468    fn test_calibration_factor() -> TestResult {
469        let camera = PinholeCamera::from([500.0, 500.0, 320.0, 240.0]);
470        let pose = SE3::identity();
471        let p_world = Vector3::new(0.1, 0.2, 1.0);
472
473        // World-to-camera convention
474        let p_cam = pose.act(&p_world, None, None);
475        let uv = camera.project(&p_cam)?;
476
477        let observations = Matrix2xX::from_columns(&[uv]);
478        let landmarks = Matrix3xX::from_columns(&[p_world]);
479
480        let factor: ProjectionFactor<PinholeCamera, OnlyIntrinsics> =
481            ProjectionFactor::new(observations, camera)
482                .with_fixed_pose(pose)
483                .with_fixed_landmarks(landmarks);
484
485        // Only intrinsics are optimized
486        let intrinsics_vec = DVector::from_vec(vec![500.0, 500.0, 320.0, 240.0]);
487        let params = vec![intrinsics_vec];
488
489        let (residual, jacobian) = factor.linearize(&params, true);
490
491        assert!(residual.norm() < 1e-10);
492
493        // Jacobian dimensions: 2×4 (only intrinsics)
494        let jac = jacobian.ok_or("Jacobian should be Some")?;
495        assert_eq!(jac.nrows(), 2);
496        assert_eq!(jac.ncols(), 4);
497
498        Ok(())
499    }
500
501    #[test]
502    fn test_invalid_projection_handling() -> TestResult {
503        let camera = PinholeCamera::from([500.0, 500.0, 320.0, 240.0]);
504        let observations = Matrix2xX::from_columns(&[Vector2::new(100.0, 150.0)]);
505
506        let factor: ProjectionFactor<PinholeCamera, BundleAdjustment> =
507            ProjectionFactor::new(observations, camera).with_verbose_cheirality();
508
509        let pose = SE3::identity();
510        // Landmark behind camera
511        let _landmarks = Matrix3xX::from_columns(&[Vector3::new(0.0, 0.0, -1.0)]);
512
513        let pose_vec: DVector<f64> = pose.into();
514        let landmarks_vec = DVector::from_vec(vec![0.0, 0.0, -1.0]);
515        let params = vec![pose_vec, landmarks_vec];
516
517        let (residual, _) = factor.linearize(&params, false);
518
519        // Invalid projections should have zero residual (Ceres convention)
520        assert!(residual[0].abs() < 1e-10);
521        assert!(residual[1].abs() < 1e-10);
522
523        Ok(())
524    }
525}
apex_solver/factors/projection_factor.rs

apex_solver/factors/
projection_factor.rs
