apex_solver/optimizer/
mod.rs

1//! Optimization solvers for nonlinear least squares problems.
2//!
3//! This module provides various optimization algorithms specifically designed
4//! for nonlinear least squares problems commonly found in computer vision:
5//! - Levenberg-Marquardt algorithm
6//! - Gauss-Newton algorithm
7//! - Dog Leg algorithm
8
9use crate::core::problem;
10use crate::linalg;
11use crate::manifold;
12use faer;
13use nalgebra;
14use std::collections;
15use std::fmt;
16use std::time;
17use thiserror;
18
19pub mod dog_leg;
20pub mod gauss_newton;
21pub mod levenberg_marquardt;
22pub mod visualization;
23
24pub use dog_leg::DogLeg;
25pub use gauss_newton::GaussNewton;
26pub use levenberg_marquardt::LevenbergMarquardt;
27pub use visualization::OptimizationVisualizer;
28
29/// Type of optimization solver algorithm to use
30#[derive(Default, Clone, Copy, PartialEq, Eq)]
31pub enum OptimizerType {
32    /// Levenberg-Marquardt algorithm (robust, adaptive damping)
33    #[default]
34    LevenbergMarquardt,
35    /// Gauss-Newton algorithm (fast convergence, may be unstable)
36    GaussNewton,
37    /// Dog Leg algorithm (trust region method)
38    DogLeg,
39}
40
41impl fmt::Display for OptimizerType {
42    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
43        match self {
44            OptimizerType::LevenbergMarquardt => write!(f, "Levenberg-Marquardt"),
45            OptimizerType::GaussNewton => write!(f, "Gauss-Newton"),
46            OptimizerType::DogLeg => write!(f, "Dog Leg"),
47        }
48    }
49}
50
51/// Optimizer-specific error types for apex-solver
52#[derive(Debug, Clone, thiserror::Error)]
53pub enum OptimizerError {
54    /// Linear system solve failed during optimization
55    #[error("Linear system solve failed: {0}")]
56    LinearSolveFailed(String),
57
58    /// Maximum iterations reached without achieving convergence
59    #[error("Maximum iterations ({max_iters}) reached without convergence")]
60    MaxIterationsReached { max_iters: usize },
61
62    /// Trust region radius became too small
63    #[error("Trust region radius became too small: {radius:.6e} < {min_radius:.6e}")]
64    TrustRegionFailure { radius: f64, min_radius: f64 },
65
66    /// Damping parameter became too large (LM-specific)
67    #[error("Damping parameter became too large: {damping:.6e} > {max_damping:.6e}")]
68    DampingFailure { damping: f64, max_damping: f64 },
69
70    /// Cost increased unexpectedly when it should decrease
71    #[error("Cost increased unexpectedly: {old_cost:.6e} -> {new_cost:.6e}")]
72    CostIncrease { old_cost: f64, new_cost: f64 },
73
74    /// Jacobian computation failed
75    #[error("Jacobian computation failed: {0}")]
76    JacobianFailed(String),
77
78    /// Invalid optimization parameters provided
79    #[error("Invalid optimization parameters: {0}")]
80    InvalidParameters(String),
81
82    /// Numerical instability detected (NaN, Inf in cost, gradient, or parameters)
83    #[error("Numerical instability detected: {0}")]
84    NumericalInstability(String),
85
86    /// Linear algebra operation failed
87    #[error("Linear algebra error: {0}")]
88    LinAlg(#[from] linalg::LinAlgError),
89
90    /// Problem has no variables to optimize
91    #[error("Problem has no variables to optimize")]
92    EmptyProblem,
93
94    /// Problem has no residual blocks
95    #[error("Problem has no residual blocks")]
96    NoResidualBlocks,
97}
98
99/// Result type for optimizer operations
100pub type OptimizerResult<T> = Result<T, OptimizerError>;
101
102// State information during iterative optimization.
103// #[derive(Debug, Clone)]
104// pub struct IterativeState {
105//     /// Current iteration number
106//     pub iteration: usize,
107//     /// Current cost value
108//     pub cost: f64,
109//     /// Current gradient norm
110//     pub gradient_norm: f64,
111//     /// Current parameter update norm
112//     pub parameter_update_norm: f64,
113//     /// Time elapsed since start
114//     pub elapsed_time: Duration,
115// }
116
117/// Detailed convergence information.
118#[derive(Debug, Clone)]
119pub struct ConvergenceInfo {
120    /// Final gradient norm
121    pub final_gradient_norm: f64,
122    /// Final parameter update norm
123    pub final_parameter_update_norm: f64,
124    /// Cost function evaluation count
125    pub cost_evaluations: usize,
126    /// Jacobian evaluation count
127    pub jacobian_evaluations: usize,
128}
129
130impl fmt::Display for ConvergenceInfo {
131    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
132        write!(
133            f,
134            "Final gradient norm: {:.2e}, Final parameter update norm: {:.2e}, Cost evaluations: {}, Jacobian evaluations: {}",
135            self.final_gradient_norm,
136            self.final_parameter_update_norm,
137            self.cost_evaluations,
138            self.jacobian_evaluations
139        )
140    }
141}
142
143/// Status of an optimization process
144#[derive(Debug, Clone, PartialEq, Eq)]
145pub enum OptimizationStatus {
146    /// Optimization converged successfully
147    Converged,
148    /// Maximum number of iterations reached
149    MaxIterationsReached,
150    /// Cost function tolerance reached
151    CostToleranceReached,
152    /// Parameter tolerance reached
153    ParameterToleranceReached,
154    /// Gradient tolerance reached
155    GradientToleranceReached,
156    /// Optimization failed due to numerical issues
157    NumericalFailure,
158    /// User requested termination
159    UserTerminated,
160    /// Timeout reached
161    Timeout,
162    /// Trust region radius fell below minimum threshold
163    TrustRegionRadiusTooSmall,
164    /// Objective function fell below user-specified cutoff
165    MinCostThresholdReached,
166    /// Jacobian matrix is singular or ill-conditioned
167    IllConditionedJacobian,
168    /// NaN or Inf detected in cost or parameters
169    InvalidNumericalValues,
170    /// Other failure
171    Failed(String),
172}
173
174impl fmt::Display for OptimizationStatus {
175    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
176        match self {
177            OptimizationStatus::Converged => write!(f, "Converged"),
178            OptimizationStatus::MaxIterationsReached => write!(f, "Maximum iterations reached"),
179            OptimizationStatus::CostToleranceReached => write!(f, "Cost tolerance reached"),
180            OptimizationStatus::ParameterToleranceReached => {
181                write!(f, "Parameter tolerance reached")
182            }
183            OptimizationStatus::GradientToleranceReached => write!(f, "Gradient tolerance reached"),
184            OptimizationStatus::NumericalFailure => write!(f, "Numerical failure"),
185            OptimizationStatus::UserTerminated => write!(f, "User terminated"),
186            OptimizationStatus::Timeout => write!(f, "Timeout"),
187            OptimizationStatus::TrustRegionRadiusTooSmall => {
188                write!(f, "Trust region radius too small")
189            }
190            OptimizationStatus::MinCostThresholdReached => {
191                write!(f, "Minimum cost threshold reached")
192            }
193            OptimizationStatus::IllConditionedJacobian => {
194                write!(f, "Ill-conditioned Jacobian matrix")
195            }
196            OptimizationStatus::InvalidNumericalValues => {
197                write!(f, "Invalid numerical values (NaN/Inf) detected")
198            }
199            OptimizationStatus::Failed(msg) => write!(f, "Failed: {msg}"),
200        }
201    }
202}
203
204/// Result of a solver execution.
205#[derive(Clone)]
206pub struct SolverResult<T> {
207    /// Final parameters
208    pub parameters: T,
209    /// Final optimization status
210    pub status: OptimizationStatus,
211    /// Initial cost value
212    pub initial_cost: f64,
213    /// Final cost value
214    pub final_cost: f64,
215    /// Number of iterations performed
216    pub iterations: usize,
217    /// Total time elapsed
218    pub elapsed_time: time::Duration,
219    /// Convergence statistics
220    pub convergence_info: Option<ConvergenceInfo>,
221    /// Per-variable covariance matrices (uncertainty estimation)
222    ///
223    /// This is `None` if covariance computation was not enabled in the solver configuration.
224    /// When present, it contains a mapping from variable names to their covariance matrices
225    /// in tangent space. For example, for SE3 variables this would be 6×6 matrices.
226    ///
227    /// Enable covariance computation by setting `compute_covariances: true` in the optimizer config.
228    pub covariances: Option<std::collections::HashMap<String, faer::Mat<f64>>>,
229}
230
231/// Core trait for optimization solvers.
232pub trait Solver {
233    /// Configuration type for this solver
234    type Config;
235    /// Error type
236    type Error;
237
238    /// Create a new solver with the given configuration
239    fn new() -> Self;
240
241    /// Optimize the problem to minimize the cost function
242    fn optimize(
243        &mut self,
244        problem: &problem::Problem,
245        initial_params: &collections::HashMap<
246            String,
247            (manifold::ManifoldType, nalgebra::DVector<f64>),
248        >,
249    ) -> Result<SolverResult<collections::HashMap<String, problem::VariableEnum>>, Self::Error>;
250}
251
252/// Apply parameter update step to all variables.
253///
254/// This is a common operation used by all optimizers (Levenberg-Marquardt, Gauss-Newton, Dog Leg).
255/// It applies a tangent space perturbation to each variable using the proper manifold plus operation.
256///
257/// # Arguments
258/// * `variables` - Mutable map of variables to update
259/// * `step` - Full step vector in tangent space (faer matrix view)
260/// * `variable_order` - Ordered list of variable names (defines indexing into step vector)
261///
262/// # Returns
263/// * Step norm (L2 norm) for convergence checking
264///
265/// # Implementation Notes
266/// The step vector contains concatenated tangent vectors for all variables in the order
267/// specified by `variable_order`. Each variable's tangent space dimension determines
268/// how many elements it occupies in the step vector.
269///
270pub fn apply_parameter_step(
271    variables: &mut collections::HashMap<String, problem::VariableEnum>,
272    step: faer::MatRef<f64>,
273    variable_order: &[String],
274) -> f64 {
275    let mut step_offset = 0;
276
277    for var_name in variable_order {
278        if let Some(var) = variables.get_mut(var_name) {
279            let var_size = var.get_size();
280            let var_step = step.subrows(step_offset, var_size);
281
282            // Delegate to VariableEnum's apply_tangent_step method
283            // This handles all manifold types (SE2, SE3, SO2, SO3, Rn)
284            var.apply_tangent_step(var_step);
285
286            step_offset += var_size;
287        }
288    }
289
290    // Compute and return step norm for convergence checking
291    step.norm_l2()
292}
293
294/// Apply negative parameter step to rollback variables.
295///
296/// This is used when an optimization step is rejected (e.g., in trust region methods).
297/// It applies the negative of a tangent space perturbation to revert the previous update.
298///
299/// # Arguments
300/// * `variables` - Mutable map of variables to revert
301/// * `step` - Full step vector in tangent space (faer matrix view) to negate
302/// * `variable_order` - Ordered list of variable names (defines indexing into step vector)
303///
304pub fn apply_negative_parameter_step(
305    variables: &mut collections::HashMap<String, problem::VariableEnum>,
306    step: faer::MatRef<f64>,
307    variable_order: &[String],
308) {
309    use faer::Mat;
310
311    // Create a negated version of the step vector
312    let mut negative_step = Mat::zeros(step.nrows(), 1);
313    for i in 0..step.nrows() {
314        negative_step[(i, 0)] = -step[(i, 0)];
315    }
316
317    // Apply the negative step using the standard apply_parameter_step function
318    apply_parameter_step(variables, negative_step.as_ref(), variable_order);
319}
320
321pub fn compute_cost(residual: &faer::Mat<f64>) -> f64 {
322    let cost = residual.norm_l2();
323    0.5 * cost * cost
324}
apex_solver/optimizer/mod.rs

apex_solver/optimizer/
mod.rs
