Skip to main content

rust_robotics_slam/
pose_graph_optimization_3d.rs

1//! SE(3) pose-graph optimization on the shared factor-graph backend.
2
3use nalgebra::{DMatrix, DVector, Matrix4};
4use rust_robotics_core::{se3_adjoint, se3_exp, se3_inverse, se3_log, skew, Matrix6, Vector6};
5use rust_robotics_optimization::{
6    solve, Factor, FactorEvaluation, OptimizationResult, Problem, SolverConfig, SolverMethod,
7    TerminationReason, Variable, VariableId,
8};
9
10use crate::pose_graph_optimization::PoseGraphConfig;
11
12/// A 3D pose represented by a homogeneous SE(3) matrix.
13#[derive(Debug, Clone, PartialEq)]
14pub struct Pose3DNode {
15    pub transform: Matrix4<f64>,
16}
17
18impl Pose3DNode {
19    pub fn identity() -> Self {
20        Self {
21            transform: Matrix4::identity(),
22        }
23    }
24
25    pub fn from_tangent(tangent: &Vector6) -> Self {
26        Self {
27            transform: se3_exp(tangent),
28        }
29    }
30
31    pub fn tangent(&self) -> Vector6 {
32        se3_log(&self.transform)
33    }
34}
35
36/// A relative SE(3) constraint between two pose nodes.
37#[derive(Debug, Clone)]
38pub struct Edge3D {
39    pub from: usize,
40    pub to: usize,
41    pub measurement: Pose3DNode,
42    pub information: Matrix6,
43}
44
45/// Result of an SE(3) pose-graph solve.
46#[derive(Debug, Clone)]
47pub struct PoseGraphResult3D {
48    pub poses: Vec<Pose3DNode>,
49    pub iterations: usize,
50    pub converged: bool,
51}
52
53/// Optimizes an SE(3) pose graph, fixing the first pose as the gauge anchor.
54pub fn optimize_pose_graph_3d(
55    initial_poses: &[Pose3DNode],
56    edges: &[Edge3D],
57    config: &PoseGraphConfig,
58) -> PoseGraphResult3D {
59    if initial_poses.len() <= 1 || edges.is_empty() {
60        return PoseGraphResult3D {
61            poses: initial_poses.to_vec(),
62            iterations: 0,
63            converged: true,
64        };
65    }
66
67    let mut problem = Problem::new();
68    let ids = initial_poses
69        .iter()
70        .map(|pose| {
71            problem.add_variable(
72                Variable::with_retraction(
73                    DVector::from_column_slice(pose.tangent().as_slice()),
74                    6,
75                    retract_pose_3d,
76                )
77                .expect("SE(3) dimensions are valid"),
78            )
79        })
80        .collect::<Vec<_>>();
81    problem
82        .variable_mut(ids[0])
83        .expect("first pose exists")
84        .set_fixed(true);
85
86    for edge in edges {
87        if edge.from < initial_poses.len() && edge.to < initial_poses.len() {
88            let _ = problem.add_factor(PoseGraphFactor3D {
89                variables: [ids[edge.from], ids[edge.to]],
90                measurement: edge.measurement.transform,
91                information: edge.information,
92            });
93        }
94    }
95
96    let solver_config = SolverConfig {
97        method: SolverMethod::LevenbergMarquardt,
98        max_iterations: config.max_iterations.max(1),
99        gradient_tolerance: config.tolerance,
100        step_tolerance: config.tolerance,
101        cost_tolerance: config.tolerance * config.tolerance,
102        linear_solver: config.linear_solver,
103        ..SolverConfig::default()
104    };
105    let summary = solve(&mut problem, &solver_config).ok();
106    let poses = ids
107        .iter()
108        .map(|id| {
109            let value = problem.variable(*id).expect("pose exists").value();
110            Pose3DNode::from_tangent(&Vector6::from_column_slice(value.as_slice()))
111        })
112        .collect();
113
114    PoseGraphResult3D {
115        poses,
116        iterations: summary.as_ref().map_or(0, |result| result.iterations),
117        converged: summary
118            .is_some_and(|result| result.termination != TerminationReason::MaxIterations),
119    }
120}
121
122struct PoseGraphFactor3D {
123    variables: [VariableId; 2],
124    measurement: Matrix4<f64>,
125    information: Matrix6,
126}
127
128impl Factor for PoseGraphFactor3D {
129    fn variable_ids(&self) -> &[VariableId] {
130        &self.variables
131    }
132
133    fn evaluate(&self, values: &[&DVector<f64>]) -> OptimizationResult<FactorEvaluation> {
134        let from = se3_exp(&Vector6::from_column_slice(values[0].as_slice()));
135        let to = se3_exp(&Vector6::from_column_slice(values[1].as_slice()));
136        let residual = edge_residual(&from, &to, &self.measurement);
137        let left_inverse = se3_left_jacobian_inverse(&residual);
138        let right_inverse = se3_left_jacobian_inverse(&(-residual));
139        let jacobians = vec![
140            DMatrix::from_column_slice(
141                6,
142                6,
143                (-left_inverse * se3_adjoint(&se3_inverse(&self.measurement))).as_slice(),
144            ),
145            DMatrix::from_column_slice(6, 6, right_inverse.as_slice()),
146        ];
147        Ok(FactorEvaluation {
148            residual: DVector::from_column_slice(residual.as_slice()),
149            information: DMatrix::from_column_slice(6, 6, self.information.as_slice()),
150            jacobians,
151        })
152    }
153}
154
155fn edge_residual(from: &Matrix4<f64>, to: &Matrix4<f64>, measurement: &Matrix4<f64>) -> Vector6 {
156    se3_log(&(se3_inverse(measurement) * se3_inverse(from) * to))
157}
158
159fn se3_left_jacobian_inverse(tangent: &Vector6) -> Matrix6 {
160    let velocity = tangent.fixed_rows::<3>(0).into_owned();
161    let omega = tangent.fixed_rows::<3>(3).into_owned();
162    let mut ad = Matrix6::zeros();
163    ad.fixed_view_mut::<3, 3>(0, 0).copy_from(&skew(&omega));
164    ad.fixed_view_mut::<3, 3>(0, 3).copy_from(&skew(&velocity));
165    ad.fixed_view_mut::<3, 3>(3, 3).copy_from(&skew(&omega));
166
167    // J_l(x) = sum ad(x)^n / (n + 1)!. Evaluating the matrix series and
168    // inverting it avoids finite-differencing the nonlinear factor while
169    // remaining stable at the identity.
170    let mut jacobian = Matrix6::identity();
171    let mut power = Matrix6::identity();
172    let mut factorial = 1.0;
173    for order in 1..=24 {
174        power *= ad;
175        factorial *= (order + 1) as f64;
176        jacobian += power / factorial;
177    }
178    jacobian
179        .try_inverse()
180        .expect("the SE(3) left Jacobian is nonsingular on the principal branch")
181}
182
183#[cfg(test)]
184fn numerical_jacobian(
185    from: &Matrix4<f64>,
186    to: &Matrix4<f64>,
187    measurement: &Matrix4<f64>,
188    perturb_from: bool,
189) -> DMatrix<f64> {
190    let epsilon = 1.0e-6;
191    let mut jacobian = DMatrix::zeros(6, 6);
192    for column in 0..6 {
193        let mut delta = Vector6::zeros();
194        delta[column] = epsilon;
195        let increment = se3_exp(&delta);
196        let decrement = se3_exp(&(-delta));
197        let (plus, minus) = if perturb_from {
198            (
199                edge_residual(&(from * increment), to, measurement),
200                edge_residual(&(from * decrement), to, measurement),
201            )
202        } else {
203            (
204                edge_residual(from, &(to * increment), measurement),
205                edge_residual(from, &(to * decrement), measurement),
206            )
207        };
208        jacobian.set_column(column, &((plus - minus) / (2.0 * epsilon)));
209    }
210    jacobian
211}
212
213fn retract_pose_3d(
214    value: &DVector<f64>,
215    increment: &DVector<f64>,
216) -> OptimizationResult<DVector<f64>> {
217    let pose = se3_exp(&Vector6::from_column_slice(value.as_slice()));
218    let delta = se3_exp(&Vector6::from_column_slice(increment.as_slice()));
219    Ok(DVector::from_column_slice(
220        se3_log(&(pose * delta)).as_slice(),
221    ))
222}
223
224#[cfg(test)]
225mod tests {
226    use nalgebra::Vector3;
227
228    use super::*;
229
230    fn relative(from: &Pose3DNode, to: &Pose3DNode) -> Pose3DNode {
231        Pose3DNode {
232            transform: se3_inverse(&from.transform) * to.transform,
233        }
234    }
235
236    #[test]
237    fn se3_triangle_converges() {
238        let truth = [
239            Pose3DNode::identity(),
240            Pose3DNode::from_tangent(&Vector6::new(1.0, 0.0, 0.0, 0.1, 0.0, 0.0)),
241            Pose3DNode::from_tangent(&Vector6::new(1.0, 1.0, 0.2, 0.1, -0.2, 0.3)),
242        ];
243        let initial = vec![
244            truth[0].clone(),
245            Pose3DNode::from_tangent(&Vector6::new(1.1, -0.1, 0.1, 0.15, 0.02, -0.03)),
246            Pose3DNode::from_tangent(&Vector6::new(0.8, 1.2, 0.1, 0.05, -0.1, 0.4)),
247        ];
248        let edges = [(0, 1), (1, 2), (0, 2)]
249            .into_iter()
250            .map(|(from, to)| Edge3D {
251                from,
252                to,
253                measurement: relative(&truth[from], &truth[to]),
254                information: Matrix6::identity(),
255            })
256            .collect::<Vec<_>>();
257
258        let result = optimize_pose_graph_3d(&initial, &edges, &PoseGraphConfig::default());
259        assert!(result.converged);
260        for (expected, actual) in truth.iter().zip(result.poses.iter()) {
261            let error = se3_log(&(se3_inverse(&expected.transform) * actual.transform));
262            assert!(error.fixed_rows::<3>(0).norm() < 1.0e-5);
263            assert!(Vector3::from(error.fixed_rows::<3>(3)).norm() < 1.0e-5);
264        }
265    }
266
267    #[test]
268    fn analytic_se3_jacobians_match_finite_differences() {
269        let from = se3_exp(&Vector6::new(0.4, -0.2, 0.1, 0.2, -0.1, 0.3));
270        let to = se3_exp(&Vector6::new(1.1, 0.5, -0.3, -0.1, 0.25, 0.15));
271        let measurement = se3_exp(&Vector6::new(0.6, 0.4, -0.2, 0.1, 0.2, -0.1));
272        let residual = edge_residual(&from, &to, &measurement);
273        let analytic_from =
274            -se3_left_jacobian_inverse(&residual) * se3_adjoint(&se3_inverse(&measurement));
275        let analytic_to = se3_left_jacobian_inverse(&(-residual));
276        let numeric_from = numerical_jacobian(&from, &to, &measurement, true);
277        let numeric_to = numerical_jacobian(&from, &to, &measurement, false);
278        assert!((analytic_from - numeric_from).norm() < 1.0e-7);
279        assert!((analytic_to - numeric_to).norm() < 1.0e-7);
280    }
281}