Skip to main content

rust_robotics_slam/
pose_graph_optimization.rs

1//! 2D pose graph optimization with Gauss-Newton iteration.
2//!
3//! The optimizer refines a set of initial poses from relative pose
4//! constraints represented as edges in an SE(2) graph.
5//!
6//! Reference:
7//! - Giorgio Grisetti, Rainer Kümmerle, Cyrill Stachniss, Wolfram Burgard,
8//!   "A Tutorial on Graph-Based SLAM":
9//!   <https://www.ipb.uni-bonn.de/wp-content/papercite-data/pdf/grisetti10titsmag.pdf>
10
11use nalgebra::{DMatrix, DVector, Matrix2, Matrix3, Vector2, Vector3};
12use rust_robotics_core::normalize_angle;
13use rust_robotics_optimization::{
14    solve, Factor, FactorEvaluation, LinearSolver, OptimizationResult, Problem, SolverConfig,
15    SolverMethod, TerminationReason, Variable, VariableId,
16};
17
18/// Configuration for pose graph optimization.
19#[derive(Debug, Clone, Copy)]
20pub struct PoseGraphConfig {
21    /// Maximum number of Gauss-Newton iterations.
22    pub max_iterations: usize,
23    /// Convergence threshold on the update vector norm.
24    pub tolerance: f64,
25    /// Linear backend used inside each nonlinear iteration.
26    pub linear_solver: LinearSolver,
27}
28
29impl Default for PoseGraphConfig {
30    fn default() -> Self {
31        Self {
32            max_iterations: 20,
33            tolerance: 1.0e-5,
34            linear_solver: LinearSolver::Dense,
35        }
36    }
37}
38
39/// A 2D pose node.
40#[derive(Debug, Clone, Copy, PartialEq)]
41pub struct Pose2DNode {
42    pub x: f64,
43    pub y: f64,
44    pub yaw: f64,
45}
46
47impl Pose2DNode {
48    /// Creates a pose node.
49    pub fn new(x: f64, y: f64, yaw: f64) -> Self {
50        Self { x, y, yaw }
51    }
52}
53
54/// A relative pose constraint between two nodes.
55#[derive(Debug, Clone)]
56pub struct Edge2D {
57    pub from: usize,
58    pub to: usize,
59    pub measurement: Pose2DNode,
60    pub information: Matrix3<f64>,
61}
62
63/// Optimization result.
64#[derive(Debug, Clone)]
65pub struct PoseGraphResult {
66    pub poses: Vec<Pose2DNode>,
67    pub iterations: usize,
68    pub converged: bool,
69}
70
71/// Optimizes a 2D pose graph using the workspace nonlinear least-squares
72/// backend. The first pose is fixed to remove gauge freedom.
73pub fn optimize_pose_graph(
74    initial_poses: &[Pose2DNode],
75    edges: &[Edge2D],
76    config: &PoseGraphConfig,
77) -> PoseGraphResult {
78    let damping = SolverConfig::default().initial_damping;
79    optimize_pose_graph_warm(initial_poses, edges, config, damping).0
80}
81
82/// Like [`optimize_pose_graph`], but starts Levenberg-Marquardt from
83/// `initial_damping` and also returns the damping it ended with, so a solve
84/// spread over several short calls (a few iterations per frame) continues
85/// where the last call stopped instead of restarting its damping.
86pub fn optimize_pose_graph_warm(
87    initial_poses: &[Pose2DNode],
88    edges: &[Edge2D],
89    config: &PoseGraphConfig,
90    initial_damping: f64,
91) -> (PoseGraphResult, f64) {
92    if initial_poses.len() <= 1 || edges.is_empty() {
93        return (
94            PoseGraphResult {
95                poses: initial_poses.to_vec(),
96                iterations: 0,
97                converged: true,
98            },
99            initial_damping,
100        );
101    }
102
103    let mut problem = Problem::new();
104    let variable_ids = initial_poses
105        .iter()
106        .map(|pose| {
107            problem.add_variable(
108                Variable::with_retraction(
109                    DVector::from_vec(vec![pose.x, pose.y, pose.yaw]),
110                    3,
111                    retract_pose_2d,
112                )
113                .expect("SE(2) pose dimensions are valid"),
114            )
115        })
116        .collect::<Vec<_>>();
117    problem
118        .variable_mut(variable_ids[0])
119        .expect("first pose exists")
120        .set_fixed(true);
121
122    for edge in edges {
123        if edge.from < initial_poses.len() && edge.to < initial_poses.len() {
124            let _ = problem.add_factor(PoseGraphFactor2D {
125                variables: [variable_ids[edge.from], variable_ids[edge.to]],
126                edge: edge.clone(),
127            });
128        }
129    }
130
131    let solver_config = SolverConfig {
132        method: SolverMethod::LevenbergMarquardt,
133        max_iterations: config.max_iterations.max(1),
134        gradient_tolerance: config.tolerance,
135        step_tolerance: config.tolerance,
136        cost_tolerance: config.tolerance * config.tolerance,
137        initial_damping: initial_damping.clamp(1.0e-15, 1.0e15),
138        linear_solver: config.linear_solver,
139    };
140    let summary = solve(&mut problem, &solver_config).ok();
141    let poses = variable_ids
142        .iter()
143        .map(|id| {
144            let value = problem.variable(*id).expect("pose variable exists").value();
145            Pose2DNode::new(value[0], value[1], value[2])
146        })
147        .collect();
148
149    let final_damping = summary
150        .as_ref()
151        .map_or(initial_damping, |result| result.final_damping);
152    (
153        PoseGraphResult {
154            poses,
155            iterations: summary.as_ref().map_or(0, |result| result.iterations),
156            converged: summary
157                .is_some_and(|result| result.termination != TerminationReason::MaxIterations),
158        },
159        final_damping,
160    )
161}
162
163struct PoseGraphFactor2D {
164    variables: [VariableId; 2],
165    edge: Edge2D,
166}
167
168impl Factor for PoseGraphFactor2D {
169    fn variable_ids(&self) -> &[VariableId] {
170        &self.variables
171    }
172
173    fn evaluate(&self, values: &[&DVector<f64>]) -> OptimizationResult<FactorEvaluation> {
174        let from = Pose2DNode::new(values[0][0], values[0][1], values[0][2]);
175        let to = Pose2DNode::new(values[1][0], values[1][1], values[1][2]);
176        let (error, from_jacobian, to_jacobian) =
177            edge_error_and_jacobians(&from, &to, &self.edge.measurement);
178        Ok(FactorEvaluation {
179            residual: DVector::from_column_slice(error.as_slice()),
180            information: DMatrix::from_column_slice(3, 3, self.edge.information.as_slice()),
181            jacobians: vec![
182                DMatrix::from_column_slice(3, 3, from_jacobian.as_slice()),
183                DMatrix::from_column_slice(3, 3, to_jacobian.as_slice()),
184            ],
185        })
186    }
187}
188
189fn retract_pose_2d(
190    value: &DVector<f64>,
191    increment: &DVector<f64>,
192) -> OptimizationResult<DVector<f64>> {
193    Ok(DVector::from_vec(vec![
194        value[0] + increment[0],
195        value[1] + increment[1],
196        normalize_angle(value[2] + increment[2]),
197    ]))
198}
199
200fn edge_error_and_jacobians(
201    from: &Pose2DNode,
202    to: &Pose2DNode,
203    measurement: &Pose2DNode,
204) -> (Vector3<f64>, Matrix3<f64>, Matrix3<f64>) {
205    let t_i = Vector2::new(from.x, from.y);
206    let t_j = Vector2::new(to.x, to.y);
207    let t_ij = Vector2::new(measurement.x, measurement.y);
208
209    let r_i_t = rotation_matrix(from.yaw).transpose();
210    let r_ij_t = rotation_matrix(measurement.yaw).transpose();
211    let delta_t = t_j - t_i;
212
213    let translational_error = r_ij_t * (r_i_t * delta_t - t_ij);
214    let error = Vector3::new(
215        translational_error[0],
216        translational_error[1],
217        normalize_angle(to.yaw - from.yaw - measurement.yaw),
218    );
219
220    let dr_i_t = rotation_matrix_transpose_derivative(from.yaw);
221    let a_translation = -r_ij_t * r_i_t;
222    let a_heading = r_ij_t * (dr_i_t * delta_t);
223    let b_translation = r_ij_t * r_i_t;
224
225    let j_i = Matrix3::new(
226        a_translation[(0, 0)],
227        a_translation[(0, 1)],
228        a_heading[0],
229        a_translation[(1, 0)],
230        a_translation[(1, 1)],
231        a_heading[1],
232        0.0,
233        0.0,
234        -1.0,
235    );
236    let j_j = Matrix3::new(
237        b_translation[(0, 0)],
238        b_translation[(0, 1)],
239        0.0,
240        b_translation[(1, 0)],
241        b_translation[(1, 1)],
242        0.0,
243        0.0,
244        0.0,
245        1.0,
246    );
247
248    (error, j_i, j_j)
249}
250
251#[cfg(test)]
252fn relative_transform(from: &Pose2DNode, to: &Pose2DNode) -> Pose2DNode {
253    let dx = to.x - from.x;
254    let dy = to.y - from.y;
255    let cos_yaw = from.yaw.cos();
256    let sin_yaw = from.yaw.sin();
257
258    Pose2DNode {
259        x: cos_yaw * dx + sin_yaw * dy,
260        y: -sin_yaw * dx + cos_yaw * dy,
261        yaw: normalize_angle(to.yaw - from.yaw),
262    }
263}
264
265fn rotation_matrix(yaw: f64) -> Matrix2<f64> {
266    let cos_yaw = yaw.cos();
267    let sin_yaw = yaw.sin();
268    Matrix2::new(cos_yaw, -sin_yaw, sin_yaw, cos_yaw)
269}
270
271fn rotation_matrix_transpose_derivative(yaw: f64) -> Matrix2<f64> {
272    let cos_yaw = yaw.cos();
273    let sin_yaw = yaw.sin();
274    Matrix2::new(-sin_yaw, cos_yaw, -cos_yaw, -sin_yaw)
275}
276
277#[cfg(test)]
278mod tests {
279    use super::*;
280    use std::f64::consts::PI;
281
282    fn edge_from_truth(poses: &[Pose2DNode], from: usize, to: usize) -> Edge2D {
283        Edge2D {
284            from,
285            to,
286            measurement: relative_transform(&poses[from], &poses[to]),
287            information: Matrix3::identity(),
288        }
289    }
290
291    fn perturb_pose(pose: &Pose2DNode, axis: usize, delta: f64) -> Pose2DNode {
292        let mut perturbed = *pose;
293        match axis {
294            0 => perturbed.x += delta,
295            1 => perturbed.y += delta,
296            2 => perturbed.yaw = normalize_angle(perturbed.yaw + delta),
297            _ => unreachable!("axis out of bounds"),
298        }
299        perturbed
300    }
301
302    fn error_difference(a: &Vector3<f64>, b: &Vector3<f64>) -> Vector3<f64> {
303        Vector3::new(a[0] - b[0], a[1] - b[1], normalize_angle(a[2] - b[2]))
304    }
305
306    fn numerical_jacobian(
307        from: &Pose2DNode,
308        to: &Pose2DNode,
309        measurement: &Pose2DNode,
310        perturb_from: bool,
311    ) -> Matrix3<f64> {
312        let epsilon = 1.0e-6;
313        let mut jacobian = Matrix3::zeros();
314
315        for axis in 0..3 {
316            let (from_plus, to_plus, from_minus, to_minus) = if perturb_from {
317                (
318                    perturb_pose(from, axis, epsilon),
319                    *to,
320                    perturb_pose(from, axis, -epsilon),
321                    *to,
322                )
323            } else {
324                (
325                    *from,
326                    perturb_pose(to, axis, epsilon),
327                    *from,
328                    perturb_pose(to, axis, -epsilon),
329                )
330            };
331
332            let error_plus = edge_error_and_jacobians(&from_plus, &to_plus, measurement).0;
333            let error_minus = edge_error_and_jacobians(&from_minus, &to_minus, measurement).0;
334            let column = error_difference(&error_plus, &error_minus) / (2.0 * epsilon);
335            jacobian.set_column(axis, &column);
336        }
337
338        jacobian
339    }
340
341    #[test]
342    fn test_pose_graph_config_defaults() {
343        let config = PoseGraphConfig::default();
344        assert_eq!(config.max_iterations, 20);
345        assert_eq!(config.tolerance, 1.0e-5);
346    }
347
348    #[test]
349    fn test_pose_graph_identity_solution_stays_unchanged() {
350        let poses = vec![
351            Pose2DNode::new(0.0, 0.0, 0.0),
352            Pose2DNode::new(1.0, 0.0, 0.0),
353            Pose2DNode::new(1.0, 1.0, PI / 2.0),
354        ];
355        let edges = vec![
356            edge_from_truth(&poses, 0, 1),
357            edge_from_truth(&poses, 1, 2),
358            edge_from_truth(&poses, 0, 2),
359        ];
360
361        let result = optimize_pose_graph(&poses, &edges, &PoseGraphConfig::default());
362
363        assert!(result.converged);
364        for (expected, actual) in poses.iter().zip(result.poses.iter()) {
365            assert!((expected.x - actual.x).abs() < 1.0e-9);
366            assert!((expected.y - actual.y).abs() < 1.0e-9);
367            assert!(normalize_angle(expected.yaw - actual.yaw).abs() < 1.0e-9);
368        }
369    }
370
371    #[test]
372    fn test_pose_graph_triangle_optimization_converges() {
373        let truth = vec![
374            Pose2DNode::new(0.0, 0.0, 0.0),
375            Pose2DNode::new(1.0, 0.0, 0.0),
376            Pose2DNode::new(1.0, 1.0, PI / 2.0),
377        ];
378        let initial = vec![
379            Pose2DNode::new(0.0, 0.0, 0.0),
380            Pose2DNode::new(1.15, -0.1, 0.08),
381            Pose2DNode::new(0.85, 1.2, PI / 2.0 - 0.12),
382        ];
383        let edges = vec![
384            edge_from_truth(&truth, 0, 1),
385            edge_from_truth(&truth, 1, 2),
386            edge_from_truth(&truth, 0, 2),
387        ];
388
389        let result = optimize_pose_graph(&initial, &edges, &PoseGraphConfig::default());
390
391        assert!(result.converged);
392        for (expected, actual) in truth.iter().zip(result.poses.iter()) {
393            assert!((expected.x - actual.x).abs() < 5.0e-2);
394            assert!((expected.y - actual.y).abs() < 5.0e-2);
395            assert!(normalize_angle(expected.yaw - actual.yaw).abs() < 5.0e-2);
396        }
397    }
398
399    #[test]
400    fn test_pose_graph_jacobians_match_finite_difference() {
401        let from = Pose2DNode::new(0.3, -0.4, 0.35);
402        let to = Pose2DNode::new(1.1, 0.7, -0.25);
403        let measurement = Pose2DNode::new(0.9, 0.8, -0.5);
404
405        let (_, j_i, j_j) = edge_error_and_jacobians(&from, &to, &measurement);
406        let numerical_i = numerical_jacobian(&from, &to, &measurement, true);
407        let numerical_j = numerical_jacobian(&from, &to, &measurement, false);
408
409        for row in 0..3 {
410            for col in 0..3 {
411                assert!((j_i[(row, col)] - numerical_i[(row, col)]).abs() < 1.0e-5);
412                assert!((j_j[(row, col)] - numerical_j[(row, col)]).abs() < 1.0e-5);
413            }
414        }
415    }
416}