Struct Icp 

pub struct Icp<T>
where
    T: Point + Copy,
{ /* private fields */ }

N dimensional Icp

Salient features

1. Generic Type can be any n dimensional point
2. Implement Point Trait for your point
3. Uses SVD to find rotation and translation
4. Vectorised operations to control time complexity

Examples

let max_iterations = 100;
let cost_change_threshold = 1e-5;
let icp = Icp::new(
   model_point_set.clone(),
   max_iterations,
   cost_change_threshold,
);
let result = icp.register(&target_point_set);

TODO:

1. Outlier rejection of input data
2. Voxel binning to make finding correspondences faster

Implementations

impl<T> Icp<T>

where T: Point + Copy,

pub fn new( model_point_set: PointSet<T>, max_iterations: i32, cost_change_threshold: f32, ) -> Self

pub fn register(&self, target_point_set: &PointSet<T>) -> OMatrix<f32, Dyn, Dyn>

ICP registration

1. Initialises the transformation given number of dimensions
2. For each point
   • Find closest point in reference cloud
3. Remove the means
4. Find transformaton and rotation which will minimise the error
5. Transform the target cloud
6. Iterate until within error threshold or max iterations