use crate::qr::{LinearLeastSquaresDiagonalProblem, PivotedQR};
use crate::trust_region::{determine_lambda_and_parameter_update, LMParameter};
use crate::utils::{enorm, epsmch};
use crate::LeastSquaresProblem;
use nalgebra::{
allocator::{Allocator, Reallocator},
convert,
storage::Storage,
DefaultAllocator, Dim, DimMax, DimMaximum, DimMin, Matrix, RealField, Vector, VectorN, U1,
};
use num_traits::Float;
#[cfg(test)]
#[allow(
clippy::float_cmp,
clippy::excessive_precision,
clippy::redundant_clone
)]
pub(crate) mod test_examples;
#[cfg(test)]
mod test_helpers;
#[cfg(test)]
mod test_init_step;
#[cfg(test)]
#[allow(clippy::float_cmp, clippy::clone_on_copy, clippy::redundant_clone)]
mod test_update_diag;
#[derive(PartialEq, Eq, Debug)]
pub enum TerminationReason {
User(&'static str),
Numerical(&'static str),
ResidualsZero,
Orthogonal,
Converged { ftol: bool, xtol: bool },
NoImprovementPossible(&'static str),
LostPatience,
NoParameters,
NoResiduals,
WrongDimensions(&'static str),
}
impl TerminationReason {
pub fn was_successful(&self) -> bool {
match self {
TerminationReason::ResidualsZero
| TerminationReason::Orthogonal
| TerminationReason::Converged { .. } => true,
_ => false,
}
}
pub fn was_usage_issue(&self) -> bool {
match self {
TerminationReason::NoParameters
| TerminationReason::NoResiduals
| TerminationReason::NoImprovementPossible(_)
| TerminationReason::WrongDimensions(_) => true,
_ => false,
}
}
}
#[derive(Debug)]
pub struct MinimizationReport<F: RealField> {
pub termination: TerminationReason,
pub number_of_evaluations: usize,
pub objective_function: F,
}
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
pub struct LevenbergMarquardt<F> {
ftol: F,
xtol: F,
gtol: F,
stepbound: F,
patience: usize,
scale_diag: bool,
}
impl<F: RealField + Float> Default for LevenbergMarquardt<F> {
fn default() -> Self {
Self::new()
}
}
impl<F: RealField + Float> LevenbergMarquardt<F> {
pub fn new() -> Self {
if cfg!(feature = "minpack-compat") {
let user_tol = convert(1.49012e-08);
Self {
ftol: user_tol,
xtol: user_tol,
gtol: F::zero(),
stepbound: convert(100.0),
patience: 100,
scale_diag: true,
}
} else {
let user_tol = F::default_epsilon() * convert(30.0);
Self {
ftol: user_tol,
xtol: user_tol,
gtol: user_tol,
stepbound: convert(100.0),
patience: 100,
scale_diag: true,
}
}
}
pub fn with_ftol(self, ftol: F) -> Self {
assert!(!ftol.is_negative(), "ftol must be >= 0");
Self { ftol, ..self }
}
pub fn with_xtol(self, xtol: F) -> Self {
assert!(!xtol.is_negative(), "xtol must be >= 0");
Self { xtol, ..self }
}
pub fn with_gtol(self, gtol: F) -> Self {
assert!(!gtol.is_negative(), "gtol must be >= 0");
Self { gtol, ..self }
}
pub fn with_tol(self, tol: F) -> Self {
assert!(tol.is_positive(), "tol must > 0");
Self {
ftol: tol,
xtol: tol,
gtol: F::zero(),
..self
}
}
pub fn with_stepbound(self, stepbound: F) -> Self {
assert!(stepbound.is_positive(), "stepbound must be > 0");
Self { stepbound, ..self }
}
pub fn with_patience(self, patience: usize) -> Self {
assert!(patience > 0, "patience must be > 0");
Self { patience, ..self }
}
pub fn with_scale_diag(self, scale_diag: bool) -> Self {
Self { scale_diag, ..self }
}
pub fn minimize<N, M, O>(&self, target: O) -> (O, MinimizationReport<F>)
where
N: Dim,
M: DimMin<N> + DimMax<N>,
O: LeastSquaresProblem<F, M, N>,
DefaultAllocator:
Allocator<F, N> + Reallocator<F, M, N, DimMaximum<M, N>, N> + Allocator<usize, N>,
{
let (mut lm, mut residuals) = match LM::new(self, target) {
Err(report) => return report,
Ok(res) => res,
};
let n = lm.x.nrows();
loop {
let mut lls = {
let jacobian = match lm.jacobian() {
Err(reason) => return lm.into_report(reason),
Ok(jacobian) => jacobian,
};
if jacobian.ncols() != n || jacobian.nrows() != lm.m {
return lm.into_report(TerminationReason::WrongDimensions("jacobian"));
}
let qr = PivotedQR::new(jacobian);
qr.into_least_squares_diagonal_problem(residuals)
};
if let Err(reason) = lm.update_diag(&mut lls) {
return lm.into_report(reason);
};
residuals = loop {
let param =
determine_lambda_and_parameter_update(&mut lls, &lm.diag, lm.delta, lm.lambda);
let tr_iteration = lm.trust_region_iteration(&mut lls, param);
match tr_iteration {
Ok(Some(residuals)) => break residuals,
Err(reason) => return lm.into_report(reason),
Ok(None) => (),
}
};
}
}
}
struct LM<'a, F, N, M, O>
where
F: RealField,
N: Dim,
M: DimMin<N> + DimMax<N>,
O: LeastSquaresProblem<F, M, N>,
DefaultAllocator: Allocator<F, N> + Allocator<F, DimMaximum<M, N>, N>,
{
config: &'a LevenbergMarquardt<F>,
x: Vector<F, N, O::ParameterStorage>,
tmp: Vector<F, N, O::ParameterStorage>,
target: O,
report: MinimizationReport<F>,
delta: F,
lambda: F,
xnorm: F,
gnorm: F,
residuals_norm: F,
diag: VectorN<F, N>,
first_trust_region_iteration: bool,
first_update: bool,
max_fev: usize,
m: usize,
}
impl<'a, F, N, M, O> LM<'a, F, N, M, O>
where
F: RealField + Float,
N: Dim,
M: DimMin<N> + DimMax<N>,
O: LeastSquaresProblem<F, M, N>,
DefaultAllocator: Allocator<F, N> + Allocator<F, DimMaximum<M, N>, N>,
{
#[allow(clippy::type_complexity)]
fn new(
config: &'a LevenbergMarquardt<F>,
target: O,
) -> Result<(Self, Vector<F, M, O::ResidualStorage>), (O, MinimizationReport<F>)> {
let mut report = MinimizationReport {
termination: TerminationReason::ResidualsZero,
number_of_evaluations: 1,
objective_function: <F as Float>::nan(),
};
let x = target.params();
let (residuals, residuals_norm) = if let Some(residuals) = target.residuals() {
let norm = enorm(&residuals);
report.objective_function = norm * norm * convert(0.5);
(residuals, norm)
} else {
return Err((
target,
MinimizationReport {
termination: TerminationReason::User("residuals"),
..report
},
));
};
let n = x.data.shape().0;
let diag = VectorN::<F, N>::from_element_generic(n, U1, F::one());
if diag.nrows() == 0 {
return Err((
target,
MinimizationReport {
termination: TerminationReason::NoParameters,
..report
},
));
}
let m = residuals.nrows();
if m == 0 {
return Err((
target,
MinimizationReport {
termination: TerminationReason::NoResiduals,
..report
},
));
}
if !residuals_norm.is_finite() && !cfg!(feature = "minpack-compat") {
return Err((
target,
MinimizationReport {
termination: TerminationReason::Numerical("residuals norm"),
..report
},
));
}
if residuals_norm <= Float::min_positive_value() && !cfg!(feature = "minpack-compat") {
return Err((target, report));
}
Ok((
Self {
config,
target,
report,
tmp: x.clone(),
x,
diag,
delta: F::zero(),
lambda: F::zero(),
xnorm: F::zero(),
gnorm: F::zero(),
residuals_norm,
first_trust_region_iteration: true,
first_update: true,
max_fev: config.patience * (n.value() + 1),
m,
},
residuals,
))
}
fn into_report(self, termination: TerminationReason) -> (O, MinimizationReport<F>) {
(
self.target,
MinimizationReport {
termination,
..self.report
},
)
}
fn jacobian(&self) -> Result<Matrix<F, M, N, O::JacobianStorage>, TerminationReason> {
match self.target.jacobian() {
Some(jacobian) => Ok(jacobian),
None => Err(TerminationReason::User("jacobian")),
}
}
fn update_diag(
&mut self,
lls: &mut LinearLeastSquaresDiagonalProblem<F, M, N>,
) -> Result<(), TerminationReason>
where
DefaultAllocator: Allocator<usize, N>,
{
self.gnorm = match lls.max_a_t_b_scaled(self.residuals_norm) {
Some(max_at_b) => max_at_b,
None if !cfg!(feature = "minpack-compat") => {
return Err(TerminationReason::Numerical("jacobian"))
}
None => F::zero(),
};
if self.gnorm <= self.config.gtol {
return Err(TerminationReason::Orthogonal);
}
if self.first_update {
self.xnorm = if self.config.scale_diag {
for (d, col_norm) in self.diag.iter_mut().zip(lls.column_norms.iter()) {
*d = if col_norm.is_zero() {
F::one()
} else {
*col_norm
};
}
self.tmp.cmpy(F::one(), &self.diag, &self.x, F::zero());
enorm(&self.tmp)
} else {
enorm(&self.x)
};
if !self.xnorm.is_finite() && !cfg!(feature = "minpack-compat") {
return Err(TerminationReason::Numerical("subproblem x"));
}
self.delta = if self.xnorm.is_zero() {
self.config.stepbound
} else {
self.config.stepbound * self.xnorm
};
self.first_update = false;
} else if self.config.scale_diag {
for (d, norm) in self.diag.iter_mut().zip(lls.column_norms.iter()) {
*d = Float::max(*norm, *d);
}
}
Ok(())
}
#[allow(clippy::type_complexity)]
fn trust_region_iteration(
&mut self,
lls: &mut LinearLeastSquaresDiagonalProblem<F, M, N>,
param: LMParameter<F, N>,
) -> Result<Option<Vector<F, M, O::ResidualStorage>>, TerminationReason>
where
DefaultAllocator: Allocator<usize, N>,
{
const P1: f64 = 0.1;
const P0001: f64 = 1.0e-4;
self.lambda = param.lambda;
let pnorm = param.dp_norm;
if !pnorm.is_finite() && !cfg!(feature = "minpack-compat") {
return Err(TerminationReason::Numerical("subproblem ||Dp||"));
}
let predicted_reduction;
let dir_der;
{
let temp1 = Float::powi(lls.a_x_norm(¶m.step) / self.residuals_norm, 2);
if !temp1.is_finite() && !cfg!(feature = "minpack-compat") {
return Err(TerminationReason::Numerical("trust-region reduction"));
}
let temp2 = Float::powi((Float::sqrt(self.lambda) * pnorm) / self.residuals_norm, 2);
if !temp2.is_finite() && !cfg!(feature = "minpack-compat") {
return Err(TerminationReason::Numerical("trust-region reduction"));
}
predicted_reduction = temp1 + temp2 / convert(0.5);
dir_der = -(temp1 + temp2);
}
if self.first_trust_region_iteration && pnorm < self.delta {
self.delta = pnorm;
}
self.first_trust_region_iteration = false;
self.tmp.copy_from(&self.x);
self.tmp.axpy(-F::one(), ¶m.step, F::one());
self.target.set_params(&self.tmp);
self.report.number_of_evaluations += 1;
let new_objective_function;
let (residuals, new_residuals_norm) = if let Some(residuals) = self.target.residuals() {
if residuals.nrows() != self.m {
return Err(TerminationReason::WrongDimensions("residuals"));
}
let norm = enorm(&residuals);
new_objective_function = norm * norm * convert(0.5);
(residuals, norm)
} else {
return Err(TerminationReason::User("residuals"));
};
let actual_reduction = if new_residuals_norm * convert(P1) < self.residuals_norm {
F::one() - Float::powi(new_residuals_norm / self.residuals_norm, 2)
} else {
-F::one()
};
let ratio = if predicted_reduction.is_zero() {
F::zero()
} else {
actual_reduction / predicted_reduction
};
let half: F = convert(0.5);
if ratio <= convert(0.25) {
let mut temp = if !actual_reduction.is_negative() {
half
} else {
half * dir_der / (dir_der + half * actual_reduction)
};
if new_residuals_norm * convert(P1) >= self.residuals_norm || temp < convert(P1) {
temp = convert(P1);
};
self.delta = temp * Float::min(self.delta, pnorm * convert(10.));
self.lambda /= temp;
} else if self.lambda.is_zero() || ratio >= convert(0.75) {
self.delta = pnorm / convert(0.5);
self.lambda *= half;
}
let update_considered_good = ratio >= convert(P0001);
if update_considered_good {
core::mem::swap(&mut self.x, &mut self.tmp);
self.xnorm = if self.config.scale_diag {
self.tmp.cmpy(F::one(), &self.diag, &self.x, F::zero());
enorm(&self.tmp)
} else {
enorm(&self.x)
};
if !self.xnorm.is_finite() && !cfg!(feature = "minpack-compat") {
return Err(TerminationReason::Numerical("new x"));
}
self.residuals_norm = new_residuals_norm;
self.report.objective_function = new_objective_function;
}
if !cfg!(feature = "minpack-compat") && self.residuals_norm <= F::min_positive_value() {
self.reset_params_if(!update_considered_good);
return Err(TerminationReason::ResidualsZero);
}
let ftol_check = Float::abs(actual_reduction) <= self.config.ftol
&& predicted_reduction <= self.config.ftol
&& ratio * convert(0.5) <= F::one();
let xtol_check = self.delta <= self.config.xtol * self.xnorm;
if ftol_check || xtol_check {
self.reset_params_if(!update_considered_good);
return Err(TerminationReason::Converged {
ftol: ftol_check,
xtol: xtol_check,
});
}
if self.report.number_of_evaluations >= self.max_fev {
self.reset_params_if(!update_considered_good);
return Err(TerminationReason::LostPatience);
}
if Float::abs(actual_reduction) <= epsmch()
&& predicted_reduction <= epsmch()
&& ratio * convert(0.5) <= F::one()
{
self.reset_params_if(!update_considered_good);
return Err(TerminationReason::NoImprovementPossible("ftol"));
}
if self.delta <= epsmch::<F>() * self.xnorm {
self.reset_params_if(!update_considered_good);
return Err(TerminationReason::NoImprovementPossible("xtol"));
}
if self.gnorm <= epsmch() {
self.reset_params_if(!update_considered_good);
return Err(TerminationReason::NoImprovementPossible("gtol"));
}
if update_considered_good {
Ok(Some(residuals))
} else {
Ok(None)
}
}
#[inline]
fn reset_params_if(&mut self, reset: bool) {
if reset {
self.target.set_params(&self.x);
}
}
}