1use nalgebra::{DMatrix, DVector, Matrix3, Matrix4, Vector2, Vector3};
4use rust_robotics_core::{
5 se2_exp, se2_inverse, se2_log, se3_exp, se3_inverse, se3_log, skew, Vector6,
6};
7use rust_robotics_optimization::{
8 solve, Factor, FactorEvaluation, OptimizationResult, Problem, RobustKernel, SolverConfig,
9 Variable, VariableId,
10};
11
12#[derive(Debug, Clone, Copy)]
14pub struct GeometricIcpConfig {
15 pub max_outer_iterations: usize,
16 pub max_inner_iterations: usize,
17 pub correspondence_distance: f64,
18 pub tolerance: f64,
19 pub robust_kernel: RobustKernel,
20}
21
22impl Default for GeometricIcpConfig {
23 fn default() -> Self {
24 Self {
25 max_outer_iterations: 20,
26 max_inner_iterations: 10,
27 correspondence_distance: 1.0,
28 tolerance: 1.0e-7,
29 robust_kernel: RobustKernel::Huber { delta: 0.1 },
30 }
31 }
32}
33
34#[derive(Debug, Clone)]
35pub struct PointToLineIcpResult {
36 pub transform: Matrix3<f64>,
37 pub iterations: usize,
38 pub mean_absolute_error: f64,
39 pub converged: bool,
40}
41
42#[derive(Debug, Clone)]
43pub struct PointToPlaneIcpResult {
44 pub transform: Matrix4<f64>,
45 pub iterations: usize,
46 pub mean_absolute_error: f64,
47 pub converged: bool,
48}
49
50pub fn point_to_line_icp_2d(
52 source: &[Vector2<f64>],
53 target: &[Vector2<f64>],
54 target_normals: &[Vector2<f64>],
55 initial: Matrix3<f64>,
56 config: &GeometricIcpConfig,
57) -> PointToLineIcpResult {
58 if target.len() != target_normals.len() || source.is_empty() || target.is_empty() {
59 return PointToLineIcpResult {
60 transform: initial,
61 iterations: 0,
62 mean_absolute_error: f64::INFINITY,
63 converged: false,
64 };
65 }
66 let mut transform = initial;
67 let mut last_error = f64::INFINITY;
68 for iteration in 0..config.max_outer_iterations {
69 let correspondences = source
70 .iter()
71 .filter_map(|source_point| {
72 let transformed = transform.fixed_view::<2, 2>(0, 0) * source_point
73 + transform.fixed_view::<2, 1>(0, 2);
74 nearest_2d(&transformed, target).and_then(|(index, distance)| {
75 (distance <= config.correspondence_distance).then_some((
76 *source_point,
77 target[index],
78 target_normals[index],
79 ))
80 })
81 })
82 .collect::<Vec<_>>();
83 if correspondences.len() < 2 {
84 break;
85 }
86 let previous = transform;
87 let mut problem = Problem::new();
88 let pose = problem.add_variable(
89 Variable::with_retraction(
90 DVector::from_column_slice(se2_log(&transform).as_slice()),
91 3,
92 retract_se2,
93 )
94 .expect("SE(2) dimensions are valid"),
95 );
96 for (source, target, normal) in &correspondences {
97 let _ = problem.add_factor(PointToLineFactor {
98 variables: [pose],
99 source: *source,
100 target: *target,
101 normal: normal.normalize(),
102 kernel: config.robust_kernel,
103 });
104 }
105 let _ = solve(
106 &mut problem,
107 &SolverConfig {
108 max_iterations: config.max_inner_iterations.max(1),
109 step_tolerance: config.tolerance,
110 gradient_tolerance: config.tolerance,
111 ..SolverConfig::default()
112 },
113 );
114 let value = problem.variable(pose).expect("pose exists").value();
115 transform = se2_exp(&Vector3::from_column_slice(value.as_slice()));
116 let error = correspondences
117 .iter()
118 .map(|(source, target, normal)| {
119 let transformed = transform.fixed_view::<2, 2>(0, 0) * source
120 + transform.fixed_view::<2, 1>(0, 2);
121 normal.dot(&(transformed - target)).abs()
122 })
123 .sum::<f64>()
124 / correspondences.len() as f64;
125 let step = se2_log(&(se2_inverse(&previous) * transform)).norm();
126 if step <= config.tolerance || (last_error - error).abs() <= config.tolerance {
127 return PointToLineIcpResult {
128 transform,
129 iterations: iteration + 1,
130 mean_absolute_error: error,
131 converged: true,
132 };
133 }
134 last_error = error;
135 }
136 PointToLineIcpResult {
137 transform,
138 iterations: config.max_outer_iterations,
139 mean_absolute_error: last_error,
140 converged: false,
141 }
142}
143
144pub fn point_to_plane_icp_3d(
146 source: &[Vector3<f64>],
147 target: &[Vector3<f64>],
148 target_normals: &[Vector3<f64>],
149 initial: Matrix4<f64>,
150 config: &GeometricIcpConfig,
151) -> PointToPlaneIcpResult {
152 if target.len() != target_normals.len() || source.is_empty() || target.is_empty() {
153 return PointToPlaneIcpResult {
154 transform: initial,
155 iterations: 0,
156 mean_absolute_error: f64::INFINITY,
157 converged: false,
158 };
159 }
160 let mut transform = initial;
161 let mut last_error = f64::INFINITY;
162 for iteration in 0..config.max_outer_iterations {
163 let correspondences = source
164 .iter()
165 .filter_map(|source_point| {
166 let transformed = transform.fixed_view::<3, 3>(0, 0) * source_point
167 + transform.fixed_view::<3, 1>(0, 3);
168 nearest_3d(&transformed, target).and_then(|(index, distance)| {
169 (distance <= config.correspondence_distance).then_some((
170 *source_point,
171 target[index],
172 target_normals[index],
173 ))
174 })
175 })
176 .collect::<Vec<_>>();
177 if correspondences.len() < 3 {
178 break;
179 }
180 let previous = transform;
181 let mut problem = Problem::new();
182 let pose = problem.add_variable(
183 Variable::with_retraction(
184 DVector::from_column_slice(se3_log(&transform).as_slice()),
185 6,
186 retract_se3,
187 )
188 .expect("SE(3) dimensions are valid"),
189 );
190 for (source, target, normal) in &correspondences {
191 let _ = problem.add_factor(PointToPlaneFactor {
192 variables: [pose],
193 source: *source,
194 target: *target,
195 normal: normal.normalize(),
196 kernel: config.robust_kernel,
197 });
198 }
199 let _ = solve(
200 &mut problem,
201 &SolverConfig {
202 max_iterations: config.max_inner_iterations.max(1),
203 step_tolerance: config.tolerance,
204 gradient_tolerance: config.tolerance,
205 ..SolverConfig::default()
206 },
207 );
208 let value = problem.variable(pose).expect("pose exists").value();
209 transform = se3_exp(&Vector6::from_column_slice(value.as_slice()));
210 let error = correspondences
211 .iter()
212 .map(|(source, target, normal)| {
213 let transformed = transform.fixed_view::<3, 3>(0, 0) * source
214 + transform.fixed_view::<3, 1>(0, 3);
215 normal.dot(&(transformed - target)).abs()
216 })
217 .sum::<f64>()
218 / correspondences.len() as f64;
219 let step = se3_log(&(se3_inverse(&previous) * transform)).norm();
220 if step <= config.tolerance || (last_error - error).abs() <= config.tolerance {
221 return PointToPlaneIcpResult {
222 transform,
223 iterations: iteration + 1,
224 mean_absolute_error: error,
225 converged: true,
226 };
227 }
228 last_error = error;
229 }
230 PointToPlaneIcpResult {
231 transform,
232 iterations: config.max_outer_iterations,
233 mean_absolute_error: last_error,
234 converged: false,
235 }
236}
237
238struct PointToLineFactor {
239 variables: [VariableId; 1],
240 source: Vector2<f64>,
241 target: Vector2<f64>,
242 normal: Vector2<f64>,
243 kernel: RobustKernel,
244}
245
246impl Factor for PointToLineFactor {
247 fn variable_ids(&self) -> &[VariableId] {
248 &self.variables
249 }
250
251 fn robust_kernel(&self) -> RobustKernel {
252 self.kernel
253 }
254
255 fn evaluate(&self, values: &[&DVector<f64>]) -> OptimizationResult<FactorEvaluation> {
256 let transform = se2_exp(&Vector3::from_column_slice(values[0].as_slice()));
257 let rotation = transform.fixed_view::<2, 2>(0, 0).into_owned();
258 let transformed = rotation * self.source + transform.fixed_view::<2, 1>(0, 2);
259 let angular = rotation * Vector2::new(-self.source.y, self.source.x);
260 let jacobian = DMatrix::from_row_slice(
261 1,
262 3,
263 &[
264 self.normal.dot(&rotation.column(0)),
265 self.normal.dot(&rotation.column(1)),
266 self.normal.dot(&angular),
267 ],
268 );
269 Ok(FactorEvaluation::unit_weighted(
270 DVector::from_element(1, self.normal.dot(&(transformed - self.target))),
271 vec![jacobian],
272 ))
273 }
274}
275
276struct PointToPlaneFactor {
277 variables: [VariableId; 1],
278 source: Vector3<f64>,
279 target: Vector3<f64>,
280 normal: Vector3<f64>,
281 kernel: RobustKernel,
282}
283
284impl Factor for PointToPlaneFactor {
285 fn variable_ids(&self) -> &[VariableId] {
286 &self.variables
287 }
288
289 fn robust_kernel(&self) -> RobustKernel {
290 self.kernel
291 }
292
293 fn evaluate(&self, values: &[&DVector<f64>]) -> OptimizationResult<FactorEvaluation> {
294 let transform = se3_exp(&Vector6::from_column_slice(values[0].as_slice()));
295 let rotation = transform.fixed_view::<3, 3>(0, 0).into_owned();
296 let transformed = rotation * self.source + transform.fixed_view::<3, 1>(0, 3);
297 let rotation_jacobian = -rotation * skew(&self.source);
298 let mut jacobian = DMatrix::zeros(1, 6);
299 for column in 0..3 {
300 jacobian[(0, column)] = self.normal.dot(&rotation.column(column));
301 jacobian[(0, column + 3)] = self.normal.dot(&rotation_jacobian.column(column));
302 }
303 Ok(FactorEvaluation::unit_weighted(
304 DVector::from_element(1, self.normal.dot(&(transformed - self.target))),
305 vec![jacobian],
306 ))
307 }
308}
309
310fn retract_se2(value: &DVector<f64>, increment: &DVector<f64>) -> OptimizationResult<DVector<f64>> {
311 let transform = se2_exp(&Vector3::from_column_slice(value.as_slice()));
312 let delta = se2_exp(&Vector3::from_column_slice(increment.as_slice()));
313 Ok(DVector::from_column_slice(
314 se2_log(&(transform * delta)).as_slice(),
315 ))
316}
317
318fn retract_se3(value: &DVector<f64>, increment: &DVector<f64>) -> OptimizationResult<DVector<f64>> {
319 let transform = se3_exp(&Vector6::from_column_slice(value.as_slice()));
320 let delta = se3_exp(&Vector6::from_column_slice(increment.as_slice()));
321 Ok(DVector::from_column_slice(
322 se3_log(&(transform * delta)).as_slice(),
323 ))
324}
325
326fn nearest_2d(query: &Vector2<f64>, points: &[Vector2<f64>]) -> Option<(usize, f64)> {
327 points
328 .iter()
329 .enumerate()
330 .map(|(index, point)| (index, (query - point).norm()))
331 .min_by(|left, right| left.1.total_cmp(&right.1))
332}
333
334fn nearest_3d(query: &Vector3<f64>, points: &[Vector3<f64>]) -> Option<(usize, f64)> {
335 points
336 .iter()
337 .enumerate()
338 .map(|(index, point)| (index, (query - point).norm()))
339 .min_by(|left, right| left.1.total_cmp(&right.1))
340}
341
342#[cfg(test)]
343mod tests {
344 use super::*;
345
346 #[test]
347 fn point_to_line_recovers_normal_translation() {
348 let target = (-10..=10)
349 .map(|index| Vector2::new(index as f64 * 0.1, 0.0))
350 .collect::<Vec<_>>();
351 let source = target
352 .iter()
353 .map(|point| point + Vector2::new(0.0, 0.3))
354 .collect::<Vec<_>>();
355 let normals = vec![Vector2::y(); target.len()];
356 let result = point_to_line_icp_2d(
357 &source,
358 &target,
359 &normals,
360 Matrix3::identity(),
361 &GeometricIcpConfig::default(),
362 );
363 assert!(result.converged);
364 assert!((result.transform[(1, 2)] + 0.3).abs() < 1.0e-6);
365 }
366
367 #[test]
368 fn point_to_plane_recovers_normal_translation() {
369 let target = (-2..=2)
370 .flat_map(|x| (-2..=2).map(move |y| Vector3::new(x as f64 * 0.2, y as f64 * 0.2, 0.0)))
371 .collect::<Vec<_>>();
372 let source = target
373 .iter()
374 .map(|point| point + Vector3::new(0.0, 0.0, 0.4))
375 .collect::<Vec<_>>();
376 let normals = vec![Vector3::z(); target.len()];
377 let result = point_to_plane_icp_3d(
378 &source,
379 &target,
380 &normals,
381 Matrix4::identity(),
382 &GeometricIcpConfig::default(),
383 );
384 assert!(result.converged);
385 assert!((result.transform[(2, 3)] + 0.4).abs() < 1.0e-6);
386 }
387}