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`](camera::ProjectionFactor) with a specific [`CameraModel`](camera::CameraModel).
29//!
30//! Supported camera models:
31//! - [`PinholeCamera`](camera::PinholeCamera)
32//! - [`DoubleSphereCamera`](camera::DoubleSphereCamera)
33//! - [`EucmCamera`](camera::EucmCamera)
34//! - [`FovCamera`](camera::FovCamera)
35//! - [`KannalaBrandtCamera`](camera::KannalaBrandtCamera)
36//! - [`RadTanCamera`](camera::RadTanCamera)
37//! - [`UcmCamera`](camera::UcmCamera)
38//!
39//! # Linearization
40//!
41//! Each factor must provide a `linearize` method that computes:
42//! 1. **Residual** `r(x)`: The error at the current variable values
43//! 2. **Jacobian** `J = ∂r/∂x`: How the residual changes with each variable
44//!
45//! This information is used by the optimizer to compute parameter updates via Newton-type methods.
46
47use nalgebra::{DMatrix, DVector};
48use thiserror::Error;
49use tracing::error;
50
51// Pose factors
52pub mod between_factor;
53pub mod prior_factor;
54pub mod projection_factor;
55
56pub use between_factor::BetweenFactor;
57pub use prior_factor::PriorFactor;
58pub use projection_factor::ProjectionFactor;
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
134impl FactorError {
135    /// Log the error with tracing::error and return self for chaining
136    ///
137    /// This method allows for a consistent error logging pattern throughout
138    /// the factors module, ensuring all errors are properly recorded.
139    ///
140    /// # Example
141    /// ```
142    /// # use apex_solver::factors::FactorError;
143    /// # fn operation() -> Result<(), FactorError> { Ok(()) }
144    /// # fn example() -> Result<(), FactorError> {
145    /// operation()
146    ///     .map_err(|e| e.log())?;
147    /// # Ok(())
148    /// # }
149    /// ```
150    #[must_use]
151    pub fn log(self) -> Self {
152        error!("{}", self);
153        self
154    }
155
156    /// Log the error with the original source error for debugging context
157    ///
158    /// This method logs both the FactorError and the underlying error
159    /// from external libraries or internal operations, providing full
160    /// debugging context when errors occur.
161    ///
162    /// # Arguments
163    /// * `source_error` - The original error (must implement Debug)
164    ///
165    /// # Example
166    /// ```
167    /// # use apex_solver::factors::FactorError;
168    /// # fn compute_jacobian() -> Result<(), std::io::Error> { Ok(()) }
169    /// # fn example() -> Result<(), FactorError> {
170    /// compute_jacobian()
171    ///     .map_err(|e| {
172    ///         FactorError::JacobianFailed("Matrix computation failed".to_string())
173    ///             .log_with_source(e)
174    ///     })?;
175    /// # Ok(())
176    /// # }
177    /// ```
178    #[must_use]
179    pub fn log_with_source<E: std::fmt::Debug>(self, source_error: E) -> Self {
180        error!("{} | Source: {:?}", self, source_error);
181        self
182    }
183}
184
185/// Result type for factor operations
186pub type FactorResult<T> = Result<T, FactorError>;
187
188/// Trait for factor (constraint) implementations in factor graph optimization.
189///
190/// A factor represents a measurement or constraint connecting one or more variables.
191/// It computes the residual (error) and Jacobian for the current variable values,
192/// which are used by the optimizer to minimize the total cost.
193///
194/// # Implementing Custom Factors
195///
196/// To create a custom factor:
197/// 1. Implement this trait
198/// 2. Define the residual function `r(x)` (how to compute error from variable values)
199/// 3. Compute the Jacobian `J = ∂r/∂x` (analytically or numerically)
200/// 4. Return the residual dimension
201///
202/// # Thread Safety
203///
204/// Factors must be `Send + Sync` to enable parallel residual/Jacobian evaluation.
205///
206/// # Example
207///
208/// ```
209/// use apex_solver::factors::Factor;
210/// use nalgebra::{DMatrix, DVector};
211///
212/// // Simple 1D range measurement factor
213/// struct RangeFactor {
214///     measurement: f64,  // Measured distance
215/// }
216///
217/// impl Factor for RangeFactor {
218///     fn linearize(&self, params: &[DVector<f64>], compute_jacobian: bool) -> (DVector<f64>, Option<DMatrix<f64>>) {
219///         // params[0] is a 2D point [x, y]
220///         let x = params[0][0];
221///         let y = params[0][1];
222///
223///         // Residual: measured distance - actual distance
224///         let predicted_distance = (x * x + y * y).sqrt();
225///         let residual = DVector::from_vec(vec![self.measurement - predicted_distance]);
226///
227///         // Jacobian: ∂(residual)/∂[x, y]
228///         let jacobian = if compute_jacobian {
229///             Some(DMatrix::from_row_slice(1, 2, &[
230///                 -x / predicted_distance,
231///                 -y / predicted_distance,
232///             ]))
233///         } else {
234///             None
235///         };
236///
237///         (residual, jacobian)
238///     }
239///
240///     fn get_dimension(&self) -> usize { 1 }
241/// }
242/// ```
243pub trait Factor: Send + Sync {
244    /// Compute the residual and Jacobian at the given parameter values.
245    ///
246    /// # Arguments
247    ///
248    /// * `params` - Slice of variable values (one `DVector` per connected variable)
249    /// * `compute_jacobian` - Whether to compute the Jacobian matrix
250    ///
251    /// # Returns
252    ///
253    /// Tuple `(residual, jacobian)` where:
254    /// - `residual`: N-dimensional error vector
255    /// - `jacobian`: N × M matrix where M is the total DOF of all variables
256    ///
257    /// # Example
258    ///
259    /// For a between factor connecting two SE2 poses (3 DOF each):
260    /// - Input: `params = [pose1 (3×1), pose2 (3×1)]`
261    /// - Output: `(residual (3×1), jacobian (3×6))`
262    fn linearize(
263        &self,
264        params: &[DVector<f64>],
265        compute_jacobian: bool,
266    ) -> (DVector<f64>, Option<DMatrix<f64>>);
267
268    /// Get the dimension of the residual vector.
269    ///
270    /// # Returns
271    ///
272    /// Number of elements in the residual vector (number of constraints)
273    ///
274    /// # Example
275    ///
276    /// - SE2 between factor: 3 (dx, dy, dtheta)
277    /// - SE3 between factor: 6 (translation + rotation)
278    /// - Prior factor: dimension of the variable
279    fn get_dimension(&self) -> usize;
280}
apex_solver/factors/mod.rs

apex_solver/factors/
mod.rs
