tiny_solver/
corrector.rs

1use nalgebra as na;
2/// https://github.com/ceres-solver/ceres-solver/blob/master/internal/ceres/corrector.cc
3use std::ops::Mul;
4
5pub(crate) struct Corrector {
6    sqrt_rho1: f64,
7    residual_scaling: f64,
8    alpha_sq_norm: f64,
9}
10
11impl Corrector {
12    pub fn new(sq_norm: f64, rho: &[f64; 3]) -> Corrector {
13        let sqrt_rho1 = rho[1].sqrt();
14        // is always well formed.
15        if (sq_norm == 0.0) || (rho[2] <= 0.0) {
16            let residual_scaling = sqrt_rho1;
17            let alpha_sq_norm = 0.0;
18            Corrector {
19                sqrt_rho1,
20                residual_scaling,
21                alpha_sq_norm,
22            }
23        } else {
24            let d = 1.0 + 2.0 * sq_norm * rho[2] / rho[1];
25
26            let alpha = 1.0 - d.sqrt();
27
28            let residual_scaling = sqrt_rho1 / (1.0 - alpha);
29            let alpha_sq_norm = alpha / sq_norm;
30            Corrector {
31                sqrt_rho1,
32                residual_scaling,
33                alpha_sq_norm,
34            }
35        }
36    }
37    pub fn correct_jacobian(&self, residuals: &na::DVector<f64>, jacobian: &mut na::DMatrix<f64>) {
38        // The common case (rho[2] <= 0).
39        if self.alpha_sq_norm == 0.0 {
40            *jacobian = jacobian.clone().mul(self.sqrt_rho1);
41            return;
42        }
43
44        //  J = sqrt(rho) * (J - alpha^2 r * r' J)
45        let r_rtj = residuals.clone() * residuals.transpose() * jacobian.clone();
46        *jacobian = (jacobian.clone() - r_rtj.mul(self.alpha_sq_norm)).mul(self.sqrt_rho1);
47    }
48    pub fn correct_residuals(&self, residuals: &mut na::DVector<f64>) {
49        *residuals = residuals.clone().mul(self.residual_scaling);
50    }
51}
tiny_solver/corrector.rs

tiny_solver/
corrector.rs
