Skip to main content

rust_robotics_slam/
geometric_icp.rs

1//! Point-to-line and point-to-plane ICP using the shared robust optimizer.
2
3use 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/// Shared outer-loop and robust-loss settings.
13#[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
50/// Aligns 2D source points to target tangent lines.
51pub 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
144/// Aligns 3D source points to target tangent planes.
145pub 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}