tiny-solver 0.18.0

Factor graph solver
Documentation 
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
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66

use std::fmt::Debug;
use std::ops::Mul;

use faer::linalg::solvers::Solve;
use faer::sparse::linalg::solvers;

use super::sparse::SparseLinearSolver;

// #[pyclass]
#[derive(Debug, Clone)]
pub struct SparseCholeskySolver {
    symbolic_pattern: Option<solvers::SymbolicLlt<usize>>,
}

impl SparseCholeskySolver {
    pub fn new() -> Self {
        SparseCholeskySolver {
            symbolic_pattern: None,
        }
    }
}
impl Default for SparseCholeskySolver {
    fn default() -> Self {
        Self::new()
    }
}
impl SparseLinearSolver for SparseCholeskySolver {
    fn solve(
        &mut self,
        residuals: &faer::Mat<f64>,
        jacobians: &faer::sparse::SparseColMat<usize, f64>,
    ) -> Option<faer::Mat<f64>> {
        let jtj = jacobians
            .as_ref()
            .transpose()
            .to_col_major()
            .unwrap()
            .mul(jacobians.as_ref());
        let jtr = jacobians.as_ref().transpose().mul(-residuals);

        self.solve_jtj(&jtr, &jtj)
    }

    fn solve_jtj(
        &mut self,
        jtr: &faer::Mat<f64>,
        jtj: &faer::sparse::SparseColMat<usize, f64>,
    ) -> Option<faer::Mat<f64>> {
        // initialize the pattern
        if self.symbolic_pattern.is_none() {
            self.symbolic_pattern =
                Some(solvers::SymbolicLlt::try_new(jtj.symbolic(), faer::Side::Lower).unwrap());
        }

        let sym = self.symbolic_pattern.as_ref().unwrap();
        if let Ok(cholesky) =
            solvers::Llt::try_new_with_symbolic(sym.clone(), jtj.as_ref(), faer::Side::Lower)
        {
            let dx = cholesky.solve(jtr);

            Some(dx)
        } else {
            None
        }
    }
}