nd_icp/
types.rs

1// Created by Indraneel on 7th Dec
2
3use nalgebra::{Const, Dyn, Matrix, OMatrix, Storage, U1, U4};
4use ply_rs::ply;
5
6/// Generic container for holding a set of points, enforces
7/// the type of points to implement the Point trait
8#[derive(Clone, Debug)]
9pub struct PointSet<T>
10where
11    T: Point,
12{
13    pub points: Vec<T>,
14}
15
16/// trait definition for a point, implement these functions
17/// for your point struct
18pub trait Point {
19    /// Number of dimensions for the point
20    fn get_dimensions(&self) -> usize;
21    /// Takes in a transformation matrix, applies it and updates the point
22    fn apply_transformation(&mut self, transformation: &OMatrix<f32, Dyn, Dyn>);
23    /// Euclidean distance squared between two points
24    fn find_distance_squared(&self, other_point: &Self) -> f32;
25    /// Converts the point into a vector container
26    fn to_vec(&self) -> Vec<f32>;
27    /// Builds the point struct from a 1 dimensional matrix
28    fn from_matrix<S>(matrix: &Matrix<f32, Const<1>, Dyn, S>) -> Self
29    where
30        S: Storage<f32, Const<1>, Dyn>;
31}
32
33/// Example 3D implementation of a point for use with ICP
34#[derive(Debug, Clone, Copy)]
35pub struct Point3D {
36    pub x: f32,
37    pub y: f32,
38    pub z: f32,
39}
40
41impl Point for Point3D {
42    fn get_dimensions(&self) -> usize {
43        3
44    }
45
46    fn apply_transformation(&mut self, transformation: &OMatrix<f32, Dyn, Dyn>) {
47        // Express point in homogenous coordinates
48        let homogenous_coordinates: OMatrix<f32, U4, U1> =
49            OMatrix::from([self.x, self.y, self.z, 1.0]);
50
51        let transformed_coordinates = transformation * homogenous_coordinates;
52
53        // Update point
54        self.x = transformed_coordinates[(0, 0)];
55        self.y = transformed_coordinates[(1, 0)];
56        self.z = transformed_coordinates[(2, 0)];
57    }
58
59    fn find_distance_squared(&self, other_point: &Self) -> f32 {
60        (other_point.x - self.x) * (other_point.x - self.x)
61            + (other_point.y - self.y) * (other_point.y - self.y)
62            + ((other_point.z - self.z) * (other_point.z - self.z))
63    }
64
65    fn to_vec(&self) -> Vec<f32> {
66        vec![self.x, self.y, self.z]
67    }
68
69    fn from_matrix<S>(matrix: &Matrix<f32, Const<1>, Dyn, S>) -> Self
70    where
71        S: Storage<f32, Const<1>, Dyn>,
72    {
73        if matrix.ncols() < 3 {
74            panic!("Matrix must have at least 3 columns to convert to Point3D");
75        }
76
77        Point3D {
78            x: matrix[(0, 0)],
79            y: matrix[(0, 1)],
80            z: matrix[(0, 2)],
81        }
82    }
83}
84
85impl ply::PropertyAccess for Point3D {
86    fn new() -> Self {
87        Point3D {
88            x: 0.0,
89            y: 0.0,
90            z: 0.0,
91        }
92    }
93    fn set_property(&mut self, key: String, property: ply::Property) {
94        match (key.as_ref(), property) {
95            ("x", ply::Property::Float(v)) => self.x = v,
96            ("y", ply::Property::Float(v)) => self.y = v,
97            ("z", ply::Property::Float(v)) => self.z = v,
98            (k, _) => panic!("Vertex: Unexpected key/value combination: key: {}", k),
99        }
100    }
101}
nd_icp/types.rs

nd_icp/
types.rs
