tiny_solver/
corrector.rs

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
use nalgebra as na;
/// https://github.com/ceres-solver/ceres-solver/blob/master/internal/ceres/corrector.cc
use std::ops::Mul;

pub(crate) struct Corrector {
    sqrt_rho1: f64,
    residual_scaling: f64,
    alpha_sq_norm: f64,
}

impl Corrector {
    pub fn new(sq_norm: f64, rho: &[f64; 3]) -> Corrector {
        let sqrt_rho1 = rho[1].sqrt();
        // is always well formed.
        if (sq_norm == 0.0) || (rho[2] <= 0.0) {
            let residual_scaling = sqrt_rho1;
            let alpha_sq_norm = 0.0;
            Corrector {
                sqrt_rho1,
                residual_scaling,
                alpha_sq_norm,
            }
        } else {
            let d = 1.0 + 2.0 * sq_norm * rho[2] / rho[1];

            let alpha = 1.0 - d.sqrt();

            let residual_scaling = sqrt_rho1 / (1.0 - alpha);
            let alpha_sq_norm = alpha / sq_norm;
            Corrector {
                sqrt_rho1,
                residual_scaling,
                alpha_sq_norm,
            }
        }
    }
    pub fn correct_jacobian(&self, residuals: &na::DVector<f64>, jacobian: &mut na::DMatrix<f64>) {
        // The common case (rho[2] <= 0).
        if self.alpha_sq_norm == 0.0 {
            *jacobian = jacobian.clone().mul(self.sqrt_rho1);
            return;
        }

        //  J = sqrt(rho) * (J - alpha^2 r * r' J)
        let r_rtj = residuals.clone() * residuals.transpose() * jacobian.clone();
        *jacobian = (jacobian.clone() - r_rtj.mul(self.alpha_sq_norm)).mul(self.sqrt_rho1);
    }
    pub fn correct_residuals(&self, residuals: &mut na::DVector<f64>) {
        *residuals = residuals.clone().mul(self.residual_scaling);
    }
}