robotics/mapping/
pose_graph_optimization.rs

1#![allow(non_snake_case)]
2#![allow(non_camel_case_types)]
3#![allow(clippy::deprecated_cfg_attr)]
4use nalgebra::{
5    DVector, Isometry2, Matrix2, Matrix2x3, Matrix3, SMatrix, SVector, Translation2,
6    Vector2, Vector3, UnitComplex,
7};
8use plotpy::{Curve, Plot};
9use russell_lab::Vector;
10use russell_sparse::{ConfigSolver, Solver, SparseTriplet, Symmetry};
11use rustc_hash::FxHashMap;
12use std::error::Error;
13
14#[derive(Debug)]
15pub enum Edge<T> {
16    SE2(EdgeSE2<T>),
17    SE2_XY(EdgeSE2_XY<T>),
18    SE3,
19    SE3_XYZ
20}
21
22#[derive(Debug)]
23pub struct EdgeSE2<T> {
24    from: u32,
25    to: u32,
26    measurement: Isometry2<T>,
27    information: Matrix3<T>,
28}
29
30impl<T> EdgeSE2<T> {
31    pub fn new(
32        from: u32,
33        to: u32,
34        measurement: Isometry2<T>,
35        information: Matrix3<T>,
36    ) -> EdgeSE2<T> {
37        EdgeSE2 {
38            from,
39            to,
40            measurement,
41            information,
42        }
43    }
44}
45
46#[derive(Debug)]
47pub struct EdgeSE2_XY<T> {
48    from: u32,
49    to: u32,
50    measurement: Vector2<T>,
51    information: Matrix2<T>,
52}
53
54impl<T> EdgeSE2_XY<T> {
55    pub fn new(
56        from: u32,
57        to: u32,
58        measurement: Vector2<T>,
59        information: Matrix2<T>,
60    ) -> EdgeSE2_XY<T> {
61        EdgeSE2_XY {
62            from,
63            to,
64            measurement,
65            information,
66        }
67    }
68}
69
70#[derive(PartialEq)]
71pub enum Node {
72    SE2,
73    SE3,
74    XY,
75    XYZ
76}
77pub struct PoseGraph {
78    x: DVector<f64>,
79    nodes: FxHashMap<u32, Node>,
80    edges: Vec<Edge<f64>>,
81    lut: FxHashMap<u32, usize>,
82    iteration: usize,
83    name: String,
84}
85
86#[allow(clippy::too_many_arguments)]
87fn update_linear_system<const X1: usize, const X2: usize>(
88    H: &mut SparseTriplet,
89    b: &mut Vector,
90    e: &SVector<f64, X2>,
91    A: &SMatrix<f64, X2, X1>,
92    B: &SMatrix<f64, X2, X2>,
93    omega: &SMatrix<f64, X2, X2>,
94    from: usize,
95    to: usize,
96) -> Result<(), Box<dyn Error>> {
97    // Compute jacobians
98    let H_ii = A.transpose() * omega * A;
99    let H_ij = A.transpose() * omega * B;
100    let H_ji = H_ij.transpose();
101    let H_jj = B.transpose() * omega * B;
102
103    let b_i = A.transpose() * omega * e;
104    let b_j = B.transpose() * omega * e;
105
106    // Update the linear system
107    set_matrix(H, from, from, &H_ii)?;
108    set_matrix(H, from, to, &H_ij)?;
109    set_matrix(H, to, from, &H_ji)?;
110    set_matrix(H, to, to, &H_jj)?;
111
112    set_vector(b, from, &b_i);
113    set_vector(b, to, &b_j);
114    Ok(())
115}
116
117fn set_matrix<const R: usize, const C: usize>(
118    trip: &mut SparseTriplet,
119    i: usize,
120    j: usize,
121    m: &SMatrix<f64, R, C>,
122) -> Result<(), Box<dyn Error>> {
123    for ii in 0..R {
124        for jj in 0..C {
125            trip.put(i + ii, j + jj, m.fixed_view::<1, 1>(ii, jj).x)?
126        }
127    }
128    Ok(())
129}
130
131fn set_vector<const D: usize>(v: &mut Vector, i: usize, source: &SVector<f64, D>) {
132    for ii in 0..D {
133        v.set(i + ii, source.fixed_view::<1, 1>(ii, 0).x + v.get(i + ii))
134    }
135}
136
137impl PoseGraph {
138    pub fn new(
139        x: DVector<f64>,
140        nodes: FxHashMap<u32, Node>,
141        edges: Vec<Edge<f64>>,
142        lut: FxHashMap<u32, usize>,
143        name: String,
144    ) -> PoseGraph {
145        PoseGraph {
146            x,
147            nodes,
148            edges,
149            lut,
150            iteration: 0,
151            name,
152        }
153    }
154
155    pub fn from_g2o(filename: &str) -> Result<PoseGraph, Box<dyn Error>> {
156        let mut edges = Vec::new();
157        let mut lut = FxHashMap::default();
158        let mut nodes = FxHashMap::default();
159        let mut offset = 0;
160        let mut X = Vec::new();
161
162        for line in std::fs::read_to_string(filename)?.lines() {
163            let line: Vec<&str> = line.split(' ').collect();
164            match line[0] {
165                "VERTEX_SE2" => {
166                    let id = line[1].parse::<u32>()?;
167                    let x = line[2].parse::<f64>()?;
168                    let y = line[3].parse::<f64>()?;
169                    let angle = line[4].parse::<f64>()?;
170                    nodes.insert(id, Node::SE2);
171                    lut.insert(id, offset);
172                    offset += 3;
173                    X.extend_from_slice(&[x, y, angle]);
174                }
175                "VERTEX_XY" => {
176                    let id = line[1].parse::<u32>()?;
177                    let x = line[2].parse::<f64>()?;
178                    let y = line[3].parse::<f64>()?;
179                    nodes.insert(id, Node::XY);
180                    lut.insert(id, offset);
181                    offset += 2;
182                    X.extend_from_slice(&[x, y]);
183                }
184                "VERTEX_SE3:QUAT" => {
185                    // let id = line[1].parse::<u32>()?;
186                    // let x = line[2].parse::<f64>()?;
187                    // let y = line[3].parse::<f64>()?;
188                    // let z = line[4].parse::<f64>()?;
189                    // let qx = line[5].parse::<f64>()?;
190                    // let qy = line[6].parse::<f64>()?;
191                    // let qz = line[7].parse::<f64>()?;
192                    // let qw = line[8].parse::<f64>()?;
193                    unimplemented!("VERTEX_SE3:QUAT")
194                }
195                "EDGE_SE2" => {
196                    let from = line[1].parse::<u32>()?;
197                    let to = line[2].parse::<u32>()?;
198                    let x = line[3].parse::<f64>()?;
199                    let y = line[4].parse::<f64>()?;
200                    let angle = line[5].parse::<f64>()?;
201                    let tri_0 = line[6].parse::<f64>()?;
202                    let tri_1 = line[7].parse::<f64>()?;
203                    let tri_2 = line[8].parse::<f64>()?;
204                    let tri_3 = line[9].parse::<f64>()?;
205                    let tri_4 = line[10].parse::<f64>()?;
206                    let tri_5 = line[11].parse::<f64>()?;
207
208                    let translation = Translation2::new(x, y);
209                    let rotation = UnitComplex::from_angle(angle);
210                    let measurement = Isometry2::from_parts(translation, rotation);
211
212                    #[allow(clippy::deprecated_cfg_attr)]
213                    #[cfg_attr(rustfmt, rustfmt_skip)]
214                    let information = Matrix3::new(
215                        tri_0, tri_1, tri_2,
216                        tri_1, tri_3, tri_4,
217                        tri_2, tri_4, tri_5
218                    );
219                    let edge = Edge::SE2(EdgeSE2::new(from, to, measurement, information));
220                    edges.push(edge);
221                }
222                "EDGE_SE2_XY" => {
223                    let from = line[1].parse::<u32>()?;
224                    let to = line[2].parse::<u32>()?;
225                    let x = line[3].parse::<f64>()?;
226                    let y = line[4].parse::<f64>()?;
227                    let tri_0 = line[5].parse::<f64>()?;
228                    let tri_1 = line[6].parse::<f64>()?;
229                    let tri_2 = line[7].parse::<f64>()?;
230
231                    let measurement = Vector2::new(x, y);
232
233                    #[allow(clippy::deprecated_cfg_attr)]
234                    #[cfg_attr(rustfmt, rustfmt_skip)]
235                    let information = Matrix2::new(
236                        tri_0, tri_1,
237                        tri_1, tri_2
238                    );
239                    let edge = Edge::SE2_XY(EdgeSE2_XY::new(from, to, measurement, information));
240                    edges.push(edge);
241                }
242                "EDGE_SE3:QUAT" => {
243                    unimplemented!("EDGE_SE3:QUAT")
244                }
245                _ => unimplemented!("{}", line[0]),
246            }
247        }
248        let name = filename
249            .split('/')
250            .last()
251            .unwrap()
252            .split('.')
253            .next()
254            .unwrap()
255            .to_string();
256        Ok(PoseGraph::new(
257            DVector::from_vec(X),
258            nodes,
259            edges,
260            lut,
261            name,
262        ))
263    }
264
265    pub fn optimize(
266        &mut self,
267        num_iterations: usize,
268        log: bool,
269        plot: bool,
270    ) -> Result<(), Box<dyn Error>> {
271        let tolerance = 1e-4;
272        let mut norms = Vec::new();
273        let mut errors = vec![compute_global_error(self)];
274        if log {
275            println!(
276                "Loaded graph with {} nodes and {} edges",
277                self.nodes.len(),
278                self.edges.len()
279            );
280            println!("initial error :{:.5}", errors.last().unwrap());
281        }
282        if plot {
283            self.plot()?;
284        }
285        for i in 0..num_iterations {
286            self.iteration += 1;
287            let dx = self.linearize_and_solve()?;
288            self.x += &dx;
289            let norm_dx = dx.norm();
290            norms.push(norm_dx);
291            errors.push(compute_global_error(self));
292
293            if log {
294                println!(
295                    "step {i:3} : |dx| = {norm_dx:3.5}, error = {:3.5}",
296                    errors.last().unwrap()
297                );
298            }
299            if plot {
300                self.plot()?;
301            }
302
303            if norm_dx < tolerance {
304                break;
305            }
306        }
307        Ok(())
308    }
309
310    fn linearize_and_solve(&self) -> Result<DVector<f64>, Box<dyn Error>> {
311        let n = self.x.shape().0;
312
313        let mut H = SparseTriplet::new(n, n, n * n, Symmetry::General)?;
314        let mut b = Vector::new(n);
315
316        let mut need_to_add_prior = true;
317
318        // make linear system
319        for edge in &self.edges {
320            match edge {
321                Edge::SE2(edge) => {
322                    let from_idx = *self.lut.get(&edge.from).unwrap();
323                    let to_idx = *self.lut.get(&edge.to).unwrap();
324
325                    let x1 = isometry(&self.x, from_idx);
326                    let x2 = isometry(&self.x, to_idx);
327
328                    let z = edge.measurement;
329                    let omega = edge.information;
330
331                    let e = pose_pose_constraint(&x1, &x2, &z);
332                    let (A, B) = linearize_pose_pose_constraint(&x1, &x2, &z);
333
334                    update_linear_system(&mut H, &mut b, &e, &A, &B, &omega, from_idx, to_idx)?;
335
336                    if need_to_add_prior {
337                        H.put(from_idx, from_idx, 1000.0)?;
338                        H.put(from_idx + 1, from_idx + 1, 1000.0)?;
339                        H.put(from_idx + 2, from_idx + 2, 1000.0)?;
340
341                        need_to_add_prior = false;
342                    }
343                }
344                Edge::SE2_XY(edge) => {
345                    let from_idx = *self.lut.get(&edge.from).unwrap();
346                    let to_idx = *self.lut.get(&edge.to).unwrap();
347
348                    let x = isometry(&self.x, from_idx);
349                    let landmark = self.x.fixed_rows::<2>(to_idx).into();
350
351                    let z = edge.measurement;
352                    let omega = edge.information;
353
354                    let e = pose_landmark_constraint(&x, &landmark, &z);
355                    let (A, B) = linearize_pose_landmark_constraint(&x, &landmark);
356
357                    update_linear_system(&mut H, &mut b, &e, &A, &B, &omega, from_idx, to_idx)?;
358                }
359                Edge::SE3 => todo!(),
360                Edge::SE3_XYZ => todo!()
361            }
362        }
363        // Use Russell Sparse because it is much faster then nalgebra_sparse
364        b.map(|x| -x);
365        let mut solution = Vector::new(n);
366        let config = ConfigSolver::new();
367        let mut solver = Solver::new(config)?;
368        solver.initialize(&H)?;
369        solver.factorize()?;
370        solver.solve(&mut solution, &b)?;
371        Ok(DVector::from_vec(solution.as_data().clone()))
372    }
373
374    pub fn plot(&self) -> Result<(), Box<dyn Error>> {
375        // poses + landarks
376        let mut landmarks_present = false;
377        let mut poses_seq = Vec::new();
378        let mut poses = Curve::new();
379        poses
380            .set_line_style("None")
381            .set_marker_color("b")
382            .set_marker_style("o");
383
384        let mut landmarks = Curve::new();
385        landmarks
386            .set_line_style("None")
387            .set_marker_color("r")
388            .set_marker_style("*");
389        poses.points_begin();
390        landmarks.points_begin();
391        for (id, node) in self.nodes.iter() {
392            let idx = *self.lut.get(id).unwrap();
393            let xy = self.x.fixed_rows::<2>(idx);
394            match *node {
395                Node::SE2 => {
396                    poses.points_add(xy.x, xy.y);
397                    poses_seq.push((id, xy));
398                }
399                Node::XY => {
400                    landmarks.points_add(xy.x, xy.y);
401                    landmarks_present = true;
402                }
403                Node::SE3 => todo!(),
404                Node::XYZ => todo!()
405            }
406        }
407        poses.points_end();
408        landmarks.points_end();
409
410        poses_seq.sort_by(|a, b| (a.0).partial_cmp(b.0).unwrap());
411        let mut poses_seq_curve = Curve::new();
412        poses_seq_curve.set_line_color("r");
413
414        poses_seq_curve.points_begin();
415        for (_, p) in poses_seq {
416            poses_seq_curve.points_add(p.x, p.y);
417        }
418        poses_seq_curve.points_end();
419
420        // add features to plot
421        let mut plot = Plot::new();
422
423        plot.add(&poses).add(&poses_seq_curve);
424        if landmarks_present {
425            plot.add(&landmarks);
426        }
427        // save figure
428        plot.set_equal_axes(true)
429            // .set_figure_size_points(600.0, 600.0)
430            .save(format!("img/{}-{}.svg", self.name, self.iteration).as_str())?;
431        Ok(())
432    }
433}
434
435fn pose_pose_constraint(
436    x1: &Isometry2<f64>,
437    x2: &Isometry2<f64>,
438    z: &Isometry2<f64>,
439) -> Vector3<f64> {
440    let constraint = z.inverse() * x1.inverse() * x2;
441    Vector3::new(
442        constraint.translation.x,
443        constraint.translation.y,
444        constraint.rotation.angle(),
445    )
446}
447
448fn pose_landmark_constraint(
449    x: &Isometry2<f64>,
450    landmark: &Vector2<f64>,
451    z: &Vector2<f64>,
452) -> Vector2<f64> {
453    x.rotation.to_rotation_matrix().transpose() * (landmark - x.translation.vector) - z
454}
455
456fn linearize_pose_pose_constraint(
457    x1: &Isometry2<f64>,
458    x2: &Isometry2<f64>,
459    z: &Isometry2<f64>,
460) -> (Matrix3<f64>, Matrix3<f64>) {
461    let deriv = Matrix2::<f64>::new(0.0, -1.0, 1.0, 0.0);
462
463    let zr = z.rotation.to_rotation_matrix();
464    let x1r = x1.rotation.to_rotation_matrix();
465    let a_11 = -(zr.clone().inverse() * x1r.clone().inverse()).matrix();
466    let xr1d = deriv * x1.rotation.to_rotation_matrix().matrix();
467    let a_12 =
468        zr.clone().transpose() * xr1d.transpose() * (x2.translation.vector - x1.translation.vector);
469
470    #[cfg_attr(rustfmt, rustfmt_skip)]
471    let A = Matrix3::new(
472        a_11.m11, a_11.m12, a_12.x,
473        a_11.m21, a_11.m22, a_12.y,
474        0.0, 0.0, -1.0,
475    );
476
477    let b_11 = (zr.inverse() * x1r.inverse()).matrix().to_owned();
478    #[cfg_attr(rustfmt, rustfmt_skip)]
479    let B = Matrix3::new(
480        b_11.m11, b_11.m12, 0.0,
481        b_11.m21, b_11.m22, 0.0,
482        0.0, 0.0, 1.0,
483    );
484    (A, B)
485}
486
487fn linearize_pose_landmark_constraint(
488    x: &Isometry2<f64>,
489    landmark: &Vector2<f64>,
490) -> (Matrix2x3<f64>, Matrix2<f64>) {
491    let deriv = Matrix2::<f64>::new(0.0, -1.0, 1.0, 0.0);
492
493    let a_1 = -x.rotation.to_rotation_matrix().transpose().matrix();
494    let xrd = deriv * *x.rotation.to_rotation_matrix().matrix();
495    let a_2 = xrd.transpose() * (landmark - x.translation.vector);
496
497    #[cfg_attr(rustfmt, rustfmt_skip)]
498    let A = Matrix2x3::new(
499        a_1.m11, a_1.m12, a_2.x,
500        a_1.m21, a_1.m22, a_2.y,
501    );
502
503    let B = *x.rotation.to_rotation_matrix().transpose().matrix();
504
505    (A, B)
506}
507
508fn isometry(x: &DVector<f64>, idx: usize) -> Isometry2<f64> {
509    let p = x.fixed_rows::<3>(idx);
510    Isometry2::from_parts(Translation2::new(p.x, p.y), UnitComplex::from_angle(p.z))
511}
512
513fn compute_global_error(graph: &PoseGraph) -> f64 {
514    let mut error = 0.0;
515    for edge in &graph.edges {
516        match edge {
517            Edge::SE2(e) => {
518                let from_idx = *graph.lut.get(&e.from).unwrap();
519                let to_idx = *graph.lut.get(&e.to).unwrap();
520
521                let x1 = isometry(&graph.x, from_idx);
522                let x2 = isometry(&graph.x, to_idx);
523
524                let z = e.measurement;
525
526                error += pose_pose_constraint(&x1, &x2, &z).norm();
527            }
528            Edge::SE2_XY(e) => {
529                let from_idx = *graph.lut.get(&e.from).unwrap();
530                let to_idx = *graph.lut.get(&e.to).unwrap();
531
532                // TODO: change to only read the rotation, dont use translation anyway
533                let x = isometry(&graph.x, from_idx);
534                let l = graph.x.fixed_rows::<2>(to_idx);
535                let z = e.measurement;
536
537                error += pose_landmark_constraint(&x, &l.into(), &z).norm();
538            }
539            Edge::SE3 => todo!(),
540            Edge::SE3_XYZ => todo!()
541        }
542    }
543    error
544}
545
546#[cfg(test)]
547mod tests {
548    use super::*;
549
550    #[test]
551    fn read_g2o_file_runs() -> Result<(), Box<dyn Error>> {
552        let filename = "dataset/g2o/simulation-pose-pose.g2o";
553        let graph = PoseGraph::from_g2o(filename)?;
554        assert_eq!(400, graph.nodes.len());
555        assert_eq!(1773, graph.edges.len());
556        assert_eq!(1200, graph.x.shape().0);
557
558        let filename = "dataset/g2o/simulation-pose-landmark.g2o";
559        let graph = PoseGraph::from_g2o(filename)?;
560        assert_eq!(77, graph.nodes.len());
561        assert_eq!(297, graph.edges.len());
562        assert_eq!(195, graph.x.shape().0);
563
564        let filename = "dataset/g2o/intel.g2o";
565        let graph = PoseGraph::from_g2o(filename)?;
566        assert_eq!(1728, graph.nodes.len());
567        assert_eq!(4830, graph.edges.len());
568        assert_eq!(5184, graph.x.shape().0);
569
570        let filename = "dataset/g2o/dlr.g2o";
571        let graph = PoseGraph::from_g2o(filename)?;
572        assert_eq!(3873, graph.nodes.len());
573        assert_eq!(17605, graph.edges.len());
574        assert_eq!(11043, graph.x.shape().0);
575        Ok(())
576    }
577
578    #[test]
579    fn compute_global_error_correct_pose_pose() -> Result<(), Box<dyn Error>> {
580        let filename = "dataset/g2o/simulation-pose-pose.g2o";
581        let graph = PoseGraph::from_g2o(filename)?;
582        approx::assert_abs_diff_eq!(3558.0723, compute_global_error(&graph), epsilon = 1e-2);
583
584        Ok(())
585    }
586
587    #[test]
588    fn compute_global_error_correct_pose_landmark() -> Result<(), Box<dyn Error>> {
589        let filename = "dataset/g2o/simulation-pose-landmark.g2o";
590        let graph = PoseGraph::from_g2o(filename)?;
591        approx::assert_abs_diff_eq!(72.50542, compute_global_error(&graph), epsilon = 1e-2);
592
593        Ok(())
594    }
595
596    #[test]
597    fn compute_global_error_correct_intel() -> Result<(), Box<dyn Error>> {
598        let filename = "dataset/g2o/intel.g2o";
599        let graph = PoseGraph::from_g2o(filename)?;
600        approx::assert_abs_diff_eq!(6109.409, compute_global_error(&graph), epsilon = 1e-2);
601        Ok(())
602    }
603
604    #[test]
605    fn compute_global_error_correct_dlr() -> Result<(), Box<dyn Error>> {
606        let filename = "dataset/g2o/dlr.g2o";
607        let graph = PoseGraph::from_g2o(filename)?;
608        approx::assert_abs_diff_eq!(37338.21, compute_global_error(&graph), epsilon = 1e-2);
609        Ok(())
610    }
611
612    #[test]
613    fn linearize_pose_pose_constraint_correct() -> Result<(), Box<dyn Error>> {
614        let filename = "dataset/g2o/simulation-pose-landmark.g2o";
615        let graph = PoseGraph::from_g2o(filename)?;
616
617        match &graph.edges[0] {
618            Edge::SE2(e) => {
619                let from_idx = *graph.lut.get(&e.from).unwrap();
620                let to_idx = *graph.lut.get(&e.to).unwrap();
621
622                let x1 = isometry(&graph.x, from_idx);
623                let x2 = isometry(&graph.x, to_idx);
624
625                let z = e.measurement;
626
627                let e = pose_pose_constraint(&x1, &x2, &z);
628                let (A, B) = linearize_pose_pose_constraint(&x1, &x2, &z);
629
630                let A_expected = Matrix3::new(0.0, 1.0, 0.113, -1., 0., 0.024, 0., 0., -1.);
631
632                let B_expected = Matrix3::new(0.0, -1.0, 0.0, 1., 0., 0.0, 0., 0., 1.);
633
634                approx::assert_abs_diff_eq!(A_expected, A, epsilon = 1e-3);
635                approx::assert_abs_diff_eq!(B_expected, B, epsilon = 1e-3);
636                approx::assert_abs_diff_eq!(Vector3::zeros(), e, epsilon = 1e-3);
637            }
638            _ => panic!(),
639        }
640
641        match &graph.edges[10] {
642            Edge::SE2(e) => {
643                let from_idx = *graph.lut.get(&e.from).unwrap();
644                let to_idx = *graph.lut.get(&e.to).unwrap();
645
646                let x1 = isometry(&graph.x, from_idx);
647                let x2 = isometry(&graph.x, to_idx);
648
649                let z = e.measurement;
650
651                let e = pose_pose_constraint(&x1, &x2, &z);
652                let (A, B) = linearize_pose_pose_constraint(&x1, &x2, &z);
653
654                let A_expected =
655                    Matrix3::new(0.037, 0.999, 0.138, -0.999, 0.037, -0.982, 0., 0., -1.);
656
657                let B_expected = Matrix3::new(-0.037, -0.999, 0.0, 0.999, -0.037, 0.0, 0., 0., 1.);
658
659                approx::assert_abs_diff_eq!(A_expected, A, epsilon = 1e-3);
660                approx::assert_abs_diff_eq!(B_expected, B, epsilon = 1e-3);
661                approx::assert_abs_diff_eq!(Vector3::zeros(), e, epsilon = 1e-3);
662            }
663            _ => panic!(),
664        }
665
666        Ok(())
667    }
668
669    #[test]
670    fn linearize_pose_landmark_constraint_correct() -> Result<(), Box<dyn Error>> {
671        let filename = "dataset/g2o/simulation-pose-landmark.g2o";
672        let graph = PoseGraph::from_g2o(filename)?;
673
674        match &graph.edges[1] {
675            Edge::SE2_XY(edge) => {
676                let from_idx = *graph.lut.get(&edge.from).unwrap();
677                let to_idx = *graph.lut.get(&edge.to).unwrap();
678
679                let x = isometry(&graph.x, from_idx);
680                let landmark = graph.x.fixed_rows::<2>(to_idx);
681                let z = edge.measurement;
682
683                let e = pose_landmark_constraint(&x, &landmark.into(), &z);
684                let (A, B) = linearize_pose_landmark_constraint(&x, &landmark.into());
685
686                let A_expected = Matrix2x3::new(0.0, 1.0, 0.358, -1., 0., -0.051);
687
688                let B_expected = Matrix2::new(0.0, -1.0, 1., 0.);
689
690                approx::assert_abs_diff_eq!(A_expected, A, epsilon = 1e-3);
691                approx::assert_abs_diff_eq!(B_expected, B, epsilon = 1e-3);
692                approx::assert_abs_diff_eq!(Vector2::zeros(), e, epsilon = 1e-3);
693            }
694            _ => panic!()
695        }
696        Ok(())
697    }
698
699    #[test]
700    fn linearize_and_solve_correct() -> Result<(), Box<dyn Error>> {
701        let filename = "dataset/g2o/simulation-pose-landmark.g2o";
702        let graph = PoseGraph::from_g2o(filename)?;
703        let dx = graph.linearize_and_solve()?;
704        let expected_first_5 = DVector::<f64>::from_vec(vec![
705            1.68518905e-01,
706            5.74311089e-01,
707            -5.08805168e-02,
708            -3.67482151e-02,
709            8.89458085e-01,
710        ]);
711        let first_5 = dx.rows(0, 5).clone_owned();
712        approx::assert_abs_diff_eq!(expected_first_5, first_5, epsilon = 1e-3);
713        Ok(())
714    }
715}
robotics/mapping/pose_graph_optimization.rs

robotics/mapping/
pose_graph_optimization.rs
