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

use std::ops::Mul;

use faer::prelude::SpSolver;

pub fn sparse_cholesky(
    residuals: &faer::Mat<f64>,
    jacobians: &faer::sparse::SparseColMat<usize, f64>,
) -> faer::prelude::Mat<f64> {
    let hessian = jacobians
        .as_ref()
        .transpose()
        .to_col_major()
        .unwrap()
        .mul(jacobians.as_ref());
    let b = jacobians.as_ref().transpose().mul(-residuals);
    let dx = hessian.sp_cholesky(faer::Side::Lower).unwrap().solve(b);
    dx
}

pub fn sparse_qr(
    residuals: &faer::Mat<f64>,
    jacobians: &faer::sparse::SparseColMat<usize, f64>,
) -> faer::prelude::Mat<f64> {
    let dx = jacobians.sp_qr().unwrap().solve(-residuals);
    dx
}