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

apex_solver/optimizer/
mod.rs
