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
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128

use std::collections::{HashMap, HashSet};
use std::ops::Add;

use nalgebra as na;

use crate::parameter_block::ParameterBlock;
use crate::problem;
use crate::sparse::LinearSolverType;

pub trait Optimizer {
    fn optimize(
        &self,
        problem: &problem::Problem,
        initial_values: &HashMap<String, na::DVector<f64>>,
        optimizer_option: Option<OptimizerOptions>,
    ) -> Option<HashMap<String, na::DVector<f64>>>;
    fn apply_dx(
        &self,
        dx: &na::DVector<f64>,
        params: &mut HashMap<String, na::DVector<f64>>,
        variable_name_to_col_idx_dict: &HashMap<String, usize>,
        fixed_var_indexes: &HashMap<String, HashSet<usize>>,
        variable_bounds: &HashMap<String, HashMap<usize, (f64, f64)>>,
    ) {
        for (key, param) in params.iter_mut() {
            if let Some(col_idx) = variable_name_to_col_idx_dict.get(key) {
                let var_size = param.shape().0;
                let mut updated_param = param.clone().add(dx.rows(*col_idx, var_size));
                if let Some(indexes_to_fix) = fixed_var_indexes.get(key) {
                    for &idx in indexes_to_fix {
                        log::debug!("Fix {} {}", key, idx);
                        updated_param[idx] = param[idx];
                    }
                }
                if let Some(indexes_to_bound) = variable_bounds.get(key) {
                    for (&idx, &(lower, upper)) in indexes_to_bound {
                        let old = updated_param[idx];
                        updated_param[idx] = updated_param[idx].max(lower).min(upper);
                        log::debug!("bound {} {} {} -> {}", key, idx, old, updated_param[idx]);
                    }
                }
                param.copy_from(&updated_param);
            }
        }
    }
    fn apply_dx2(
        &self,
        dx: &na::DVector<f64>,
        params: &mut HashMap<String, ParameterBlock>,
        variable_name_to_col_idx_dict: &HashMap<String, usize>,
    ) {
        params.iter_mut().for_each(|(key, param)| {
            if let Some(col_idx) = variable_name_to_col_idx_dict.get(key) {
                let tangent_size = param.tangent_size();
                let effective_size = if param.manifold.is_some() {
                    tangent_size
                } else {
                    tangent_size - param.fixed_variables.len()
                };

                let dx_reduced = dx.rows(*col_idx, effective_size);

                let mut dx_full = na::DVector::zeros(tangent_size);
                if param.manifold.is_some() {
                    dx_full.copy_from(&dx_reduced);
                } else {
                    let mut reduced_idx = 0;
                    for i in 0..tangent_size {
                        if !param.fixed_variables.contains(&i) {
                            dx_full[i] = dx_reduced[reduced_idx];
                            reduced_idx += 1;
                        }
                    }
                }
                param.update_params(param.plus_f64(dx_full.rows(0, tangent_size)));
            }
        });
        // for (key, param) in params.par_iter_mut() {
        // }
    }
    fn compute_error(
        &self,
        problem: &problem::Problem,
        params: &HashMap<String, ParameterBlock>,
    ) -> f64 {
        problem
            .compute_residuals(params, true)
            .as_ref()
            .squared_norm_l2()
    }
}

#[derive(PartialEq, Debug)]
pub enum SolverStatus {
    Running,
    // Resulting solution may be OK to use.
    GradientTooSmall,         // eps > max(J'*f(x))
    RelativeStepSizeTooSmall, // eps > ||dx|| / ||x||
    ErrorTooSmall,            // eps > ||f(x)||
    HitMaxIterations,
    // Numerical issues
    // FAILED_TO_EVALUATE_COST_FUNCTION,
    // FAILED_TO_SOLVER_LINEAR_SYSTEM,
}

#[derive(Clone)]
pub struct OptimizerOptions {
    pub max_iteration: usize,
    pub linear_solver_type: LinearSolverType,
    pub verbosity_level: usize,
    pub min_abs_error_decrease_threshold: f64,
    pub min_rel_error_decrease_threshold: f64,
    pub min_error_threshold: f64,
    // pub relative_step_threshold: 1e-16,
}

impl Default for OptimizerOptions {
    fn default() -> Self {
        OptimizerOptions {
            max_iteration: 100,
            linear_solver_type: LinearSolverType::SparseCholesky,
            verbosity_level: 0,
            min_abs_error_decrease_threshold: 1e-5,
            min_rel_error_decrease_threshold: 1e-5,
            min_error_threshold: 1e-10,
        }
    }
}