1use 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#[derive(Debug, Clone, Copy)]
20pub struct PoseGraphConfig {
21 pub max_iterations: usize,
23 pub tolerance: f64,
25 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#[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 pub fn new(x: f64, y: f64, yaw: f64) -> Self {
50 Self { x, y, yaw }
51 }
52}
53
54#[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#[derive(Debug, Clone)]
65pub struct PoseGraphResult {
66 pub poses: Vec<Pose2DNode>,
67 pub iterations: usize,
68 pub converged: bool,
69}
70
71pub 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
82pub 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}