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//! Each camera model provides two factor types:
29//! 1. **CameraParamsFactor**: Optimizes camera intrinsic parameters (fx, fy, cx, cy, distortion)
30//! 2. **ProjectionFactor**: Optimizes 3D point positions with fixed camera parameters
31//!
32//! ### Double Sphere
33//! - [`DoubleSphereCameraParamsFactor`]: Optimize camera intrinsics
34//! - [`DoubleSphereProjectionFactor`]: Optimize 3D point positions
35//!
36//! ### EUCM (Extended Unified Camera Model)
37//! - [`EucmCameraParamsFactor`]: Optimize camera intrinsics
38//! - [`EucmProjectionFactor`]: Optimize 3D point positions
39//!
40//! ### FOV (Field of View)
41//! - [`FovCameraParamsFactor`]: Optimize camera intrinsics
42//! - [`FovProjectionFactor`]: Optimize 3D point positions
43//!
44//! ### Kannala-Brandt
45//! - [`KannalaBrandtCameraParamsFactor`]: Optimize camera intrinsics
46//! - [`KannalaBrandtProjectionFactor`]: Optimize 3D point positions
47//!
48//! ### Radial-Tangential (RadTan)
49//! - [`RadTanCameraParamsFactor`]: Optimize camera intrinsics
50//! - [`RadTanProjectionFactor`]: Optimize 3D point positions
51//!
52//! ### UCM (Unified Camera Model)
53//! - [`UcmCameraParamsFactor`]: Optimize camera intrinsics
54//! - [`UcmProjectionFactor`]: Optimize 3D point positions
55//!
56//! # Linearization
57//!
58//! Each factor must provide a `linearize` method that computes:
59//! 1. **Residual** `r(x)`: The error at the current variable values
60//! 2. **Jacobian** `J = ∂r/∂x`: How the residual changes with each variable
61//!
62//! This information is used by the optimizer to compute parameter updates via Newton-type methods.
63
64use nalgebra::{DMatrix, DVector};
65use thiserror::Error;
66use tracing::error;
67
68/// Factor-specific error types for apex-solver
69#[derive(Debug, Clone, Error)]
70pub enum FactorError {
71    /// Invalid dimension mismatch between expected and actual
72    #[error("Invalid dimension: expected {expected}, got {actual}")]
73    InvalidDimension { expected: usize, actual: usize },
74
75    /// Invalid projection (point behind camera or outside valid range)
76    #[error("Invalid projection: {0}")]
77    InvalidProjection(String),
78
79    /// Jacobian computation failed
80    #[error("Jacobian computation failed: {0}")]
81    JacobianFailed(String),
82
83    /// Invalid parameter values
84    #[error("Invalid parameter values: {0}")]
85    InvalidParameters(String),
86
87    /// Numerical instability detected
88    #[error("Numerical instability: {0}")]
89    NumericalInstability(String),
90}
91
92impl FactorError {
93    /// Log the error with tracing::error and return self for chaining
94    ///
95    /// This method allows for a consistent error logging pattern throughout
96    /// the factors module, ensuring all errors are properly recorded.
97    ///
98    /// # Example
99    /// ```ignore
100    /// operation()
101    ///     .map_err(|e| FactorError::from(e).log())?;
102    /// ```
103    #[must_use]
104    pub fn log(self) -> Self {
105        error!("{}", self);
106        self
107    }
108
109    /// Log the error with the original source error for debugging context
110    ///
111    /// This method logs both the FactorError and the underlying error
112    /// from external libraries or internal operations, providing full
113    /// debugging context when errors occur.
114    ///
115    /// # Arguments
116    /// * `source_error` - The original error (must implement Debug)
117    ///
118    /// # Example
119    /// ```ignore
120    /// compute_jacobian()
121    ///     .map_err(|e| {
122    ///         FactorError::JacobianFailed("Matrix computation failed".to_string())
123    ///             .log_with_source(e)
124    ///     })?;
125    /// ```
126    #[must_use]
127    pub fn log_with_source<E: std::fmt::Debug>(self, source_error: E) -> Self {
128        error!("{} | Source: {:?}", self, source_error);
129        self
130    }
131}
132
133/// Result type for factor operations
134pub type FactorResult<T> = Result<T, FactorError>;
135
136/// Trait for factor (constraint) implementations in factor graph optimization.
137///
138/// A factor represents a measurement or constraint connecting one or more variables.
139/// It computes the residual (error) and Jacobian for the current variable values,
140/// which are used by the optimizer to minimize the total cost.
141///
142/// # Implementing Custom Factors
143///
144/// To create a custom factor:
145/// 1. Implement this trait
146/// 2. Define the residual function `r(x)` (how to compute error from variable values)
147/// 3. Compute the Jacobian `J = ∂r/∂x` (analytically or numerically)
148/// 4. Return the residual dimension
149///
150/// # Thread Safety
151///
152/// Factors must be `Send + Sync` to enable parallel residual/Jacobian evaluation.
153///
154/// # Example
155///
156/// ```
157/// use apex_solver::factors::Factor;
158/// use nalgebra::{DMatrix, DVector};
159///
160/// // Simple 1D range measurement factor
161/// struct RangeFactor {
162///     measurement: f64,  // Measured distance
163/// }
164///
165/// impl Factor for RangeFactor {
166///     fn linearize(&self, params: &[DVector<f64>], compute_jacobian: bool) -> (DVector<f64>, Option<DMatrix<f64>>) {
167///         // params[0] is a 2D point [x, y]
168///         let x = params[0][0];
169///         let y = params[0][1];
170///
171///         // Residual: measured distance - actual distance
172///         let predicted_distance = (x * x + y * y).sqrt();
173///         let residual = DVector::from_vec(vec![self.measurement - predicted_distance]);
174///
175///         // Jacobian: ∂(residual)/∂[x, y]
176///         let jacobian = if compute_jacobian {
177///             Some(DMatrix::from_row_slice(1, 2, &[
178///                 -x / predicted_distance,
179///                 -y / predicted_distance,
180///             ]))
181///         } else {
182///             None
183///         };
184///
185///         (residual, jacobian)
186///     }
187///
188///     fn get_dimension(&self) -> usize { 1 }
189/// }
190/// ```
191pub trait Factor: Send + Sync {
192    /// Compute the residual and Jacobian at the given parameter values.
193    ///
194    /// # Arguments
195    ///
196    /// * `params` - Slice of variable values (one `DVector` per connected variable)
197    /// * `compute_jacobian` - Whether to compute the Jacobian matrix
198    ///
199    /// # Returns
200    ///
201    /// Tuple `(residual, jacobian)` where:
202    /// - `residual`: N-dimensional error vector
203    /// - `jacobian`: N × M matrix where M is the total DOF of all variables
204    ///
205    /// # Example
206    ///
207    /// For a between factor connecting two SE2 poses (3 DOF each):
208    /// - Input: `params = [pose1 (3×1), pose2 (3×1)]`
209    /// - Output: `(residual (3×1), jacobian (3×6))`
210    fn linearize(
211        &self,
212        params: &[DVector<f64>],
213        compute_jacobian: bool,
214    ) -> (DVector<f64>, Option<DMatrix<f64>>);
215
216    /// Get the dimension of the residual vector.
217    ///
218    /// # Returns
219    ///
220    /// Number of elements in the residual vector (number of constraints)
221    ///
222    /// # Example
223    ///
224    /// - SE2 between factor: 3 (dx, dy, dtheta)
225    /// - SE3 between factor: 6 (translation + rotation)
226    /// - Prior factor: dimension of the variable
227    fn get_dimension(&self) -> usize;
228}
229
230// Pose factors
231pub mod prior_factor;
232pub mod se2_factor;
233pub mod se3_factor;
234
235// Camera projection factors
236pub mod double_sphere_factor;
237pub mod eucm_factor;
238pub mod fov_factor;
239pub mod kannala_brandt_factor;
240pub mod rad_tan_factor;
241pub mod ucm_factor;
242
243// Re-export all factor types
244
245// Pose factors
246pub use prior_factor::PriorFactor;
247pub use se2_factor::BetweenFactorSE2;
248pub use se3_factor::BetweenFactorSE3;
249
250// Camera projection factors - Double Sphere
251pub use double_sphere_factor::{DoubleSphereCameraParamsFactor, DoubleSphereProjectionFactor};
252
253// Camera projection factors - EUCM
254pub use eucm_factor::{EucmCameraParamsFactor, EucmProjectionFactor};
255
256// Camera projection factors - FOV
257pub use fov_factor::{FovCameraParamsFactor, FovProjectionFactor};
258
259// Camera projection factors - Kannala-Brandt
260pub use kannala_brandt_factor::{KannalaBrandtCameraParamsFactor, KannalaBrandtProjectionFactor};
261
262// Camera projection factors - RadTan
263pub use rad_tan_factor::{RadTanCameraParamsFactor, RadTanProjectionFactor};
264
265// Camera projection factors - UCM
266pub use ucm_factor::{UcmCameraParamsFactor, UcmProjectionFactor};
apex_solver/factors/mod.rs

apex_solver/factors/
mod.rs
