mapping_algorithms/point_clouds/icp/
types.rs

1// SPDX-License-Identifier: MIT
2/*
3 * Copyright (c) [2023 - Present] Emily Matheys <emilymatt96@gmail.com>
4 *
5 * Permission is hereby granted, free of charge, to any person obtaining a copy
6 * of this software and associated documentation files (the "Software"), to deal
7 * in the Software without restriction, including without limitation the rights
8 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 * copies of the Software, and to permit persons to whom the Software is
10 * furnished to do so, subject to the following conditions:
11 *
12 * The above copyright notice and this permission notice shall be included in all
13 * copies or substantial portions of the Software.
14 *
15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 * SOFTWARE.
22 */
23
24use nalgebra::{AbstractRotation, Isometry, Point, Scalar};
25use num_traits::AsPrimitive;
26
27use crate::Debug;
28
29/// Contains the resulting transform, the resulting Mean Squared Error, and the number of iterations taken for a successful ICP convergence.
30#[derive(Debug)]
31pub struct ICPSuccess<T: Scalar, R: AbstractRotation<T, N>, const N: usize> {
32    /// An isometric matrix, containing the translation and rotation between the point sets.
33    /// In 2D space, its rotation component would be a [`UnitComplex`](nalgebra::UnitComplex), in 3D space it would be a [`UnitQuaternion`](nalgebra::UnitQuaternion).
34    pub transform: Isometry<T, R, N>,
35    /// Mean Squared Error, this is the distances between each point in `points_a` and its corresponding point in `points_b`,
36    /// This can be used to determine whether the ICP converged correctly, or simply on its local minimum.
37    pub mse: T,
38    /// The amount of iterations passed until convergence.
39    pub iteration_num: usize,
40}
41
42/// An error type containing the various errors that might arise during an ICP algorithm, when compiling with the `std` feature, it will also derive [`thiserror::Error`].
43#[derive(Copy, Clone, Debug, Eq, PartialEq)]
44#[cfg_attr(feature = "std", derive(thiserror::Error))]
45pub enum ICPError<T: Scalar, const N: usize> {
46    /// The source point cloud is empty.
47    SourcePointCloudEmpty,
48    /// The target point cloud is empty.
49    TargetPointCloudEmpty,
50    /// A source point had no nearest neighbour in the target point cloud.
51    NoNearestNeighbour,
52    /// The amount of iterations was set to zero.
53    IterationNumIsZero,
54    /// The Mean Squared Error interval threshold was set to zero.
55    MSEIntervalThreshold,
56    /// The Mean Squared Error absolute threshold was set to zero.
57    MSEAbsoluteThreshold,
58    /// The Current iteration did not converge, returns the current mean points.
59    IterationDidNotConverge((Point<T, N>, Point<T, N>)),
60    /// The algorithm did not converge after the maximum amount of iterations.
61    AlrogithmDidNotConverge,
62}
63
64/// A type alias for the result of an ICP algorithm, containing either the successful result or an error.
65pub type ICPResult<T, R, const N: usize> = Result<ICPSuccess<T, R, N>, ICPError<T, N>>;
66
67/// A struct specifying configuration options for an ICP algorithm.
68#[derive(Clone, Debug)]
69pub struct ICPConfiguration<T> {
70    /// Whether to use a KDTree structure to find nearest neighbours, becomes increasingly effective with point cloud growth.
71    pub(crate) use_kd_tree: bool,
72    /// The amount of iterations before giving up and exiting the algorithm.
73    pub(crate) max_iterations: usize,
74    /// When provided, the algorithm will consider itself converged when the MSE is smaller than the given value, without any more iterations.
75    pub(crate) mse_absolute_threshold: Option<T>,
76    /// This will specify the interval between iteration MSE's than when reached, will declare ICP convergence.
77    pub(crate) mse_interval_threshold: T,
78}
79
80impl<T: 'static + Copy> ICPConfiguration<T>
81where
82    f32: AsPrimitive<T>,
83{
84    /// Returns a builder for the configuration struct.
85    ///
86    /// # Returns
87    /// An [`ICPConfigurationBuilder`].
88    pub fn builder() -> ICPConfigurationBuilder<T> {
89        ICPConfigurationBuilder {
90            _internal: ICPConfiguration {
91                use_kd_tree: false,
92                max_iterations: 20,
93                mse_absolute_threshold: None,
94                mse_interval_threshold: 0.01.as_(),
95            },
96        }
97    }
98}
99
100/// A Builder-pattern struct for safely constructing an [`ICPConfiguration`] struct.
101#[derive(Clone, Debug)]
102pub struct ICPConfigurationBuilder<T> {
103    _internal: ICPConfiguration<T>,
104}
105
106impl<T: Copy> ICPConfigurationBuilder<T> {
107    /// Enables usage of a KD Tree structure to find nearest neighbours, or use a native On^2 search,
108    /// a KD Tree becomes increasingly effective with point cloud growth.
109    ///
110    /// # Arguments
111    /// * `use_kd_tree`: Whether to use a KD Tree search method.
112    ///
113    /// # Returns
114    /// A copy of self, with the updated parameters
115    pub fn with_kd_tree(&self, use_kd_tree: bool) -> Self {
116        Self {
117            _internal: ICPConfiguration {
118                use_kd_tree,
119                ..self._internal
120            },
121        }
122    }
123
124    /// The amount of iterations before giving up and exiting the algorithm.
125    ///
126    /// # Arguments
127    /// * `max_iterations`: The maximum number of iterations to allow.
128    ///
129    /// # Returns
130    /// A copy of self, with the updated parameters
131    pub fn with_max_iterations(&self, max_iterations: usize) -> Self {
132        Self {
133            _internal: ICPConfiguration {
134                max_iterations,
135                ..self._internal
136            },
137        }
138    }
139
140    /// When provided, the algorithm will consider itself converged when the MSE is smaller than the given value, without any more iterations.
141    ///
142    /// # Arguments
143    /// * `mse_absolute_threshold`: If is [`Some`], sets the minimum accepted MSE difference, that will return a convergence.
144    ///
145    /// # Returns
146    /// A copy of self, with the updated parameters
147    pub fn with_absolute_mse_threshold(&self, mse_absolute_threshold: Option<T>) -> Self {
148        Self {
149            _internal: ICPConfiguration {
150                mse_absolute_threshold,
151                ..self._internal
152            },
153        }
154    }
155
156    /// This will specify the interval between iteration MSE's than when reached, will declare ICP convergence.
157    ///
158    /// # Arguments
159    /// * `mse_interval_threshold`: The minimum threshold for an MSE, anything below will return a convergence.
160    ///
161    /// # Returns
162    /// A copy of self, with the updated parameters
163    pub fn with_mse_interval_threshold(&self, mse_interval_threshold: T) -> Self {
164        Self {
165            _internal: ICPConfiguration {
166                mse_interval_threshold,
167                ..self._internal
168            },
169        }
170    }
171
172    /// Generates an [`ICPConfiguration`] from the struct currently contained by the builder
173    ///
174    /// # Returns
175    /// An [`ICPConfiguration`], note that this does not consume the builder, leaving it intact for another use.
176    pub fn build(&self) -> ICPConfiguration<T> {
177        self._internal.clone()
178    }
179}
mapping_algorithms/point_clouds/icp/types.rs

mapping_algorithms/point_clouds/icp/
types.rs
