align3d/icp/
pcl_icp.rs

1use super::cost_function::PointPlaneDistance;
2use super::icp_params::IcpParams;
3use crate::{
4    extra_math,
5    kdtree::R3dTree,
6    optim::GaussNewton,
7    pointcloud::PointCloud,
8    transform::{LieGroup, Transform},
9};
10use itertools::izip;
11use num::Float;
12
13/// Standard Iterative Closest Point (ICP) algorithm for aligning two point clouds.
14/// This implementation uses the point-to-plane distance.
15pub struct Icp<'target> {
16    // Parameters of the ICP algorithm.
17    pub params: IcpParams,
18    // Initial transformation to start the algorithm. Default is the identity.
19    pub initial_transform: Transform,
20    target: &'target PointCloud,
21    kdtree: R3dTree,
22}
23
24impl<'target> Icp<'target> {
25    /// Create a new ICP instance.
26    ///
27    /// # Arguments
28    ///
29    /// * params - Parameters of the ICP algorithm.
30    /// * target - Target point cloud.
31    pub fn new(params: IcpParams, target: &'target PointCloud) -> Self {
32        Self {
33            params,
34            initial_transform: Transform::eye(),
35            target,
36            kdtree: R3dTree::new(&target.points.view()),
37        }
38    }
39
40    /// Aligns the source point cloud to the target point cloud.
41    ///
42    /// # Arguments
43    ///
44    /// * source - Source point cloud.
45    ///
46    /// # Returns
47    ///
48    /// The transformation that aligns the source point cloud to the target point cloud.
49    pub fn align(&self, source: &PointCloud) -> Transform {
50        let target_normals = self
51            .target
52            .normals
53            .as_ref()
54            .expect("Please, the target point cloud should have normals.");
55        let source_normals = source
56            .normals
57            .as_ref()
58            .expect("Please, the source point cloud should have normals.");
59        let mut optim_transform = Transform::eye();
60        let mut optimizer = GaussNewton::<6>::new();
61        let geom_cost = PointPlaneDistance {};
62
63        let max_distance_sqr = self.params.max_distance * self.params.max_distance;
64
65        let mut best_residual = Float::infinity();
66        let mut best_transform = optim_transform.clone();
67        for _ in 0..self.params.max_iterations {
68            for (source_point, source_normal) in izip!(source.points.iter(), source_normals.iter())
69            {
70                let source_point = optim_transform.transform_vector(source_point);
71                let source_normal = optim_transform.transform_normal(source_normal);
72
73                let (found_index, found_sqr_distance) = self.kdtree.nearest(&source_point);
74                if found_sqr_distance > max_distance_sqr {
75                    continue;
76                }
77
78                let target_normal = target_normals[found_index];
79
80                if extra_math::angle_between_normals(&source_normal, &target_normal)
81                    > self.params.max_normal_angle
82                {
83                    continue;
84                }
85
86                let target_point = self.target.points[found_index];
87
88                let (residual, jacobian) =
89                    geom_cost.jacobian(&source_point, &target_point, &target_normal);
90
91                optimizer.step(residual, &jacobian);
92            }
93
94            let residual = optimizer.mean_squared_residual();
95            optimizer.weight(self.params.weight);
96            let update = optimizer.solve().unwrap();
97            optim_transform = &Transform::exp(&LieGroup::Se3(update)) * &optim_transform;
98            optimizer.reset();
99
100            if residual < best_residual {
101                best_residual = residual;
102                best_transform = optim_transform.clone();
103            }
104        }
105
106        best_transform
107    }
108}
109
110#[cfg(test)]
111mod tests {
112    use super::*;
113    use rstest::*;
114
115    use crate::{
116        metrics::TransformMetrics,
117        unit_test::{sample_pcl_ds1, TestPclDataset},
118    };
119
120    /// Test the ICP algorithm.
121    #[rstest]
122    fn test_icp(sample_pcl_ds1: TestPclDataset) {
123        let target_pcl = sample_pcl_ds1.get(0);
124        let source_pcl = sample_pcl_ds1.get(1);
125
126        let actual = Icp::new(
127            IcpParams {
128                max_iterations: 5,
129                ..Default::default()
130            },
131            &target_pcl,
132        )
133        .align(&source_pcl);
134        let gt_transform = sample_pcl_ds1.get_ground_truth(1, 0);
135        assert!(TransformMetrics::new(&actual, &gt_transform).angle.abs() < 0.1);
136    }
137}
align3d/icp/pcl_icp.rs

align3d/icp/
pcl_icp.rs
