//! Dog Leg trust region optimization algorithm implementation.
//!
//! The Dog Leg method is a robust trust region algorithm for solving nonlinear least squares problems:
//!
//! ```text
//! min f(x) = ½||r(x)||² = ½Σᵢ rᵢ(x)²
//! ```
//!
//! where `r: ℝⁿ → ℝᵐ` is the residual vector function.
//!
//! # Algorithm Overview
//!
//! Powell's Dog Leg method constructs a piecewise linear path within a spherical trust region
//! of radius Δ, connecting three key points:
//!
//! 1. **Origin** (current position)
//! 2. **Cauchy Point** p_c = -α·g (optimal steepest descent step)
//! 3. **Gauss-Newton Point** h_gn (full Newton step solving J^T·J·h = -J^T·r)
//!
//! The "dog leg" path travels from the origin to the Cauchy point, then from the Cauchy point
//! toward the Gauss-Newton point, stopping at the trust region boundary.
//!
//! ## Step Selection Strategy
//!
//! Given trust region radius Δ, the algorithm selects a step h based on three cases:
//!
//! **Case 1: GN step inside trust region** (`||h_gn|| ≤ Δ`)
//! ```text
//! h = h_gn (full Gauss-Newton step)
//! ```
//!
//! **Case 2: Even Cauchy point outside trust region** (`||p_c|| ≥ Δ`)
//! ```text
//! h = (Δ / ||g||) · (-g) (scaled steepest descent to boundary)
//! ```
//!
//! **Case 3: Dog leg interpolation** (`||p_c|| < Δ < ||h_gn||`)
//! ```text
//! h(β) = p_c + β·(h_gn - p_c), where β ∈ [0,1] satisfies ||h(β)|| = Δ
//! ```
//!
//! ## Cauchy Point Computation
//!
//! The Cauchy point is the optimal step along the steepest descent direction -g:
//!
//! ```text
//! α = (g^T·g) / (g^T·H·g) where H = J^T·J
//! p_c = -α·g
//! ```
//!
//! This minimizes the quadratic model along the gradient direction.
//!
//! ## Trust Region Management
//!
//! The trust region radius Δ adapts based on the gain ratio:
//!
//! ```text
//! ρ = (actual reduction) / (predicted reduction)
//! ```
//!
//! **Good step** (`ρ > 0.75`): Increase radius `Δ ← max(Δ, 3·||h||)`
//! **Poor step** (`ρ < 0.25`): Decrease radius `Δ ← Δ/2`
//! **Moderate step**: Keep radius unchanged
//!
//! # Advanced Features (Ceres Solver Enhancements)
//!
//! This implementation includes several improvements from Google's Ceres Solver:
//!
//! ## 1. Adaptive μ Regularization
//!
//! Instead of solving `J^T·J·h = -J^T·r` directly, we solve:
//!
//! ```text
//! (J^T·J + μI)·h = -J^T·r
//! ```
//!
//! where μ adapts to handle ill-conditioned Hessians:
//! - **Increases** (×10) when linear solve fails
//! - **Decreases** (÷5) when steps are accepted
//! - **Bounded** between `min_mu` (1e-8) and `max_mu` (1.0)
//!
//! Default `initial_mu = 1e-4` provides good numerical stability.
//!
//! ## 2. Numerically Robust Beta Computation
//!
//! When computing β for dog leg interpolation, solves: `||p_c + β·v||² = Δ²`
//!
//! Uses two formulas to avoid catastrophic cancellation:
//! ```text
//! If b ≤ 0: β = (-b + √(b²-ac)) / a (standard formula)
//! If b > 0: β = -c / (b + √(b²-ac)) (alternative, avoids cancellation)
//! ```
//!
//! ## 3. Step Reuse Mechanism
//!
//! When a step is rejected, caches the Gauss-Newton step, Cauchy point, and gradient.
//! On the next iteration (with smaller Δ), reuses these cached values instead of
//! recomputing them. This avoids expensive linear solves when trust region shrinks.
//!
//! **Safety limits:**
//! - Maximum 5 consecutive reuses before forcing fresh computation
//! - Cache invalidated when steps are accepted (parameters have moved)
//!
//! ## 4. Jacobi Scaling (Diagonal Preconditioning)
//!
//! Optionally applies column scaling to J before forming J^T·J:
//! ```text
//! S_ii = 1 / (1 + ||J_i||) where J_i is column i
//! ```
//!
//! This creates an **elliptical trust region** instead of spherical, improving
//! convergence for problems with mixed parameter scales.
//!
//! # Mathematical Background
//!
//! ## Why Dog Leg Works
//!
//! The dog leg path is a cheap approximation to the optimal trust region step
//! (which would require solving a constrained optimization problem). It:
//!
//! 1. Exploits the fact that optimal steps often lie near the 2D subspace
//! spanned by the gradient and Gauss-Newton directions
//! 2. Provides global convergence guarantees (always finds descent direction)
//! 3. Achieves local quadratic convergence (like Gauss-Newton near solution)
//! 4. Requires only one linear solve per iteration (same as Gauss-Newton)
//!
//! ## Convergence Properties
//!
//! - **Global convergence**: Guaranteed descent at each iteration
//! - **Local quadratic convergence**: Reduces to Gauss-Newton near solution
//! - **Robustness**: Handles ill-conditioning via trust region + μ regularization
//! - **Efficiency**: Comparable cost to Gauss-Newton with better reliability
//!
//! # When to Use
//!
//! Dog Leg is an excellent choice when:
//! - You want explicit control over step size (via trust region radius)
//! - The problem may be ill-conditioned
//! - You need guaranteed descent at each iteration
//! - Initial guess may be poor but you want reliable convergence
//!
//! Compared to alternatives:
//! - **vs Gauss-Newton**: More robust but similar computational cost
//! - **vs Levenberg-Marquardt**: Explicit trust region vs implicit damping
//! - Both Dog Leg and LM are excellent general-purpose choices
//!
//! # Examples
//!
//! ## Basic usage
//!
//! ```no_run
//! use apex_solver::optimizer::DogLeg;
//! use apex_solver::core::problem::Problem;
//! use apex_solver::JacobianMode;
//!
//! # type TestResult = Result<(), Box<dyn std::error::Error>>;
//! # fn main() -> TestResult {
//! let mut problem = Problem::new(JacobianMode::Sparse);
//! // ... add residual blocks (factors) to problem ...
//!
//! let mut solver = DogLeg::new();
//! let result = solver.optimize(&mut problem)?;
//! # Ok(())
//! # }
//! ```
//!
//! ## Advanced configuration with Ceres enhancements
//!
//! ```no_run
//! use apex_solver::optimizer::dog_leg::{DogLegConfig, DogLeg};
//! use apex_solver::linalg::LinearSolverType;
//!
//! # fn main() {
//! let config = DogLegConfig::new()
//! .with_max_iterations(100)
//! .with_trust_region_radius(1e4) // Large initial radius
//! .with_trust_region_bounds(1e-3, 1e6) // Min/max radius
//! .with_mu_params(1e-4, 1e-8, 1.0, 10.0) // Conservative regularization
//! .with_jacobi_scaling(true) // Enable elliptical trust regions
//! .with_step_reuse(true); // Enable Ceres-style caching
//!
//! let mut solver = DogLeg::with_config(config);
//! # }
//! ```
//!
//! # References
//!
//! - Powell, M. J. D. (1970). "A Hybrid Method for Nonlinear Equations". *Numerical Methods for Nonlinear Algebraic Equations*. Gordon and Breach.
//! - Nocedal, J. & Wright, S. (2006). *Numerical Optimization* (2nd ed.). Springer. Chapter 4 (Trust Region Methods).
//! - Madsen, K., Nielsen, H. B., & Tingleff, O. (2004). *Methods for Non-Linear Least Squares Problems* (2nd ed.). Chapter 6.
//! - Conn, A. R., Gould, N. I. M., & Toint, P. L. (2000). *Trust-Region Methods*. SIAM.
//! - Ceres Solver: <http://ceres-solver.org/> - Google's C++ nonlinear least squares library
use crate::error::ErrorLogging;
use crate::{core::problem, error, linalg, optimizer};
use std::{fmt, time};
use tracing::debug;
use crate::linalg::{
CovarianceOptions, Damping, DenseCholeskySolver, DenseMode, DenseQRSolver, ExplicitDenseSchur,
ExplicitSchurVariant, ExplicitSparseSchur, ImplicitSparseSchur, JacobianMode, LinearSolver,
LinearSolverType, SchurPreconditioner, SparseCholeskySolver, SparseMode, SparseQRSolver,
StructureAware,
};
use crate::optimizer::{AssemblyBackend, IterationStats};
/// Configuration parameters for the Dog Leg trust region optimizer.
///
/// Controls trust region management, convergence criteria, adaptive regularization,
/// and Ceres Solver enhancements for the Dog Leg algorithm.
///
/// # Builder Pattern
///
/// All configuration options can be set using the builder pattern:
///
/// ```
/// use apex_solver::optimizer::dog_leg::DogLegConfig;
///
/// let config = DogLegConfig::new()
/// .with_max_iterations(100)
/// .with_trust_region_radius(1e4)
/// .with_mu_params(1e-4, 1e-8, 1.0, 10.0)
/// .with_jacobi_scaling(true)
/// .with_step_reuse(true);
/// ```
///
/// # Trust Region Behavior
///
/// The trust region radius Δ controls the maximum allowed step size:
///
/// - **Initial radius** (`trust_region_radius`): Starting value (default: 1e4)
/// - **Bounds** (`trust_region_min`, `trust_region_max`): Valid range (default: 1e-3 to 1e6)
/// - **Adaptation**: Increases for good steps, decreases for poor steps
///
/// # Adaptive μ Regularization (Ceres Enhancement)
///
/// Controls the regularization parameter in `(J^T·J + μI)·h = -J^T·r`:
///
/// - `initial_mu`: Starting value (default: 1e-4 for numerical stability)
/// - `min_mu`, `max_mu`: Bounds (default: 1e-8 to 1.0)
/// - `mu_increase_factor`: Multiplier when solve fails (default: 10.0)
///
/// # Convergence Criteria
///
/// The optimizer terminates when ANY of the following conditions is met:
///
/// - **Cost tolerance**: `|cost_k - cost_{k-1}| < cost_tolerance`
/// - **Parameter tolerance**: `||step|| < parameter_tolerance`
/// - **Gradient tolerance**: `||J^T·r|| < gradient_tolerance`
/// - **Maximum iterations**: `iteration >= max_iterations`
/// - **Timeout**: `elapsed_time >= timeout`
///
/// # See Also
///
/// - [`DogLeg`] - The solver that uses this configuration
/// - [`LevenbergMarquardtConfig`](crate::optimizer::levenberg_marquardt::LevenbergMarquardtConfig) - Alternative damping approach
/// - [`GaussNewtonConfig`](crate::optimizer::gauss_newton::GaussNewtonConfig) - Undamped variant
#[derive(Clone)]
pub struct DogLegConfig {
/// Type of linear solver for the linear systems
pub linear_solver_type: linalg::LinearSolverType,
/// Maximum number of iterations
pub max_iterations: usize,
/// Convergence tolerance for cost function
pub cost_tolerance: f64,
/// Convergence tolerance for parameter updates
pub parameter_tolerance: f64,
/// Convergence tolerance for gradient norm
pub gradient_tolerance: f64,
/// Timeout duration
pub timeout: Option<time::Duration>,
/// Initial trust region radius
pub trust_region_radius: f64,
/// Minimum trust region radius
pub trust_region_min: f64,
/// Maximum trust region radius
pub trust_region_max: f64,
/// Trust region increase factor (for good steps, rho > 0.75)
pub trust_region_increase_factor: f64,
/// Trust region decrease factor (for poor steps, rho < 0.25)
pub trust_region_decrease_factor: f64,
/// Deprecated: superseded by
/// [`min_relative_decrease`](Self::min_relative_decrease).
///
/// Both named the same thing — the ρ below which a step is rejected — and
/// only one can be the gate. `min_relative_decrease` won because it carries
/// Ceres' name and semantics and exists on the Levenberg-Marquardt config
/// too, so the acceptance rule is the same across optimizers.
#[deprecated(
since = "1.6.0",
note = "never read; the acceptance threshold is `min_relative_decrease`"
)]
pub min_step_quality: f64,
/// Good step quality threshold (typically 0.75)
pub good_step_quality: f64,
/// Poor step quality threshold (typically 0.25)
pub poor_step_quality: f64,
/// Use Jacobi column scaling (preconditioning)
pub use_jacobi_scaling: bool,
// Ceres-style adaptive mu regularization parameters
/// Initial mu regularization parameter for Gauss-Newton step
pub initial_mu: f64,
/// Minimum mu regularization parameter
pub min_mu: f64,
/// Maximum mu regularization parameter
pub max_mu: f64,
/// Factor to increase mu when linear solver fails
pub mu_increase_factor: f64,
// Ceres-style step reuse optimization
/// Enable step reuse after rejection (Ceres-style efficiency optimization)
pub enable_step_reuse: bool,
/// Minimum objective function cutoff (optional early termination)
///
/// If set, optimization terminates when cost falls below this threshold.
/// Useful for early stopping when a "good enough" solution is acceptable.
///
/// Default: None (disabled)
pub min_cost_threshold: Option<f64>,
/// Maximum condition number for Jacobian matrix (optional check)
///
/// If set, the optimizer checks if condition_number(J^T*J) exceeds this
/// threshold and terminates with IllConditionedJacobian status.
/// Note: Computing condition number is expensive, so this is disabled by default.
///
/// Default: None (disabled)
pub max_condition_number: Option<f64>,
/// Minimum relative cost decrease for step acceptance
///
/// Used in computing step quality (rho = actual_reduction / predicted_reduction).
/// Steps with rho < min_relative_decrease are rejected. Matches Ceres Solver's
/// min_relative_decrease parameter.
///
/// Default: 1e-3 (Ceres-compatible)
pub min_relative_decrease: f64,
/// Compute per-variable covariance matrices (uncertainty estimation)
///
/// When enabled, computes covariance by re-linearizing the problem at the
/// solution and inverting the Gauss-Newton Hessian `H = JᵀJ` after
/// convergence. The full covariance matrix is extracted into per-variable
/// blocks stored in both Variable structs and optimizer::SolverResult.
///
/// Scaled (σ̂²·H⁻¹) versus unscaled (H⁻¹) covariance, the factorization
/// algorithm, and the pseudo-inverse cutoff are configured through
/// [`covariance_options`](Self::covariance_options).
///
/// Default: false (to avoid performance overhead)
pub compute_covariances: bool,
/// Options for covariance estimation when `compute_covariances` is enabled.
///
/// Defaults to unscaled `H⁻¹` via sparse Cholesky — the correct choice when
/// residuals are whitened by their measurement information matrix. Set
/// `apply_variance_scaling` for unweighted least squares, where the noise
/// scale `σ̂² = 2·cost/(m−n)` must be estimated from the fit. See
/// [`CovarianceOptions`](crate::linalg::covariance::CovarianceOptions).
pub covariance_options: CovarianceOptions,
/// [`ExplicitSparseSchur`] sub-variant (for
/// [`LinearSolverType::ExplicitSparseSchur`]).
///
/// - [`ExplicitSchurVariant::Sparse`]: form S, sparse Cholesky. Most
/// accurate; memory is O(kept_dof²).
/// - [`ExplicitSchurVariant::Chunked`]: form S straight from `J`, never
/// materializing `JᵀJ`. Same algebra, far less memory on large problems.
/// - [`ExplicitSchurVariant::Iterative`]: form S, then PCG instead of
/// Cholesky.
///
/// To never form `S` at all, use
/// [`LinearSolverType::ImplicitSparseSchur`] instead — a different
/// solver, not a variant of this one.
///
/// The Cauchy point and predicted reduction use the solver's
/// `hessian_vec_product`, which every variant serves (the chunked path
/// evaluates `Jᵀ(J·v)` from the retained Jacobian).
///
/// Default: `Sparse`
pub schur_variant: ExplicitSchurVariant,
/// Preconditioner for the PCG used by the iterative Schur variants.
///
/// - [`SchurPreconditioner::None`]: unconditioned CG.
/// - [`SchurPreconditioner::BlockDiagonal`]: block diagonal of `H_kk`.
/// - [`SchurPreconditioner::SchurJacobi`]: block diagonal of `S` itself —
/// Ceres's `SCHUR_JACOBI`, and usually the best convergence.
///
/// Ignored by the direct variants, which factorize instead of iterating.
///
/// Default: `SchurJacobi`
pub schur_preconditioner: SchurPreconditioner,
/// Maximum PCG iterations per linear solve, for the iterative variants.
///
/// Default: 200 (Ceres's default)
pub schur_cg_max_iterations: usize,
/// Relative residual tolerance ending a PCG solve, for the iterative
/// variants.
///
/// Default: 1e-6
pub schur_cg_tolerance: f64,
/// Forcing-sequence parameter η for the Schur PCG paths (Ceres's
/// `Solver::Options::eta`).
///
/// PCG stops once the quadratic model's relative improvement per iteration
/// falls below `η/i`, which is what makes early Newton steps cheap. `0.0`
/// disables the rule, leaving only `schur_cg_tolerance` and the iteration
/// cap — ask for that when you want an exact linear solve.
///
/// Default: 0.1
pub schur_cg_q_tolerance: f64,
/// Deprecated: never read. Visualization goes through the observer pattern.
///
/// Setting this has no effect. Attach an observer to the solver instead:
/// `solver.add_observer(RerunObserver::new(true)?)`, which is how
/// Levenberg-Marquardt has always done it and works for every optimizer.
///
/// Default: false
#[cfg(feature = "visualization")]
#[deprecated(
since = "1.6.0",
note = "never read; attach a `RerunObserver` with `add_observer` instead"
)]
pub enable_visualization: bool,
}
impl Default for DogLegConfig {
#[allow(deprecated)]
fn default() -> Self {
Self {
linear_solver_type: linalg::LinearSolverType::default(),
// Ceres Solver default: 50 (changed from 100 for compatibility)
max_iterations: 50,
// Ceres Solver default: 1e-6 (changed from 1e-8 for compatibility)
cost_tolerance: 1e-6,
// Ceres Solver default: 1e-8 (unchanged)
parameter_tolerance: 1e-8,
// Ceres Solver default: 1e-10 (changed from 1e-8 for compatibility)
gradient_tolerance: 1e-10,
timeout: None,
// Ceres-style: larger initial radius for better global convergence
trust_region_radius: 1e4,
trust_region_min: 1e-12,
trust_region_max: 1e12,
// Ceres uses adaptive increase (max(radius, 3*step_norm)),
// but we keep factor for simpler config
trust_region_increase_factor: 3.0,
trust_region_decrease_factor: 0.5,
min_step_quality: 0.0,
good_step_quality: 0.75,
poor_step_quality: 0.25,
// Ceres-style: Enable diagonal scaling by default for elliptical trust region
use_jacobi_scaling: true,
// Ceres-style adaptive mu regularization defaults
// Start with more conservative regularization to avoid singular Hessian
initial_mu: 1e-4,
min_mu: 1e-8,
max_mu: 1.0,
mu_increase_factor: 10.0,
// Ceres-style step reuse optimization
enable_step_reuse: true,
// New Ceres-compatible termination parameters
min_cost_threshold: None,
max_condition_number: None,
min_relative_decrease: 1e-3,
compute_covariances: false,
covariance_options: CovarianceOptions::default(),
schur_variant: ExplicitSchurVariant::default(),
schur_preconditioner: SchurPreconditioner::default(),
schur_cg_max_iterations: 200,
schur_cg_tolerance: 1e-6,
schur_cg_q_tolerance: crate::linalg::schur::DEFAULT_ETA,
#[cfg(feature = "visualization")]
#[allow(deprecated)]
enable_visualization: false,
}
}
}
impl DogLegConfig {
/// Create a new Dog Leg configuration with default values.
pub fn new() -> Self {
Self::default()
}
/// Set the linear solver type
pub fn with_linear_solver_type(mut self, linear_solver_type: linalg::LinearSolverType) -> Self {
self.linear_solver_type = linear_solver_type;
self
}
/// Set the maximum number of iterations
pub fn with_max_iterations(mut self, max_iterations: usize) -> Self {
self.max_iterations = max_iterations;
self
}
/// Set the cost tolerance
pub fn with_cost_tolerance(mut self, cost_tolerance: f64) -> Self {
self.cost_tolerance = cost_tolerance;
self
}
/// Set the parameter tolerance
pub fn with_parameter_tolerance(mut self, parameter_tolerance: f64) -> Self {
self.parameter_tolerance = parameter_tolerance;
self
}
/// Set the gradient tolerance
pub fn with_gradient_tolerance(mut self, gradient_tolerance: f64) -> Self {
self.gradient_tolerance = gradient_tolerance;
self
}
/// Set the timeout duration
pub fn with_timeout(mut self, timeout: time::Duration) -> Self {
self.timeout = Some(timeout);
self
}
/// Set the initial trust region radius
pub fn with_trust_region_radius(mut self, radius: f64) -> Self {
self.trust_region_radius = radius;
self
}
/// Set the trust region radius bounds
pub fn with_trust_region_bounds(mut self, min: f64, max: f64) -> Self {
self.trust_region_min = min;
self.trust_region_max = max;
self
}
/// Set the trust region adjustment factors
pub fn with_trust_region_factors(mut self, increase: f64, decrease: f64) -> Self {
self.trust_region_increase_factor = increase;
self.trust_region_decrease_factor = decrease;
self
}
/// Set the trust region quality thresholds.
///
/// `min_quality` is accepted for source compatibility but has no effect —
/// see [`DogLegConfig::min_step_quality`]. Use
/// [`with_min_relative_decrease`](Self::with_min_relative_decrease) to move
/// the acceptance threshold.
#[allow(deprecated)]
pub fn with_step_quality_thresholds(
mut self,
min_quality: f64,
poor_quality: f64,
good_quality: f64,
) -> Self {
self.min_step_quality = min_quality;
self.poor_step_quality = poor_quality;
self.good_step_quality = good_quality;
self
}
/// Enable or disable Jacobi column scaling (preconditioning)
pub fn with_jacobi_scaling(mut self, use_jacobi_scaling: bool) -> Self {
self.use_jacobi_scaling = use_jacobi_scaling;
self
}
/// Set adaptive mu regularization parameters (Ceres-style)
pub fn with_mu_params(
mut self,
initial_mu: f64,
min_mu: f64,
max_mu: f64,
increase_factor: f64,
) -> Self {
self.initial_mu = initial_mu;
self.min_mu = min_mu;
self.max_mu = max_mu;
self.mu_increase_factor = increase_factor;
self
}
/// Enable or disable step reuse optimization (Ceres-style)
pub fn with_step_reuse(mut self, enable_step_reuse: bool) -> Self {
self.enable_step_reuse = enable_step_reuse;
self
}
/// Set minimum objective function cutoff for early termination.
///
/// When set, optimization terminates with MinCostThresholdReached status
/// if the cost falls below this threshold. Useful for early stopping when
/// a "good enough" solution is acceptable.
pub fn with_min_cost_threshold(mut self, min_cost: f64) -> Self {
self.min_cost_threshold = Some(min_cost);
self
}
/// Set maximum condition number for Jacobian matrix.
///
/// If set, the optimizer checks if condition_number(J^T*J) exceeds this
/// threshold and terminates with IllConditionedJacobian status.
/// Note: Computing condition number is expensive, disabled by default.
pub fn with_max_condition_number(mut self, max_cond: f64) -> Self {
self.max_condition_number = Some(max_cond);
self
}
/// Set minimum relative cost decrease for step acceptance.
///
/// Steps with rho = (actual_reduction / predicted_reduction) below this
/// threshold are rejected. Default: 1e-3 (Ceres-compatible)
pub fn with_min_relative_decrease(mut self, min_decrease: f64) -> Self {
self.min_relative_decrease = min_decrease;
self
}
/// Enable or disable covariance computation (uncertainty estimation).
///
/// When enabled, computes the full covariance matrix by inverting the Hessian
/// after convergence, then extracts per-variable covariance blocks.
pub fn with_compute_covariances(mut self, compute_covariances: bool) -> Self {
self.compute_covariances = compute_covariances;
self
}
/// Set the options used for covariance estimation.
///
/// Controls scaled (`σ̂²·H⁻¹`) versus unscaled (`H⁻¹`) covariance, the
/// factorization algorithm (sparse Cholesky or dense SVD pseudo-inverse),
/// and the singular-value cutoff. Only takes effect when
/// `compute_covariances` is enabled.
pub fn with_covariance_options(mut self, options: CovarianceOptions) -> Self {
self.covariance_options = options;
self
}
/// Set the [`ExplicitSparseSchur`] sub-variant.
///
/// Takes effect when `linear_solver_type` is
/// [`LinearSolverType::ExplicitSparseSchur`].
pub fn with_schur_variant(mut self, variant: ExplicitSchurVariant) -> Self {
self.schur_variant = variant;
self
}
/// Set Schur complement preconditioner for the iterative variants
pub fn with_schur_preconditioner(mut self, preconditioner: SchurPreconditioner) -> Self {
self.schur_preconditioner = preconditioner;
self
}
/// PCG budget for the iterative Schur variants.
pub fn with_schur_cg_params(mut self, max_iterations: usize, tolerance: f64) -> Self {
self.schur_cg_max_iterations = max_iterations;
self.schur_cg_tolerance = tolerance;
self
}
/// Set the PCG forcing-sequence parameter η. `0.0` disables the
/// quadratic-model stopping rule; see [`Self::schur_cg_q_tolerance`].
pub fn with_schur_cg_q_tolerance(mut self, q_tolerance: f64) -> Self {
self.schur_cg_q_tolerance = q_tolerance;
self
}
/// Deprecated: no-op. See [`enable_visualization`](Self::enable_visualization).
#[cfg(feature = "visualization")]
#[deprecated(
since = "1.6.0",
note = "no-op; attach a `RerunObserver` with `add_observer` instead"
)]
#[allow(deprecated)]
pub fn with_visualization(mut self, enable: bool) -> Self {
self.enable_visualization = enable;
self
}
/// Print configuration parameters (info level logging)
#[allow(deprecated)]
pub fn print_configuration(&self) {
debug!(
"Configuration:\n Solver: Dog-Leg\n Linear solver: {:?}\n Schur variant: {:?}\n Schur preconditioner: {:?}\n Schur CG budget: {} iters @ {:.2e}\n Loss function: N/A\n\nConvergence Criteria:\n Max iterations: {}\n Cost tolerance: {:.2e}\n Parameter tolerance: {:.2e}\n Gradient tolerance: {:.2e}\n Timeout: {:?}\n\nTrust Region:\n Initial radius: {:.2e}\n Radius range: [{:.2e}, {:.2e}]\n Min step quality: {:.2}\n Good step quality: {:.2}\n Poor step quality: {:.2}\n\nRegularization:\n Initial mu: {:.2e}\n Mu range: [{:.2e}, {:.2e}]\n Mu increase factor: {:.2}\n\nNumerical Settings:\n Jacobi scaling: {}\n Step reuse: {}\n Compute covariances: {}",
self.linear_solver_type,
self.schur_variant,
self.schur_preconditioner,
self.schur_cg_max_iterations,
self.schur_cg_tolerance,
self.max_iterations,
self.cost_tolerance,
self.parameter_tolerance,
self.gradient_tolerance,
self.timeout,
self.trust_region_radius,
self.trust_region_min,
self.trust_region_max,
self.min_step_quality,
self.good_step_quality,
self.poor_step_quality,
self.initial_mu,
self.min_mu,
self.max_mu,
self.mu_increase_factor,
if self.use_jacobi_scaling {
"enabled"
} else {
"disabled"
},
if self.enable_step_reuse {
"enabled"
} else {
"disabled"
},
if self.compute_covariances {
"enabled"
} else {
"disabled"
}
);
}
}
/// Result from step computation
struct StepResult {
step: faer::Mat<f64>,
gradient_norm: f64,
predicted_reduction: f64,
/// ‖step‖ measured in the space the trust-region radius bounds.
///
/// Equal to `‖step‖` when Jacobi scaling is off; when it is on, the radius
/// constrains the scaled step, so the radius update has to be fed this norm
/// and not the un-scaled one.
scaled_step_norm: f64,
}
/// Type of step taken
#[derive(Debug, Clone, Copy)]
enum StepType {
/// Full Gauss-Newton step
GaussNewton,
/// Scaled steepest descent (Cauchy point)
SteepestDescent,
/// Dog leg interpolation
DogLeg,
}
impl fmt::Display for StepType {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
StepType::GaussNewton => write!(f, "GN"),
StepType::SteepestDescent => write!(f, "SD"),
StepType::DogLeg => write!(f, "DL"),
}
}
}
/// Result from step evaluation
struct StepEvaluation {
accepted: bool,
cost_reduction: f64,
rho: f64,
}
/// Dog Leg trust region solver for nonlinear least squares optimization.
///
/// Implements Powell's Dog Leg algorithm with Ceres Solver enhancements including
/// adaptive μ regularization, numerically robust beta computation, and step reuse caching.
///
/// # Algorithm
///
/// At each iteration k:
/// 1. Compute residual `r(xₖ)` and Jacobian `J(xₖ)`
/// 2. Solve for Gauss-Newton step: `(J^T·J + μI)·h_gn = -J^T·r`
/// 3. Compute steepest descent direction: `-g` where `g = J^T·r`
/// 4. Compute Cauchy point: `p_c = -α·g` (optimal along steepest descent)
/// 5. Construct dog leg step based on trust region radius Δ:
/// - If `||h_gn|| ≤ Δ`: Take full GN step
/// - Else if `||p_c|| ≥ Δ`: Take scaled SD to boundary
/// - Else: Interpolate `h = p_c + β·(h_gn - p_c)` where `||h|| = Δ`
/// 6. Evaluate gain ratio: `ρ = (actual reduction) / (predicted reduction)`
/// 7. Update trust region radius based on ρ
/// 8. Accept/reject step and update parameters
///
/// # Ceres Solver Enhancements
///
/// This implementation includes four major improvements from Google's Ceres Solver:
///
/// **1. Adaptive μ Regularization:** Dynamically adjusts regularization parameter
/// to handle ill-conditioned Hessians (increases on failure, decreases on success).
///
/// **2. Numerically Robust Beta:** Uses two formulas for computing dog leg
/// interpolation parameter β to avoid catastrophic cancellation.
///
/// **3. Step Reuse Mechanism:** Caches GN step, Cauchy point, and gradient when
/// steps are rejected. Limited to 5 consecutive reuses to prevent staleness.
///
/// **4. Jacobi Scaling:** Optional diagonal preconditioning creates elliptical
/// trust regions for better handling of mixed-scale problems.
///
/// # Examples
///
/// ```no_run
/// use apex_solver::optimizer::DogLeg;
/// use apex_solver::core::problem::Problem;
/// use apex_solver::JacobianMode;
///
/// # type TestResult = Result<(), Box<dyn std::error::Error>>;
/// # fn main() -> TestResult {
/// let mut problem = Problem::new(JacobianMode::Sparse);
/// // ... add factors to problem ...
///
/// let mut solver = DogLeg::new();
/// let result = solver.optimize(&mut problem)?;
/// # Ok(())
/// # }
/// ```
///
/// # See Also
///
/// - [`DogLegConfig`] - Configuration options
/// - [`LevenbergMarquardt`](crate::optimizer::LevenbergMarquardt) - Alternative adaptive damping
/// - [`GaussNewton`](crate::optimizer::GaussNewton) - Undamped variant
pub struct DogLeg {
config: DogLegConfig,
jacobi_scaling: Option<Vec<f64>>,
observers: optimizer::OptObserverVec,
// Adaptive mu regularization (Ceres-style)
mu: f64,
min_mu: f64,
max_mu: f64,
mu_increase_factor: f64,
/// Run state: the live trust-region radius, seeded from
/// `config.trust_region_radius` at the start of every solve.
///
/// The radius is iteration state, not configuration. Keeping it here rather
/// than mutating `config` in place means a second `optimize()` call on the
/// same solver starts from the configured radius instead of inheriting
/// whatever the previous run collapsed to.
trust_region_radius: f64,
// Step reuse mechanism (Ceres-style efficiency optimization)
reuse_step_on_rejection: bool,
cached_gn_step: Option<faer::Mat<f64>>,
cached_cauchy_point: Option<faer::Mat<f64>>,
cached_gradient: Option<faer::Mat<f64>>,
cached_alpha: Option<f64>,
cache_reuse_count: usize, // Track consecutive reuses to prevent staleness
}
impl Default for DogLeg {
fn default() -> Self {
Self::new()
}
}
impl DogLeg {
/// Create a new Dog Leg solver with default configuration.
pub fn new() -> Self {
Self::with_config(DogLegConfig::default())
}
/// Create a new Dog Leg solver with the given configuration.
pub fn with_config(config: DogLegConfig) -> Self {
Self {
// Initialize adaptive mu from config
mu: config.initial_mu,
min_mu: config.min_mu,
max_mu: config.max_mu,
mu_increase_factor: config.mu_increase_factor,
trust_region_radius: config.trust_region_radius,
// Initialize step reuse mechanism (disabled initially, enabled after first rejection)
reuse_step_on_rejection: false,
cached_gn_step: None,
cached_cauchy_point: None,
cached_gradient: None,
cached_alpha: None,
cache_reuse_count: 0,
config,
jacobi_scaling: None,
observers: optimizer::OptObserverVec::new(),
}
}
/// Add an observer to the solver.
///
/// Observers are notified at each iteration with the current variable values.
/// This enables real-time visualization, logging, metrics collection, etc.
///
/// # Examples
///
/// ```no_run
/// use apex_solver::optimizer::DogLeg;
/// # use apex_solver::optimizer::OptObserver;
/// # use apex_solver::core::VarKey;
/// # use apex_solver::core::variable::ManifoldVariable;
/// # use slotmap::SlotMap;
///
/// # struct MyObserver;
/// # impl OptObserver for MyObserver {
/// # fn on_step(&self, _: &SlotMap<VarKey, Box<dyn ManifoldVariable>>, _: usize) {}
/// # }
/// let mut solver = DogLeg::new();
/// solver.add_observer(MyObserver);
/// ```
pub fn add_observer(&mut self, observer: impl optimizer::OptObserver + 'static) {
self.observers.add(observer);
}
/// Compute Cauchy point and optimal step length for steepest descent (generic).
///
/// Returns (alpha, cauchy_point) where:
/// - alpha: optimal step length α = ||g||² / (g^T H g)
/// - cauchy_point: p_c = -α * g (the Cauchy point)
///
/// `h_g` is `H·g`; only the Hessian's action is needed here, never the
/// matrix itself.
fn compute_cauchy_point_and_alpha_generic(
&self,
gradient: &faer::Mat<f64>,
h_g: &faer::Mat<f64>,
) -> (f64, faer::Mat<f64>) {
// Optimal step size along steepest descent: α = (g^T*g) / (g^T*H*g)
let g_norm_sq_mat = gradient.transpose() * gradient;
let g_norm_sq = g_norm_sq_mat[(0, 0)];
let g_h_g_mat = gradient.transpose() * h_g;
let g_h_g = g_h_g_mat[(0, 0)];
// Avoid division by zero
let alpha = if g_h_g.abs() > 1e-15 {
g_norm_sq / g_h_g
} else {
1.0
};
// Compute Cauchy point: p_c = -α * gradient
let mut cauchy_point = faer::Mat::zeros(gradient.nrows(), 1);
for i in 0..gradient.nrows() {
cauchy_point[(i, 0)] = -alpha * gradient[(i, 0)];
}
(alpha, cauchy_point)
}
/// Compute the dog leg step using Powell's Dog Leg method
///
/// The dog leg path consists of two segments:
/// 1. From origin to Cauchy point (optimal along steepest descent)
/// 2. From Cauchy point to Gauss-Newton step
///
/// Arguments:
/// - steepest_descent_dir: -gradient (steepest descent direction, not scaled)
/// - cauchy_point: p_c = α * (-gradient), the optimal steepest descent step
/// - h_gn: Gauss-Newton step
/// - delta: Trust region radius
///
/// Returns: (step, step_type)
fn compute_dog_leg_step(
&self,
steepest_descent_dir: &faer::Mat<f64>,
cauchy_point: &faer::Mat<f64>,
h_gn: &faer::Mat<f64>,
delta: f64,
) -> (faer::Mat<f64>, StepType) {
let gn_norm = h_gn.norm_l2();
let cauchy_norm = cauchy_point.norm_l2();
let sd_norm = steepest_descent_dir.norm_l2();
// Case 1: Full Gauss-Newton step fits in trust region
if gn_norm <= delta {
return (h_gn.clone(), StepType::GaussNewton);
}
// Case 2: Even Cauchy point is outside trust region
// Scale steepest descent direction to boundary: (delta / ||δ_sd||) * δ_sd
if cauchy_norm >= delta {
let mut scaled_sd = faer::Mat::zeros(steepest_descent_dir.nrows(), 1);
let scale = delta / sd_norm;
for i in 0..steepest_descent_dir.nrows() {
scaled_sd[(i, 0)] = steepest_descent_dir[(i, 0)] * scale;
}
return (scaled_sd, StepType::SteepestDescent);
}
// Case 3: Dog leg interpolation between Cauchy point and GN step
// Use Ceres-style numerically robust formula
//
// Following Ceres solver implementation for numerical stability:
// Compute intersection of trust region boundary with line from Cauchy point to GN step
//
// Let v = δ_gn - p_c
// Solve: ||p_c + β*v||² = delta²
// This gives: a*β² + 2*b*β + c = 0
// where:
// a = v^T·v = ||v||²
// b = p_c^T·v
// c = p_c^T·p_c - delta² = ||p_c||² - delta²
let mut v = faer::Mat::zeros(cauchy_point.nrows(), 1);
for i in 0..cauchy_point.nrows() {
v[(i, 0)] = h_gn[(i, 0)] - cauchy_point[(i, 0)];
}
// Compute coefficients
let v_squared_norm = v.transpose() * &v;
let a = v_squared_norm[(0, 0)];
let pc_dot_v = cauchy_point.transpose() * &v;
let b = pc_dot_v[(0, 0)];
let c = cauchy_norm * cauchy_norm - delta * delta;
// Ceres-style numerically robust beta computation
// Uses two different formulas based on sign of b to avoid catastrophic cancellation
let d_squared = b * b - a * c;
let beta = if d_squared < 0.0 {
// Should not happen geometrically, but handle gracefully
1.0
} else if a.abs() < 1e-15 {
// Degenerate case: v is nearly zero
1.0
} else {
let d = d_squared.sqrt();
// Ceres formula: choose formula based on sign of b to avoid cancellation
// If b <= 0: beta = (-b + d) / a (standard formula, no cancellation)
// If b > 0: beta = -c / (b + d) (alternative formula, avoids cancellation)
if b <= 0.0 { (-b + d) / a } else { -c / (b + d) }
};
// Clamp beta to [0, 1] for safety
let beta = beta.clamp(0.0, 1.0);
// Compute dog leg step: p_dl = p_c + β*(δ_gn - p_c)
let mut dog_leg = faer::Mat::zeros(cauchy_point.nrows(), 1);
for i in 0..cauchy_point.nrows() {
dog_leg[(i, 0)] = cauchy_point[(i, 0)] + beta * v[(i, 0)];
}
(dog_leg, StepType::DogLeg)
}
/// Update the trust-region radius from the step quality (Ceres-style).
///
/// `scaled_step_norm` must be measured in the same space the radius bounds —
/// the *scaled* space when Jacobi scaling is on, which is Dog Leg's default.
/// Feeding the un-scaled norm here would compare two different units and
/// grow the radius by an arbitrary factor.
fn update_trust_region(&mut self, rho: f64, scaled_step_norm: f64) -> bool {
if rho > self.config.good_step_quality {
// Good step: grow the region to at least `increase_factor` times the
// step that just succeeded (Ceres' `max(radius, 3·‖step‖)`).
let new_radius = self
.trust_region_radius
.max(self.config.trust_region_increase_factor * scaled_step_norm);
self.trust_region_radius = new_radius.min(self.config.trust_region_max);
// Decrease mu on successful step (Ceres-style adaptive regularization)
self.mu = (self.mu / (0.5 * self.mu_increase_factor)).max(self.min_mu);
// Clear reuse flag and invalidate cache on acceptance (parameters have moved)
self.reuse_step_on_rejection = false;
self.cached_gn_step = None;
self.cached_cauchy_point = None;
self.cached_gradient = None;
self.cached_alpha = None;
self.cache_reuse_count = 0;
true
} else if rho < self.config.poor_step_quality {
// Poor step, decrease trust region
self.trust_region_radius = (self.trust_region_radius
* self.config.trust_region_decrease_factor)
.max(self.config.trust_region_min);
// Enable step reuse flag for next iteration (Ceres-style)
self.reuse_step_on_rejection = self.config.enable_step_reuse;
false
} else {
// Moderate step, keep trust region unchanged
// Clear reuse flag and invalidate cache on acceptance (parameters have moved)
self.reuse_step_on_rejection = false;
self.cached_gn_step = None;
self.cached_cauchy_point = None;
self.cached_gradient = None;
self.cached_alpha = None;
self.cache_reuse_count = 0;
true
}
}
/// Compute dog leg optimization step (generic over assembly mode).
fn compute_optimization_step_generic<M: AssemblyBackend>(
&mut self,
residuals: &faer::Mat<f64>,
scaled_jacobian: &M::Jacobian,
linear_solver: &mut dyn LinearSolver<M>,
) -> Option<StepResult> {
// Check if we can reuse cached step (Ceres-style optimization)
// Safety limit: prevent excessive reuse that could lead to stale gradient/Hessian
const MAX_CACHE_REUSE: usize = 5;
if self.reuse_step_on_rejection
&& self.config.enable_step_reuse
&& self.cache_reuse_count < MAX_CACHE_REUSE
&& let (Some(cached_gn), Some(cached_cauchy), Some(cached_grad), Some(_cached_a)) = (
&self.cached_gn_step,
&self.cached_cauchy_point,
&self.cached_gradient,
&self.cached_alpha,
)
{
// Increment reuse counter
self.cache_reuse_count += 1;
// `cached_grad` is the (possibly Jacobi-scaled) gradient the
// solver cached; report `gradient_tolerance`'s convergence check
// against the un-scaled one (GitHub #57).
let gradient_norm = if self.config.use_jacobi_scaling {
optimizer::unscaled_gradient_norm(cached_grad, self.jacobi_scaling.as_ref()?)
} else {
cached_grad.norm_l2()
};
let mut steepest_descent = faer::Mat::zeros(cached_grad.nrows(), 1);
for i in 0..cached_grad.nrows() {
steepest_descent[(i, 0)] = -cached_grad[(i, 0)];
}
let (scaled_step, _step_type) = self.compute_dog_leg_step(
&steepest_descent,
cached_cauchy,
cached_gn,
self.trust_region_radius,
);
let scaled_step_norm = scaled_step.norm_l2();
let step = if self.config.use_jacobi_scaling {
let scaling = self.jacobi_scaling.as_ref()?;
M::apply_inverse_scaling(&scaled_step, scaling)
} else {
scaled_step.clone()
};
// For cached reuse, we need the hessian for predicted reduction.
// Use hessian from linear solver if available.
let hessian_step = linear_solver.hessian_vec_product(&scaled_step)?;
let predicted_reduction =
optimizer::compute_predicted_reduction(&scaled_step, cached_grad, &hessian_step);
return Some(StepResult {
step,
gradient_norm,
predicted_reduction,
scaled_step_norm,
});
}
// Not reusing, compute fresh step
// 1. Solve for Gauss-Newton step with adaptive mu regularization (Ceres-style)
let mut scaled_gn_step = None;
let mut mu_attempts = 0;
// Try to solve with current mu, increasing if necessary
while mu_attempts < 10 && self.mu <= self.max_mu {
// μ is a numerical stabiliser for the Gauss-Newton solve, not a
// trust region — the region is enforced geometrically by the dog-leg
// construction — so it is applied as uniform λI rather than the
// Marquardt diagonal.
let damping = Damping::identity(self.mu);
if let Ok(step) =
linear_solver.solve_augmented_equation(residuals, scaled_jacobian, &damping)
{
scaled_gn_step = Some(step);
break;
}
// Increase mu (Ceres-style)
self.mu = (self.mu * self.mu_increase_factor).min(self.max_mu);
mu_attempts += 1;
}
let scaled_gn_step = scaled_gn_step?;
// 2. Get gradient and Hessian (cached by solve_augmented_equation)
let gradient = linear_solver.get_gradient()?;
// `gradient` is the (possibly Jacobi-scaled) gradient against
// `scaled_jacobian`; report `gradient_tolerance`'s convergence check
// against the un-scaled one (GitHub #57). Every other use of
// `gradient` below (steepest descent, Cauchy point, predicted
// reduction) deliberately stays in the scaled space it was computed
// in — only this reported norm changes.
let gradient_norm = if self.config.use_jacobi_scaling {
optimizer::unscaled_gradient_norm(gradient, self.jacobi_scaling.as_ref()?)
} else {
gradient.norm_l2()
};
// 3. Compute steepest descent direction: δ_sd = -gradient
let mut steepest_descent = faer::Mat::zeros(gradient.nrows(), 1);
for i in 0..gradient.nrows() {
steepest_descent[(i, 0)] = -gradient[(i, 0)];
}
// 4. Compute Cauchy point and optimal step length
let h_g = linear_solver.hessian_vec_product(gradient)?;
let (alpha, cauchy_point) = self.compute_cauchy_point_and_alpha_generic(gradient, &h_g);
// 5. Compute dog leg step based on trust region radius
let (scaled_step, _step_type) = self.compute_dog_leg_step(
&steepest_descent,
&cauchy_point,
&scaled_gn_step,
self.trust_region_radius,
);
// 6. Apply inverse Jacobi scaling if enabled
let scaled_step_norm = scaled_step.norm_l2();
let step = if self.config.use_jacobi_scaling {
let scaling = self.jacobi_scaling.as_ref()?;
M::apply_inverse_scaling(&scaled_step, scaling)
} else {
scaled_step.clone()
};
// 7. Compute predicted reduction
let hessian_step = linear_solver.hessian_vec_product(&scaled_step)?;
let predicted_reduction =
optimizer::compute_predicted_reduction(&scaled_step, gradient, &hessian_step);
// 8. Cache step components for potential reuse (Ceres-style)
self.cached_gn_step = Some(scaled_gn_step.clone());
self.cached_cauchy_point = Some(cauchy_point.clone());
self.cached_gradient = Some(gradient.clone());
self.cached_alpha = Some(alpha);
Some(StepResult {
step,
gradient_norm,
predicted_reduction,
scaled_step_norm,
})
}
/// Evaluate and apply step
fn evaluate_and_apply_step(
&mut self,
step_result: &StepResult,
state: &mut optimizer::InitializedState,
problem: &problem::Problem,
) -> error::ApexSolverResult<StepEvaluation> {
// Apply parameter updates
let _step_norm = optimizer::apply_parameter_step(
&mut state.variables,
step_result.step.as_ref(),
&state.sorted_vars,
);
// Compute new cost (residual only, no Jacobian needed for step evaluation).
// Reuses the solve workspace instead of allocating one per iteration,
// like Levenberg-Marquardt does.
let (_new_residual, new_cost) = problem.compute_residual_and_cost_sparse_with_workspace(
&state.variables,
&mut state.workspace,
)?;
// Compute step quality
let rho = optimizer::compute_step_quality(
state.current_cost,
new_cost,
step_result.predicted_reduction,
);
// Accept on step quality, then resize the trust region. The two are
// separate decisions: `min_relative_decrease` says whether the step was
// worth keeping, `update_trust_region` says how far to trust the model
// next time.
let accepted = rho > self.config.min_relative_decrease;
self.update_trust_region(rho, step_result.scaled_step_norm);
let cost_reduction = if accepted {
let reduction = state.current_cost - new_cost;
state.current_cost = new_cost;
reduction
} else {
// Reject step - revert changes
optimizer::apply_negative_parameter_step(
&mut state.variables,
step_result.step.as_ref(),
&state.sorted_vars,
);
0.0
};
Ok(StepEvaluation {
accepted,
cost_reduction,
rho,
})
}
/// Run optimization using the specified assembly mode and linear solver.
///
/// `state` is built once by [`Self::optimize`] and handed over here —
/// see `LevenbergMarquardt::optimize_with_mode` for why: the Schur path
/// needs the variable index map to size its block structure, and rebuilding
/// it here would repeat the symbolic construction and cost evaluation.
fn optimize_with_mode<M: AssemblyBackend>(
&mut self,
problem: &mut problem::Problem,
linear_solver: &mut dyn LinearSolver<M>,
mut state: optimizer::InitializedState,
) -> optimizer::OptimizeResult {
let start_time = time::Instant::now();
let mut iteration = 0;
let mut cost_evaluations = 1;
let mut jacobian_evaluations = 0;
let mut successful_steps = 0;
let mut unsuccessful_steps = 0;
// Seed the run state from the configuration. The radius, μ and the step
// cache all evolve during the solve; resetting them here keeps
// `optimize()` idempotent when the same solver instance is reused.
self.trust_region_radius = self.config.trust_region_radius;
self.mu = self.config.initial_mu;
self.reuse_step_on_rejection = false;
self.cached_gn_step = None;
self.cached_cauchy_point = None;
self.cached_gradient = None;
self.cached_alpha = None;
self.cache_reuse_count = 0;
self.jacobi_scaling = None;
let mut max_gradient_norm: f64 = 0.0;
let mut max_parameter_update_norm: f64 = 0.0;
let mut total_cost_reduction = 0.0;
let mut final_gradient_norm;
let mut final_parameter_update_norm;
// Initialize iteration statistics tracking
let mut iteration_stats = Vec::with_capacity(self.config.max_iterations);
let mut previous_cost = state.current_cost;
// Print configuration and header if debug level is enabled
if tracing::enabled!(tracing::Level::DEBUG) {
self.config.print_configuration();
IterationStats::print_header();
}
loop {
let iter_start = time::Instant::now();
// Shared preamble: assemble, conditioning check, Jacobi scaling
let (residuals, scaled_jacobian) = match optimizer::iteration_preamble::<M>(
problem,
&mut state,
&mut self.jacobi_scaling,
self.config.use_jacobi_scaling,
iteration,
self.config.max_condition_number,
&mut jacobian_evaluations,
)? {
optimizer::IterationPreamble::Proceed {
residuals,
scaled_jacobian,
} => (residuals, scaled_jacobian),
optimizer::IterationPreamble::EarlyExit(status) => {
let elapsed = start_time.elapsed();
self.observers.notify_complete(&state.variables, iteration);
return Ok(optimizer::build_solver_result(
status,
iteration,
state,
elapsed,
0.0,
0.0,
cost_evaluations,
jacobian_evaluations,
None,
));
}
};
// Compute dog leg step
let step_result = match self.compute_optimization_step_generic::<M>(
&residuals,
&scaled_jacobian,
linear_solver,
) {
Some(result) => result,
None => {
return Err(optimizer::OptimizerError::LinearSolveFailed(
"Linear solver failed to solve system".to_string(),
)
.into());
}
};
// Update tracking
max_gradient_norm = max_gradient_norm.max(step_result.gradient_norm);
final_gradient_norm = step_result.gradient_norm;
let step_norm = step_result.step.norm_l2();
max_parameter_update_norm = max_parameter_update_norm.max(step_norm);
final_parameter_update_norm = step_norm;
// Evaluate and apply step
let step_eval = self.evaluate_and_apply_step(&step_result, &mut state, problem)?;
cost_evaluations += 1;
if step_eval.accepted {
successful_steps += 1;
total_cost_reduction += step_eval.cost_reduction;
} else {
unsuccessful_steps += 1;
}
// OPTIMIZATION: Only collect iteration statistics if debug level is enabled
if tracing::enabled!(tracing::Level::DEBUG) {
let iter_elapsed_ms = iter_start.elapsed().as_secs_f64() * 1000.0;
let total_elapsed_ms = start_time.elapsed().as_secs_f64() * 1000.0;
let stats = IterationStats {
iteration,
cost: state.current_cost,
cost_change: previous_cost - state.current_cost,
gradient_norm: step_result.gradient_norm,
step_norm,
tr_ratio: step_eval.rho,
tr_radius: self.trust_region_radius,
ls_iter: 0,
iter_time_ms: iter_elapsed_ms,
total_time_ms: total_elapsed_ms,
accepted: step_eval.accepted,
};
iteration_stats.push(stats.clone());
stats.print_line();
}
previous_cost = state.current_cost;
// Notify all observers with current state
optimizer::notify_observers_generic::<M>(
&mut self.observers,
&state.variables,
iteration,
state.current_cost,
step_result.gradient_norm,
Some(self.trust_region_radius),
step_norm,
Some(step_eval.rho),
linear_solver,
);
// Check convergence
let elapsed = start_time.elapsed();
let parameter_norm = optimizer::compute_parameter_norm(&state.variables);
let new_cost = state.current_cost;
let cost_before_step = if step_eval.accepted {
state.current_cost + step_eval.cost_reduction
} else {
state.current_cost
};
if let Some(status) = optimizer::check_convergence(&optimizer::ConvergenceParams {
iteration,
current_cost: cost_before_step,
new_cost,
parameter_norm,
parameter_update_norm: step_norm,
gradient_norm: step_result.gradient_norm,
elapsed,
step_accepted: step_eval.accepted,
max_iterations: self.config.max_iterations,
gradient_tolerance: self.config.gradient_tolerance,
parameter_tolerance: self.config.parameter_tolerance,
cost_tolerance: self.config.cost_tolerance,
min_cost_threshold: self.config.min_cost_threshold,
timeout: self.config.timeout,
trust_region_radius: Some(self.trust_region_radius),
min_trust_region_radius: Some(self.config.trust_region_min),
}) {
// Print summary only if debug level is enabled
if tracing::enabled!(tracing::Level::DEBUG) {
let summary = optimizer::create_optimizer_summary(
"Dog-Leg",
state.initial_cost,
state.current_cost,
iteration + 1,
Some(successful_steps),
Some(unsuccessful_steps),
max_gradient_norm,
final_gradient_norm,
max_parameter_update_norm,
final_parameter_update_norm,
total_cost_reduction,
elapsed,
iteration_stats.clone(),
status.clone(),
None,
Some(self.trust_region_radius),
None,
);
debug!("{}", summary);
}
// Compute covariances if enabled
let covariances = if self.config.compute_covariances {
problem.compute_and_set_covariances(
&mut state.variables,
self.config.covariance_options,
)
} else {
None
};
return Ok(optimizer::build_solver_result(
status,
iteration + 1,
state,
elapsed,
final_gradient_norm,
final_parameter_update_norm,
cost_evaluations,
jacobian_evaluations,
covariances,
));
}
iteration += 1;
}
}
/// Run optimization, automatically selecting sparse or dense path based on config.
///
/// Only solver/mode combinations that match are dispatched; anything else
/// returns an error rather than silently substituting a different solver.
/// `ExplicitSparseSchur`/`ImplicitSparseSchur` group residual rows by
/// eliminated variable and initialize the Schur block structure from the
/// shared initial state, exactly like Levenberg-Marquardt. The dog-leg
/// construction itself is solver-agnostic: the Gauss-Newton step,
/// gradient and Hessian-vector
/// products all come through the [`LinearSolver`] trait, which every Schur
/// variant serves.
pub fn optimize(&mut self, problem: &mut problem::Problem) -> optimizer::OptimizeResult {
// Chunk-wise elimination sweeps rows in chunk order, so each eliminated
// variable's rows must be adjacent. Regroup before any structure is
// built from the current layout — row order is a labelling, so this
// leaves the problem and its cost unchanged.
if matches!(
self.config.linear_solver_type,
LinearSolverType::ExplicitSparseSchur | LinearSolverType::ImplicitSparseSchur
) {
problem.group_rows_for_elimination();
}
// Built once here and moved into the chosen arm (see
// `optimize_with_mode`).
let state = optimizer::initialize_optimization_state(problem)?;
match problem.jacobian_mode {
JacobianMode::Dense => match self.config.linear_solver_type {
LinearSolverType::DenseQR => {
let mut solver = DenseQRSolver::new();
self.optimize_with_mode::<DenseMode>(problem, &mut solver, state)
}
LinearSolverType::DenseCholesky => {
let mut solver = DenseCholeskySolver::new();
self.optimize_with_mode::<DenseMode>(problem, &mut solver, state)
}
LinearSolverType::ExplicitDenseSchur => {
let init = |e: linalg::LinAlgError| {
optimizer::OptimizerError::LinearSolveFailed(format!(
"Failed to initialize Schur solver: {e}"
))
.log()
};
let mut solver = ExplicitDenseSchur::new();
solver
.initialize_structure(
&state.variables,
&state.variable_index_map,
&problem.schur_landmark_keys,
)
.map_err(init)?;
self.optimize_with_mode::<DenseMode>(problem, &mut solver, state)
}
other => Err(optimizer::OptimizerError::InvalidParameters(format!(
"Dog Leg in dense Jacobian mode supports DenseCholesky, DenseQR and \
ExplicitDenseSchur only; requested {other}"
))
.into()),
},
JacobianMode::Sparse => match self.config.linear_solver_type {
linalg::LinearSolverType::SparseQR => {
let mut solver = SparseQRSolver::new();
self.optimize_with_mode::<SparseMode>(problem, &mut solver, state)
}
linalg::LinearSolverType::SparseCholesky => {
let mut solver = SparseCholeskySolver::new();
self.optimize_with_mode::<SparseMode>(problem, &mut solver, state)
}
LinearSolverType::ExplicitSparseSchur => {
let init = |e: linalg::LinAlgError| {
optimizer::OptimizerError::LinearSolveFailed(format!(
"Failed to initialize Schur solver: {e}"
))
.log()
};
let mut solver = ExplicitSparseSchur::new()
.with_variant(self.config.schur_variant)
.with_preconditioner(self.config.schur_preconditioner)
.with_cg_params(
self.config.schur_cg_max_iterations,
self.config.schur_cg_tolerance,
)
.with_cg_q_tolerance(self.config.schur_cg_q_tolerance);
solver
.initialize_structure(
&state.variables,
&state.variable_index_map,
&problem.schur_landmark_keys,
)
.map_err(init)?;
self.optimize_with_mode::<SparseMode>(problem, &mut solver, state)
}
LinearSolverType::ImplicitSparseSchur => {
// Never forms S, so it is a different solver type rather
// than a mode of the explicit one — this is what keeps
// both the O(kept_dof²) buffer and `JᵀJ` itself out of
// the picture entirely.
let init = |e: linalg::LinAlgError| {
optimizer::OptimizerError::LinearSolveFailed(format!(
"Failed to initialize Schur solver: {e}"
))
.log()
};
let mut solver = ImplicitSparseSchur::with_config(
self.config.schur_cg_max_iterations,
self.config.schur_cg_tolerance,
self.config.schur_preconditioner,
)
.with_cg_q_tolerance(self.config.schur_cg_q_tolerance);
solver
.initialize_structure(
&state.variables,
&state.variable_index_map,
&problem.schur_landmark_keys,
)
.map_err(init)?;
self.optimize_with_mode::<SparseMode>(problem, &mut solver, state)
}
other => Err(optimizer::OptimizerError::InvalidParameters(format!(
"Dog Leg in sparse Jacobian mode supports SparseCholesky, SparseQR, \
ExplicitSparseSchur and ImplicitSparseSchur only; requested {other}"
))
.into()),
},
}
}
}
impl optimizer::Optimizer for DogLeg {
fn optimize(&mut self, problem: &mut problem::Problem) -> optimizer::OptimizeResult {
self.optimize(problem)
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::factors;
use apex_manifolds as manifold;
use faer::prelude::ReborrowMut;
use nalgebra;
type TestResult = Result<(), Box<dyn std::error::Error>>;
/// Custom Rosenbrock Factor 1: r1 = 10(x2 - x1²)
/// Demonstrates extensibility - custom factors can be defined outside of factors.rs
#[derive(Debug, Clone)]
struct RosenbrockFactor1;
impl factors::Factor for RosenbrockFactor1 {
fn linearize(
&self,
params: &[&[f64]],
residual: &mut [f64],
jacobian: Option<faer::mat::MatMut<'_, f64>>,
) {
let x1 = params[0][0];
let x2 = params[1][0];
residual[0] = 10.0 * (x2 - x1 * x1);
if let Some(mut jac) = jacobian {
*jac.rb_mut().get_mut(0, 0) = -20.0 * x1;
*jac.rb_mut().get_mut(0, 1) = 10.0;
}
}
fn residual_dim(&self) -> usize {
1
}
fn jacobian_shape(&self) -> (usize, usize) {
(1, 2)
}
}
/// Custom Rosenbrock Factor 2: r2 = 1 - x1
/// Demonstrates extensibility - custom factors can be defined outside of factors.rs
#[derive(Debug, Clone)]
struct RosenbrockFactor2;
impl factors::Factor for RosenbrockFactor2 {
fn linearize(
&self,
params: &[&[f64]],
residual: &mut [f64],
jacobian: Option<faer::mat::MatMut<'_, f64>>,
) {
residual[0] = 1.0 - params[0][0];
if let Some(mut jac) = jacobian {
*jac.rb_mut().get_mut(0, 0) = -1.0;
}
}
fn residual_dim(&self) -> usize {
1
}
fn jacobian_shape(&self) -> (usize, usize) {
(1, 1)
}
}
#[test]
fn test_rosenbrock_optimization() -> TestResult {
// Rosenbrock function test:
// Minimize: r1² + r2² where
// r1 = 10(x2 - x1²)
// r2 = 1 - x1
// Starting point: [-1.2, 1.0]
// Expected minimum: [1.0, 1.0]
let mut problem = problem::Problem::new(JacobianMode::Sparse);
let x1 = problem.add_variable(manifold::ManifoldType::RN, nalgebra::dvector![-1.2]);
let x2 = problem.add_variable(manifold::ManifoldType::RN, nalgebra::dvector![1.0]);
// Add custom factors (demonstrates extensibility!)
problem.add_residual_block(&[x1, x2], Box::new(RosenbrockFactor1), None);
problem.add_residual_block(&[x1], Box::new(RosenbrockFactor2), None);
// Configure Dog Leg optimizer with appropriate trust region
let config = DogLegConfig::new()
.with_max_iterations(100)
.with_cost_tolerance(1e-8)
.with_parameter_tolerance(1e-8)
.with_gradient_tolerance(1e-10)
.with_trust_region_radius(10.0); // Start with larger trust region
let mut solver = DogLeg::with_config(config);
let result = solver.optimize(&mut problem)?;
// Extract final values
let x1_final = result.parameters[x1].as_param_slice()[0];
let x2_final = result.parameters[x2].as_param_slice()[0];
// Verify convergence to [1.0, 1.0]
assert!(
matches!(
result.status,
optimizer::OptimizationStatus::Converged
| optimizer::OptimizationStatus::CostToleranceReached
| optimizer::OptimizationStatus::ParameterToleranceReached
| optimizer::OptimizationStatus::GradientToleranceReached
),
"Optimization should converge"
);
assert!(
(x1_final - 1.0).abs() < 1e-4,
"x1 should converge to 1.0, got {}",
x1_final
);
assert!(
(x2_final - 1.0).abs() < 1e-4,
"x2 should converge to 1.0, got {}",
x2_final
);
assert!(
result.final_cost < 1e-6,
"Final cost should be near zero, got {}",
result.final_cost
);
Ok(())
}
/// Trivial factor: r = x - target, J = [[1.0]]
struct LinearFactor {
target: f64,
}
impl factors::Factor for LinearFactor {
fn linearize(
&self,
params: &[&[f64]],
residual: &mut [f64],
jacobian: Option<faer::mat::MatMut<'_, f64>>,
) {
residual[0] = params[0][0] - self.target;
if let Some(mut jac) = jacobian {
*jac.rb_mut().get_mut(0, 0) = 1.0;
}
}
fn residual_dim(&self) -> usize {
1
}
fn jacobian_shape(&self) -> (usize, usize) {
(1, 1)
}
}
fn rosenbrock_problem() -> problem::Problem {
let mut prob = problem::Problem::new(JacobianMode::Sparse);
let x1 = prob.add_variable(manifold::ManifoldType::RN, nalgebra::dvector![-1.2]);
let x2 = prob.add_variable(manifold::ManifoldType::RN, nalgebra::dvector![1.0]);
prob.add_residual_block(&[x1, x2], Box::new(RosenbrockFactor1), None);
prob.add_residual_block(&[x1], Box::new(RosenbrockFactor2), None);
prob
}
// -------------------------------------------------------------------------
// Ceres-compatibility config fields
// -------------------------------------------------------------------------
/// `min_relative_decrease` replaces the hardcoded `rho > 1e-4` gate.
#[test]
fn dogleg_min_relative_decrease_gates_acceptance() -> TestResult {
let mut permissive_problem = rosenbrock_problem();
let permissive = DogLeg::with_config(
DogLegConfig::new()
.with_max_iterations(100)
.with_min_relative_decrease(1e-3),
)
.optimize(&mut permissive_problem)?;
let mut strict_problem = rosenbrock_problem();
let strict = DogLeg::with_config(
DogLegConfig::new()
.with_max_iterations(100)
.with_min_relative_decrease(0.999_999),
)
.optimize(&mut strict_problem)?;
assert!(
strict.final_cost > permissive.final_cost,
"a near-1.0 acceptance threshold should leave the cost higher: \
strict {:.3e} vs permissive {:.3e}",
strict.final_cost,
permissive.final_cost
);
Ok(())
}
/// `trust_region_increase_factor` drives the radius growth that used to be a
/// hardcoded `3.0`.
///
/// The initial radius has to be small for this to be observable: the growth
/// rule is `radius ← max(radius, factor · ‖step‖)`, so while the radius
/// still dwarfs the step — as it does from the default 1e4 on this problem —
/// the factor is multiplied by a number that never wins the `max`.
#[test]
fn dogleg_trust_region_increase_factor_is_read() -> TestResult {
let run = |factor: f64| -> Result<usize, Box<dyn std::error::Error>> {
let mut problem = rosenbrock_problem();
let result = DogLeg::with_config(
DogLegConfig::new()
.with_max_iterations(200)
.with_trust_region_radius(1e-3)
.with_trust_region_factors(factor, 0.5),
)
.optimize(&mut problem)?;
Ok(result.iterations)
};
let fast = run(50.0)?;
let slow = run(1.0)?;
assert!(
fast < slow,
"a 50x radius growth factor should reach the solution in fewer \
iterations than a 1x (never-grow) factor: {fast} vs {slow}"
);
Ok(())
}
/// The radius is run state, so a reused solver reproduces its first result.
#[test]
fn dogleg_repeated_optimize_calls_are_reproducible() -> TestResult {
let mut solver = DogLeg::with_config(DogLegConfig::new().with_max_iterations(100));
let mut first_problem = rosenbrock_problem();
let first = solver.optimize(&mut first_problem)?;
let mut second_problem = rosenbrock_problem();
let second = solver.optimize(&mut second_problem)?;
assert_eq!(
first.iterations, second.iterations,
"reusing a solver changed the iteration count: {} then {}",
first.iterations, second.iterations
);
assert!(
(first.final_cost - second.final_cost).abs() < 1e-15,
"reusing a solver changed the final cost: {:.17e} then {:.17e}",
first.final_cost,
second.final_cost
);
Ok(())
}
/// `max_condition_number` detects a variable no residual constrains.
#[test]
fn dogleg_max_condition_number_detects_an_unconstrained_variable() -> TestResult {
let mut problem = rosenbrock_problem();
let _orphan = problem.add_variable(manifold::ManifoldType::RN, nalgebra::dvector![0.0]);
let result = DogLeg::with_config(
DogLegConfig::new()
.with_max_iterations(10)
.with_max_condition_number(1e12),
)
.optimize(&mut problem)?;
assert_eq!(
result.status,
optimizer::OptimizationStatus::IllConditionedJacobian
);
Ok(())
}
fn linear_problem(start: f64) -> problem::Problem {
let mut prob = problem::Problem::new(JacobianMode::Sparse);
let x = prob.add_variable(manifold::ManifoldType::RN, nalgebra::dvector![start]);
prob.add_residual_block(&[x], Box::new(LinearFactor { target: 0.0 }), None);
prob
}
// -------------------------------------------------------------------------
// DogLegConfig builder tests
// -------------------------------------------------------------------------
#[test]
fn test_dl_config_default() {
let cfg = DogLegConfig::default();
assert_eq!(cfg.max_iterations, 50);
assert!((cfg.cost_tolerance - 1e-6).abs() < 1e-15);
assert!(cfg.use_jacobi_scaling); // DogLeg enables Jacobi scaling by default
assert!(!cfg.compute_covariances);
assert!(cfg.enable_step_reuse);
}
#[test]
// Asserts that the deprecated `min_step_quality` setter still stores its
// value; the field is no longer read by the algorithm.
#[allow(deprecated)]
fn test_dl_config_builders() {
use crate::linalg::LinearSolverType;
let cfg = DogLegConfig::new()
.with_max_iterations(20)
.with_cost_tolerance(1e-5)
.with_parameter_tolerance(1e-6)
.with_gradient_tolerance(1e-7)
.with_trust_region_radius(200.0)
.with_trust_region_bounds(1e-15, 1e15)
.with_trust_region_factors(4.0, 0.3)
.with_step_quality_thresholds(0.01, 0.3, 0.7)
.with_jacobi_scaling(false)
.with_mu_params(1e-3, 1e-10, 2.0, 5.0)
.with_step_reuse(false)
.with_min_cost_threshold(1e-9)
.with_compute_covariances(true)
.with_linear_solver_type(LinearSolverType::SparseQR);
assert_eq!(cfg.max_iterations, 20);
assert!((cfg.trust_region_radius - 200.0).abs() < 1e-10);
assert!(!cfg.use_jacobi_scaling);
assert!(!cfg.enable_step_reuse);
assert!(cfg.min_cost_threshold.is_some());
assert!(cfg.compute_covariances);
assert!((cfg.min_step_quality - 0.01).abs() < 1e-15);
assert!((cfg.poor_step_quality - 0.3).abs() < 1e-15);
assert!((cfg.good_step_quality - 0.7).abs() < 1e-15);
assert!((cfg.initial_mu - 1e-3).abs() < 1e-15);
}
#[test]
fn test_dl_print_configuration_no_panic() {
DogLegConfig::default().print_configuration();
}
#[test]
fn test_dl_default_equals_new() {
let _a = DogLeg::new();
let _b = DogLeg::default();
}
#[test]
fn test_dl_with_config_method() {
let cfg = DogLegConfig::new().with_max_iterations(5);
let _solver = DogLeg::with_config(cfg);
}
// -------------------------------------------------------------------------
// Convergence termination paths
// -------------------------------------------------------------------------
#[test]
fn test_dl_max_iterations_termination() -> TestResult {
let mut problem = rosenbrock_problem();
let cfg = DogLegConfig::new().with_max_iterations(2);
let mut solver = DogLeg::with_config(cfg);
let result = solver.optimize(&mut problem)?;
assert_eq!(
result.status,
optimizer::OptimizationStatus::MaxIterationsReached
);
assert!(result.iterations <= 3, "iterations={}", result.iterations);
Ok(())
}
#[test]
fn test_dl_gradient_tolerance_convergence() -> TestResult {
let mut problem = linear_problem(1.0);
let cfg = DogLegConfig::new()
.with_gradient_tolerance(1e3)
.with_cost_tolerance(1e-20)
.with_parameter_tolerance(1e-20);
let mut solver = DogLeg::with_config(cfg);
let result = solver.optimize(&mut problem)?;
assert_eq!(
result.status,
optimizer::OptimizationStatus::GradientToleranceReached
);
Ok(())
}
#[test]
fn test_dl_cost_tolerance_convergence() -> TestResult {
let mut problem = rosenbrock_problem();
let cfg = DogLegConfig::new()
.with_cost_tolerance(1e2) // very loose
.with_gradient_tolerance(1e-20)
.with_parameter_tolerance(1e-20);
let mut solver = DogLeg::with_config(cfg);
let result = solver.optimize(&mut problem)?;
assert!(matches!(
result.status,
optimizer::OptimizationStatus::CostToleranceReached
| optimizer::OptimizationStatus::GradientToleranceReached
| optimizer::OptimizationStatus::ParameterToleranceReached
| optimizer::OptimizationStatus::Converged
));
Ok(())
}
#[test]
fn test_dl_qr_solver() -> TestResult {
use crate::linalg::LinearSolverType;
let mut problem = rosenbrock_problem();
let cfg = DogLegConfig::new()
.with_linear_solver_type(LinearSolverType::SparseQR)
.with_max_iterations(100);
let mut solver = DogLeg::with_config(cfg);
let result = solver.optimize(&mut problem)?;
assert!(result.final_cost < 1e-6);
Ok(())
}
#[test]
fn test_dl_jacobi_scaling_disabled() -> TestResult {
// DogLeg has Jacobi scaling ON by default; test with it explicitly disabled
let mut problem = rosenbrock_problem();
let cfg = DogLegConfig::new()
.with_jacobi_scaling(false)
.with_max_iterations(100);
let mut solver = DogLeg::with_config(cfg);
let result = solver.optimize(&mut problem)?;
assert!(result.final_cost < 1e-6);
Ok(())
}
/// A residual whose two columns have very different norms (100 vs 1),
/// so Jacobi scaling actually changes the gradient it would report if
/// left un-corrected.
struct IllScaledFactor {
a: f64,
}
impl factors::Factor for IllScaledFactor {
fn linearize(
&self,
params: &[&[f64]],
residual: &mut [f64],
jacobian: Option<faer::mat::MatMut<'_, f64>>,
) {
let x1 = params[0][0];
let x2 = params[1][0];
residual[0] = self.a * x1 - 1.0;
residual[1] = x2 - 1.0;
if let Some(mut jac) = jacobian {
*jac.rb_mut().get_mut(0, 0) = self.a;
*jac.rb_mut().get_mut(0, 1) = 0.0;
*jac.rb_mut().get_mut(1, 0) = 0.0;
*jac.rb_mut().get_mut(1, 1) = 1.0;
}
}
fn residual_dim(&self) -> usize {
2
}
fn jacobian_shape(&self) -> (usize, usize) {
(2, 2)
}
}
/// GitHub #57: the reported `gradient_norm` must be `‖Jᵀr‖` from the
/// *un-scaled* Jacobian — matching `gradient_tolerance`'s documented
/// contract — regardless of Jacobi column scaling, which DogLeg enables
/// by default. `IllScaledFactor`'s columns (norm 100 vs 1) make the
/// scaled and un-scaled gradients differ by roughly two orders of
/// magnitude, so a stale scaled value is easy to detect.
fn assert_dl_gradient_norm_matches_unscaled_jacobian(use_jacobi_scaling: bool) -> TestResult {
let mut problem = problem::Problem::new(JacobianMode::Sparse);
let x1 = problem.add_variable(manifold::ManifoldType::RN, nalgebra::dvector![0.0]);
let x2 = problem.add_variable(manifold::ManifoldType::RN, nalgebra::dvector![0.0]);
problem.add_residual_block(&[x1, x2], Box::new(IllScaledFactor { a: 100.0 }), None);
let mut state = crate::optimizer::initialize_optimization_state(&mut problem)?;
let (residuals, jacobian) = SparseMode::assemble(
&problem,
&state.variables,
&state.variable_index_map,
state.symbolic_structure.as_ref(),
state.total_dof,
&mut state.workspace,
)?;
// Reference: ||J^T r|| from a plain, un-scaled solve.
let mut reference_solver = SparseCholeskySolver::new();
reference_solver.solve_normal_equation(&residuals, &jacobian)?;
let expected = reference_solver
.get_gradient()
.ok_or("reference solver produced no gradient")?
.norm_l2();
let cfg = DogLegConfig::new().with_jacobi_scaling(use_jacobi_scaling);
let mut solver = DogLeg::with_config(cfg);
let solver_jacobian = if use_jacobi_scaling {
crate::optimizer::process_jacobian_generic::<SparseMode>(
&jacobian,
&mut solver.jacobi_scaling,
0,
)?
} else {
jacobian.clone()
};
let mut linear_solver = SparseCholeskySolver::new();
let step_result = solver
.compute_optimization_step_generic::<SparseMode>(
&residuals,
&solver_jacobian,
&mut linear_solver,
)
.ok_or("compute_optimization_step_generic returned None")?;
assert!(
(step_result.gradient_norm - expected).abs() < 1e-6 * expected.max(1.0),
"gradient_norm {} disagrees with the un-scaled ||J^T r|| {} \
(use_jacobi_scaling = {})",
step_result.gradient_norm,
expected,
use_jacobi_scaling,
);
Ok(())
}
#[test]
fn test_dl_gradient_norm_matches_unscaled_jacobian_without_scaling() -> TestResult {
assert_dl_gradient_norm_matches_unscaled_jacobian(false)
}
#[test]
fn test_dl_gradient_norm_matches_unscaled_jacobian_with_scaling() -> TestResult {
assert_dl_gradient_norm_matches_unscaled_jacobian(true)
}
#[test]
fn test_dl_min_cost_threshold() -> TestResult {
let mut problem = rosenbrock_problem();
let cfg = DogLegConfig::new()
.with_min_cost_threshold(1e10)
.with_cost_tolerance(1e-20)
.with_gradient_tolerance(1e-20)
.with_parameter_tolerance(1e-20);
let mut solver = DogLeg::with_config(cfg);
let result = solver.optimize(&mut problem)?;
assert_eq!(
result.status,
optimizer::OptimizationStatus::MinCostThresholdReached
);
Ok(())
}
#[test]
fn test_dl_trust_region_radius_in_config() -> TestResult {
let mut problem = rosenbrock_problem();
let cfg = DogLegConfig::new()
.with_trust_region_radius(0.1) // small initial radius
.with_max_iterations(200);
let mut solver = DogLeg::with_config(cfg);
let result = solver.optimize(&mut problem)?;
assert!(result.iterations > 0);
Ok(())
}
#[test]
fn test_dl_step_reuse_config() -> TestResult {
let mut problem = rosenbrock_problem();
let cfg = DogLegConfig::new()
.with_step_reuse(true)
.with_max_iterations(100);
let mut solver = DogLeg::with_config(cfg);
let result = solver.optimize(&mut problem)?;
assert!(result.final_cost < 1e-6);
Ok(())
}
#[test]
fn test_dl_mu_params_config() {
let cfg = DogLegConfig::new().with_mu_params(1e-2, 1e-9, 10.0, 20.0);
assert!((cfg.initial_mu - 1e-2).abs() < 1e-15);
assert!((cfg.min_mu - 1e-9).abs() < 1e-20);
assert!((cfg.max_mu - 10.0).abs() < 1e-12);
assert!((cfg.mu_increase_factor - 20.0).abs() < 1e-12);
}
#[test]
// Asserts that the deprecated `min_step_quality` setter still stores its
// value; the field is no longer read by the algorithm.
#[allow(deprecated)]
fn test_dl_step_quality_thresholds() {
let cfg = DogLegConfig::new().with_step_quality_thresholds(0.05, 0.25, 0.8);
assert!((cfg.min_step_quality - 0.05).abs() < 1e-15);
assert!((cfg.poor_step_quality - 0.25).abs() < 1e-15);
assert!((cfg.good_step_quality - 0.8).abs() < 1e-15);
}
#[test]
fn test_dl_result_fields() -> TestResult {
let mut problem = rosenbrock_problem();
let mut solver = DogLeg::new();
let result = solver.optimize(&mut problem)?;
assert!(result.initial_cost > result.final_cost);
assert!(result.iterations > 0);
assert!(result.convergence_info.is_some());
Ok(())
}
#[test]
fn test_dl_timeout_config() {
let cfg = DogLegConfig::new().with_timeout(time::Duration::from_secs(60));
assert!(cfg.timeout.is_some());
}
}