1use 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#[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#[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#[derive(Debug, Clone)]
47pub struct PoseGraphResult3D {
48 pub poses: Vec<Pose3DNode>,
49 pub iterations: usize,
50 pub converged: bool,
51}
52
53pub 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 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}