tiny_solver/
helper.rs

1use std::collections::HashMap;
2use std::fs::read_to_string;
3use std::sync::Arc;
4
5use nalgebra as na;
6
7use crate::loss_functions::HuberLoss;
8use crate::manifold::se3::SE3Manifold;
9use crate::{factors, problem};
10
11pub fn translation_quaternion_to_na<T: na::RealField>(
12    tx: &T,
13    ty: &T,
14    tz: &T,
15    qx: &T,
16    qy: &T,
17    qz: &T,
18    qw: &T,
19) -> na::Isometry3<T> {
20    let rotation = na::UnitQuaternion::from_quaternion(na::Quaternion::new(
21        qw.clone(),
22        qx.clone(),
23        qy.clone(),
24        qz.clone(),
25    ));
26    na::Isometry3::from_parts(
27        na::Translation3::new(tx.clone(), ty.clone(), tz.clone()),
28        rotation,
29    )
30}
31
32pub fn read_g2o(filename: &str) -> (problem::Problem, HashMap<String, na::DVector<f64>>) {
33    let mut problem = problem::Problem::new();
34    let mut init_values = HashMap::<String, na::DVector<f64>>::new();
35    for line in read_to_string(filename).unwrap().lines() {
36        let line: Vec<&str> = line.split(' ').collect();
37        match line[0] {
38            "VERTEX_SE2" => {
39                let x = line[2].parse::<f64>().unwrap();
40                let y = line[3].parse::<f64>().unwrap();
41                let theta = line[4].parse::<f64>().unwrap();
42                init_values.insert(format!("x{}", line[1]), na::dvector![theta, x, y]);
43            }
44            "EDGE_SE2" => {
45                let id0 = format!("x{}", line[1]);
46                let id1 = format!("x{}", line[2]);
47                let dx = line[3].parse::<f64>().unwrap();
48                let dy = line[4].parse::<f64>().unwrap();
49                let dtheta = line[5].parse::<f64>().unwrap();
50                // todo add info matrix
51                let edge = factors::BetweenFactorSE2 { dx, dy, dtheta };
52                problem.add_residual_block(
53                    3,
54                    &[&id0, &id1],
55                    Box::new(edge),
56                    Some(Box::new(HuberLoss::new(1.0))),
57                );
58            }
59            "VERTEX_SE3:QUAT" => {
60                let x = line[2].parse::<f64>().expect("Failed to parse g2o");
61                let y = line[3].parse::<f64>().expect("Failed to parse g2o");
62                let z = line[4].parse::<f64>().expect("Failed to parse g2o");
63                let qx = line[5].parse::<f64>().expect("Failed to parse g2o");
64                let qy = line[6].parse::<f64>().expect("Failed to parse g2o");
65                let qz = line[7].parse::<f64>().expect("Failed to parse g2o");
66                let qw = line[8].parse::<f64>().expect("Failed to parse g2o");
67                let var_name = format!("x{}", line[1]);
68                problem.set_variable_manifold(&var_name, Arc::new(SE3Manifold));
69                init_values.insert(var_name, na::dvector![qx, qy, qz, qw, x, y, z]);
70            }
71            "EDGE_SE3:QUAT" => {
72                let id0 = format!("x{}", line[1]);
73                let id1 = format!("x{}", line[2]);
74                let dtx = line[3].parse::<f64>().expect("Failed to parse g2o");
75                let dty = line[4].parse::<f64>().expect("Failed to parse g2o");
76                let dtz = line[5].parse::<f64>().expect("Failed to parse g2o");
77                let dqx = line[6].parse::<f64>().expect("Failed to parse g2o");
78                let dqy = line[7].parse::<f64>().expect("Failed to parse g2o");
79                let dqz = line[8].parse::<f64>().expect("Failed to parse g2o");
80                let dqw = line[9].parse::<f64>().expect("Failed to parse g2o");
81                let edge = factors::BetweenFactorSE3 {
82                    dtx,
83                    dty,
84                    dtz,
85                    dqx,
86                    dqy,
87                    dqz,
88                    dqw,
89                };
90                problem.add_residual_block(
91                    6,
92                    &[&id0, &id1],
93                    Box::new(edge),
94                    Some(Box::new(HuberLoss::new(1.0))),
95                );
96            }
97            _ => {
98                println!("err");
99                break;
100            }
101        }
102    }
103    let x0 = init_values.get("x0").unwrap();
104    let origin_factor = factors::PriorFactor { v: x0.clone() };
105    problem.add_residual_block(
106        x0.shape().0,
107        &["x0"],
108        Box::new(origin_factor),
109        Some(Box::new(HuberLoss::new(1.0))),
110    );
111    (problem, init_values)
112}
tiny_solver/helper.rs

tiny_solver/
helper.rs
