apex_solver/factors/
mod.rs

1//! Factor implementations for graph-based optimization problems.
2//!
3//! Factors (also called constraints or error functions) represent measurements or relationships
4//! between variables in a factor graph. Each factor computes a residual (error) vector and its
5//! Jacobian with respect to the connected variables.
6//!
7//! # Factor Graph Formulation
8//!
9//! In graph-based SLAM and bundle adjustment, the optimization problem is represented as:
10//!
11//! ```text
12//! minimize Σ_i ||r_i(x)||²
13//! ```
14//!
15//! where:
16//! - `x` is the set of variables (poses, landmarks, etc.)
17//! - `r_i(x)` is the residual function for factor i
18//! - Each factor connects one or more variables
19//!
20//! # Factor Types
21//!
22//! ## Pose Factors
23//! - **Between factors**: Relative pose constraints (SE2, SE3)
24//! - **Prior factors**: Unary constraints on single variables
25//!
26//! ## Camera Projection Factors
27//!
28//! Use [`ProjectionFactor`] with a specific
29//! [`CameraModel`](apex_camera_models::CameraModel).
30//!
31//! Supported camera models:
32//! - [`PinholeCamera`](camera::PinholeCamera)
33//! - [`DoubleSphereCamera`](camera::DoubleSphereCamera)
34//! - [`EucmCamera`](camera::EucmCamera)
35//! - [`FovCamera`](camera::FovCamera)
36//! - [`KannalaBrandtCamera`](camera::KannalaBrandtCamera)
37//! - [`RadTanCamera`](camera::RadTanCamera)
38//! - [`UcmCamera`](camera::UcmCamera)
39//!
40//! # Linearization
41//!
42//! Each factor must provide a `linearize` method that writes the residual and Jacobian
43//! directly into caller-provided buffers — no heap allocation for the output.
44//!
45//! This information is used by the optimizer to compute parameter updates via Newton-type methods.
46
47use crate::core::variable::ManifoldVariable;
48use thiserror::Error;
49
50pub mod common;
51pub mod imu;
52pub mod lidar;
53pub mod marginal;
54pub mod motion;
55pub mod navigation;
56pub mod pose;
57pub mod ranging;
58pub mod visual;
59
60// Optimization configuration types
61
62/// Configuration for which parameters to optimize.
63///
64/// Uses const generic booleans for compile-time optimization selection.
65///
66/// # Type Parameters
67///
68/// - `POSE`: Whether to optimize camera pose (SE3 transformation)
69/// - `LANDMARK`: Whether to optimize 3D landmark positions
70/// - `INTRINSIC`: Whether to optimize camera intrinsic parameters
71#[derive(Debug, Clone, Copy, Default)]
72pub struct OptimizeParams<const POSE: bool, const LANDMARK: bool, const INTRINSIC: bool>;
73
74impl<const P: bool, const L: bool, const I: bool> OptimizeParams<P, L, I> {
75    /// Whether to optimize camera pose
76    pub const POSE: bool = P;
77    /// Whether to optimize 3D landmarks
78    pub const LANDMARK: bool = L;
79    /// Whether to optimize camera intrinsics
80    pub const INTRINSIC: bool = I;
81}
82
83/// Bundle Adjustment: optimize pose + landmarks (intrinsics fixed).
84pub type BundleAdjustment = OptimizeParams<true, true, false>;
85
86/// Self-Calibration: optimize pose + landmarks + intrinsics.
87pub type SelfCalibration = OptimizeParams<true, true, true>;
88
89/// Only Intrinsics: optimize intrinsics (pose and landmarks fixed).
90pub type OnlyIntrinsics = OptimizeParams<false, false, true>;
91
92/// Only Pose: optimize pose (landmarks and intrinsics fixed).
93pub type OnlyPose = OptimizeParams<true, false, false>;
94
95/// Only Landmarks: optimize landmarks (pose and intrinsics fixed).
96pub type OnlyLandmarks = OptimizeParams<false, true, false>;
97
98/// Pose and Intrinsics: optimize pose + intrinsics (landmarks fixed).
99pub type PoseAndIntrinsics = OptimizeParams<true, false, true>;
100
101/// Landmarks and Intrinsics: optimize landmarks + intrinsics (pose fixed).
102pub type LandmarksAndIntrinsics = OptimizeParams<false, true, true>;
103
104// Camera module alias for backward compatibility
105// Re-exports the apex-camera-models crate as `camera` module
106pub mod camera {
107    pub use apex_camera_models::*;
108}
109
110/// Factor-specific error types for apex-solver
111#[derive(Debug, Clone, Error)]
112pub enum FactorError {
113    /// Invalid dimension mismatch between expected and actual
114    #[error("Invalid dimension: expected {expected}, got {actual}")]
115    InvalidDimension { expected: usize, actual: usize },
116
117    /// Invalid projection (point behind camera or outside valid range)
118    #[error("Invalid projection: {0}")]
119    InvalidProjection(String),
120
121    /// Jacobian computation failed
122    #[error("Jacobian computation failed: {0}")]
123    JacobianFailed(String),
124
125    /// Invalid parameter values
126    #[error("Invalid parameter values: {0}")]
127    InvalidParameters(String),
128
129    /// Numerical instability detected
130    #[error("Numerical instability: {0}")]
131    NumericalInstability(String),
132}
133
134/// Result type for factor operations
135pub type FactorResult<T> = Result<T, FactorError>;
136
137/// Trait for factor (constraint) implementations in factor graph optimization.
138///
139/// A factor represents a measurement or constraint connecting one or more variables.
140/// It writes the residual and Jacobian directly into caller-provided buffers — no heap
141/// allocation for the output. Parameters arrive as zero-copy `&[f64]` slices from
142/// manifold storage.
143///
144/// # Thread Safety
145///
146/// Factors must be `Send + Sync` to enable parallel residual/Jacobian evaluation.
147///
148/// # Example
149///
150/// ```
151/// use apex_solver::factors::Factor;
152/// use faer::prelude::ReborrowMut;
153///
154/// // Simple 1D range measurement factor
155/// struct RangeFactor {
156///     measurement: f64,
157/// }
158///
159/// impl Factor for RangeFactor {
160///     fn linearize(
161///         &self,
162///         params: &[&[f64]],
163///         residual: &mut [f64],
164///         jacobian: Option<faer::mat::MatMut<'_, f64>>,
165///     ) {
166///         let x = params[0][0];
167///         let y = params[0][1];
168///         let dist = (x * x + y * y).sqrt();
169///         residual[0] = self.measurement - dist;
170///         if let Some(mut jac) = jacobian {
171///             *jac.rb_mut().get_mut(0, 0) = -x / dist;
172///             *jac.rb_mut().get_mut(0, 1) = -y / dist;
173///         }
174///     }
175///     fn residual_dim(&self) -> usize { 1 }
176///     fn jacobian_shape(&self) -> (usize, usize) { (1, 2) }
177/// }
178/// ```
179pub trait Factor: Send + Sync {
180    /// Write residual and (optionally) Jacobian into pre-allocated buffers.
181    ///
182    /// - `params`: one `&[f64]` slice per connected variable (from `ManifoldVariable::as_param_slice`)
183    /// - `residual`: pre-allocated output buffer of length `residual_dim()`
184    /// - `jacobian`: optional column-major `MatMut` of shape `jacobian_shape()`
185    fn linearize(
186        &self,
187        params: &[&[f64]],
188        residual: &mut [f64],
189        jacobian: Option<faer::mat::MatMut<'_, f64>>,
190    );
191
192    /// Number of residual rows (length of the `residual` buffer).
193    fn residual_dim(&self) -> usize;
194
195    /// `(rows, cols)` of the Jacobian — `rows == residual_dim()`, `cols == sum of variable DOFs`.
196    fn jacobian_shape(&self) -> (usize, usize);
197
198    /// Whether this factor already applies its own square-root information to
199    /// the residual and Jacobian it writes.
200    ///
201    /// Most factors return the raw residual and let the
202    /// [`NoiseModel`](crate::core::noise::NoiseModel) supplied at registration
203    /// whiten it. A few cannot: their weighting is bound up with the
204    /// measurement itself — GICP's combined point covariance, a smart factor's
205    /// internal Schur elimination, a preintegration's information matrix — so
206    /// they whiten as they go and **must** be registered with
207    /// `NoiseModel::null()`.
208    ///
209    /// Pairing an internally-whitened factor with a non-null noise model
210    /// whitens twice and silently over-weights the block; the two conventions
211    /// are indistinguishable by inspecting a residual. Overriding this lets
212    /// [`Problem::try_add_residual_block_with_noise`](crate::core::problem::Problem::try_add_residual_block_with_noise)
213    /// reject the mistake at registration instead.
214    fn whitens_internally(&self) -> bool {
215        false
216    }
217
218    /// Validate this factor against the variables it is being registered with.
219    ///
220    /// Called once by [`Problem::try_add_residual_block`](crate::core::problem::Problem::try_add_residual_block)
221    /// before the block is stored, so mismatched variable shapes surface as a
222    /// typed registration error instead of a mid-evaluation panic inside the
223    /// parallel assembly. `variables` is parallel to the `variable_keys`
224    /// passed to registration.
225    fn validate_variables(&self, _variables: &[&dyn ManifoldVariable]) -> Result<(), String> {
226        Ok(())
227    }
228}
229
230#[cfg(test)]
231mod tests {
232    use super::*;
233    use crate::error::ErrorLogging;
234    use faer::prelude::ReborrowMut;
235    use nalgebra::dvector;
236
237    // -------------------------------------------------------------------------
238    // OptimizeParams const generic flags — all 7 type aliases
239    // -------------------------------------------------------------------------
240
241    #[test]
242    fn test_optimize_params_bundle_adjustment_flags() {
243        const { assert!(BundleAdjustment::POSE) };
244        const { assert!(BundleAdjustment::LANDMARK) };
245        const { assert!(!BundleAdjustment::INTRINSIC) };
246    }
247
248    #[test]
249    fn test_optimize_params_self_calibration_flags() {
250        const { assert!(SelfCalibration::POSE) };
251        const { assert!(SelfCalibration::LANDMARK) };
252        const { assert!(SelfCalibration::INTRINSIC) };
253    }
254
255    #[test]
256    fn test_optimize_params_only_intrinsics_flags() {
257        const { assert!(!OnlyIntrinsics::POSE) };
258        const { assert!(!OnlyIntrinsics::LANDMARK) };
259        const { assert!(OnlyIntrinsics::INTRINSIC) };
260    }
261
262    #[test]
263    fn test_optimize_params_only_pose_flags() {
264        const { assert!(OnlyPose::POSE) };
265        const { assert!(!OnlyPose::LANDMARK) };
266        const { assert!(!OnlyPose::INTRINSIC) };
267    }
268
269    #[test]
270    fn test_optimize_params_only_landmarks_flags() {
271        const { assert!(!OnlyLandmarks::POSE) };
272        const { assert!(OnlyLandmarks::LANDMARK) };
273        const { assert!(!OnlyLandmarks::INTRINSIC) };
274    }
275
276    #[test]
277    fn test_optimize_params_pose_and_intrinsics_flags() {
278        const { assert!(PoseAndIntrinsics::POSE) };
279        const { assert!(!PoseAndIntrinsics::LANDMARK) };
280        const { assert!(PoseAndIntrinsics::INTRINSIC) };
281    }
282
283    #[test]
284    fn test_optimize_params_landmarks_and_intrinsics_flags() {
285        const { assert!(!LandmarksAndIntrinsics::POSE) };
286        const { assert!(LandmarksAndIntrinsics::LANDMARK) };
287        const { assert!(LandmarksAndIntrinsics::INTRINSIC) };
288    }
289
290    // -------------------------------------------------------------------------
291    // FactorError Display — one per variant
292    // -------------------------------------------------------------------------
293
294    #[test]
295    fn test_factor_error_invalid_dimension_display() {
296        let e = FactorError::InvalidDimension {
297            expected: 3,
298            actual: 6,
299        };
300        let s = e.to_string();
301        assert!(s.contains("3"), "{s}");
302        assert!(s.contains("6"), "{s}");
303    }
304
305    #[test]
306    fn test_factor_error_invalid_projection_display() {
307        let e = FactorError::InvalidProjection("behind camera".into());
308        assert!(e.to_string().contains("behind camera"));
309    }
310
311    #[test]
312    fn test_factor_error_jacobian_failed_display() {
313        let e = FactorError::JacobianFailed("singular".into());
314        assert!(e.to_string().contains("singular"));
315    }
316
317    #[test]
318    fn test_factor_error_invalid_parameters_display() {
319        let e = FactorError::InvalidParameters("nan detected".into());
320        assert!(e.to_string().contains("nan detected"));
321    }
322
323    #[test]
324    fn test_factor_error_numerical_instability_display() {
325        let e = FactorError::NumericalInstability("overflow".into());
326        assert!(e.to_string().contains("overflow"));
327    }
328
329    // -------------------------------------------------------------------------
330    // log() / log_with_source() return self
331    // -------------------------------------------------------------------------
332
333    #[test]
334    fn test_factor_error_log_returns_self() {
335        let e = FactorError::JacobianFailed("test_log".into());
336        let returned = e.log();
337        assert!(returned.to_string().contains("test_log"));
338    }
339
340    #[test]
341    fn test_factor_error_log_with_source_returns_self() {
342        let e = FactorError::InvalidProjection("proj_log".into());
343        let source = std::io::Error::other("src");
344        let returned = e.log_with_source(source);
345        assert!(returned.to_string().contains("proj_log"));
346    }
347
348    // -------------------------------------------------------------------------
349    // Factor trait — local implementation
350    // -------------------------------------------------------------------------
351
352    struct ConstantFactor {
353        value: f64,
354    }
355
356    impl Factor for ConstantFactor {
357        fn linearize(
358            &self,
359            params: &[&[f64]],
360            residual: &mut [f64],
361            jacobian: Option<faer::mat::MatMut<'_, f64>>,
362        ) {
363            residual[0] = params[0][0] - self.value;
364            if let Some(mut jac) = jacobian {
365                *jac.rb_mut().get_mut(0, 0) = 1.0;
366            }
367        }
368
369        fn residual_dim(&self) -> usize {
370            1
371        }
372
373        fn jacobian_shape(&self) -> (usize, usize) {
374            (1, 1)
375        }
376    }
377
378    #[test]
379    fn test_factor_compute_with_jacobian() {
380        let f = ConstantFactor { value: 3.0 };
381        let p = dvector![5.0];
382        let params: Vec<&[f64]> = vec![p.as_slice()];
383        let mut residual = vec![0.0f64; 1];
384        let mut jac_buf = vec![0.0f64; 1];
385        let jac_mut = faer::mat::MatMut::from_column_major_slice_mut(&mut jac_buf, 1, 1);
386        f.linearize(&params, &mut residual, Some(jac_mut));
387        assert!((residual[0] - 2.0).abs() < 1e-12);
388        assert!((jac_buf[0] - 1.0).abs() < 1e-12);
389    }
390
391    #[test]
392    fn test_factor_compute_without_jacobian() {
393        let f = ConstantFactor { value: 3.0 };
394        let p = dvector![5.0];
395        let params: Vec<&[f64]> = vec![p.as_slice()];
396        let mut residual = vec![0.0f64; 1];
397        f.linearize(&params, &mut residual, None);
398        assert!((residual[0] - 2.0).abs() < 1e-12);
399    }
400
401    #[test]
402    fn test_factor_residual_dim() {
403        let f = ConstantFactor { value: 0.0 };
404        assert_eq!(f.residual_dim(), 1);
405    }
406
407    // -------------------------------------------------------------------------
408    // FactorResult type alias
409    // -------------------------------------------------------------------------
410
411    #[test]
412    fn test_factor_result_ok() {
413        let r: FactorResult<f64> = Ok(1.0);
414        assert!(r.is_ok());
415    }
416
417    #[test]
418    fn test_factor_result_err() {
419        let r: FactorResult<f64> = Err(FactorError::InvalidParameters("bad".into()));
420        assert!(r.is_err());
421    }
422}
apex_solver/factors/mod.rs

apex_solver/factors/
mod.rs
