modern_icp/
lib.rs

1//! A modern modular pure Rust implementation of the Iterative Closest Point algorithm.
2//!
3//! ## Example
4//!
5//! ```
6//! # use modern_icp::PointCloudPoint;
7//! # use modern_icp::icp::estimate_transform;
8//! # use modern_icp::correspondence::BidirectionalDistance;
9//! # use modern_icp::transform_estimation::point_to_plane_lls;
10//! # use modern_icp::convergence::same_squared_distance_error;
11//! # use modern_icp::reject_outliers::reject_n_sigma_dist;
12//! # use modern_icp::filter_points::accept_all;
13//! # use crate::modern_icp::correspondence::CorrespondenceEstimator;
14//! # use nalgebra::{Point3, Vector3};
15//! #
16//! # // Generate random point clouds
17//! # let mut alignee_cloud = Vec::with_capacity(100);
18//! # let mut target_cloud = Vec::with_capacity(100);
19//! #
20//! # // Generate 100 random points for alignee cloud
21//! # for _ in 0..100 {
22//! #
23//! #     let pos = Point3::new(
24//! #         rand::random::<f32>() * 10.0,
25//! #         rand::random::<f32>() * 10.0,
26//! #         rand::random::<f32>() * 10.0,
27//! #     );
28//! #     let norm = Vector3::new(
29//! #         rand::random::<f32>() * 2.0 - 1.0,
30//! #         rand::random::<f32>() * 2.0 - 1.0,
31//! #         rand::random::<f32>() * 2.0 - 1.0,
32//! #     ).normalize();
33//! #     alignee_cloud.push(PointCloudPoint::from_pos_norm(pos, norm));
34//! # }
35//! #
36//! # // Generate 100 random points for target cloud
37//! # for _ in 0..100 {
38//! #     let pos = Point3::new(
39//! #         rand::random::<f32>() * 10.0,
40//! #         rand::random::<f32>() * 10.0,
41//! #         rand::random::<f32>() * 10.0,
42//! #     );
43//! #     let norm = Vector3::new(
44//! #         rand::random::<f32>() * 2.0 - 1.0,
45//! #         rand::random::<f32>() * 2.0 - 1.0,
46//! #         rand::random::<f32>() * 2.0 - 1.0,
47//! #     ).normalize();
48//! #     target_cloud.push(PointCloudPoint::from_pos_norm(pos, norm));
49//! # }
50//! #
51//! let (alignee_transform, error_sum) = estimate_transform(
52//!     alignee_cloud,
53//!     &target_cloud,
54//!     20, // max iterations
55//!     BidirectionalDistance::new(&target_cloud),
56//!     accept_all,
57//!     reject_n_sigma_dist(3.0),
58//!     point_to_plane_lls::estimate_isometry,
59//!     same_squared_distance_error(1.0),
60//! );
61//! ```
62//!
63//! ## Integrations
64//!
65//! An integrations with the modelz crate is provided so you can use `Model3D` with the `estimate_transform` function.
66//!
67//! ```
68//! # use modern_icp::PointCloudPoint;
69//! # use modern_icp::icp::estimate_transform;
70//! # use modern_icp::correspondence::BidirectionalDistance;
71//! # use modern_icp::transform_estimation::point_to_plane_lls;
72//! # use modern_icp::convergence::same_squared_distance_error;
73//! # use modern_icp::reject_outliers::reject_n_sigma_dist;
74//! # use modern_icp::filter_points::accept_all;
75//! # use crate::modern_icp::correspondence::CorrespondenceEstimator;
76//! use modelz::Model3D;
77//!
78//! if let (Ok(alignee), Ok(target)) = (Model3D::load("alignee.gltf"), Model3D::load("target.stl")) {
79//!     let (transform, error_sum) = estimate_transform(
80//!         alignee,
81//!         &target,
82//!         20, // max iterations
83//!         BidirectionalDistance::new(&target),
84//!         accept_all,
85//!         reject_n_sigma_dist(3.0),
86//!         point_to_plane_lls::estimate_isometry,
87//!         same_squared_distance_error(1.0),
88//!     );
89//! }
90//! ```
91mod align;
92mod common;
93mod integrations;
94mod plane;
95mod point_cloud;
96
97pub use align::*;
98pub use common::*;
99#[allow(unused_imports)]
100pub use integrations::*;
101pub use plane::*;
102pub use point_cloud::*;
103
104#[cfg(feature = "rerun")]
105lazy_static::lazy_static! {
106    pub static ref RR: rerun::RecordingStream = rerun::RecordingStreamBuilder::new("shrink_to_fit_mesh").spawn().unwrap();
107}
108
109#[cfg(feature = "rerun")]
110fn pt3_array<T, const D: usize>(pt: nalgebra::Point<T, D>) -> [f32; 3]
111where
112    T: Clone + PartialEq + nalgebra::Scalar,
113    f32: From<T>,
114{
115    [
116        pt[0].clone().into(),
117        pt[1].clone().into(),
118        pt[2].clone().into(),
119    ]
120}
121
122#[cfg(feature = "rerun")]
123fn vec3_array<T, const D: usize>(vec: nalgebra::OVector<T, nalgebra::Const<D>>) -> [f32; 3]
124where
125    T: Clone + PartialEq + nalgebra::Scalar,
126    f32: From<T>,
127{
128    [
129        vec[0].clone().into(),
130        vec[1].clone().into(),
131        vec[2].clone().into(),
132    ]
133}
134
135#[cfg(feature = "rerun")]
136pub fn unit_quat_array<T>(quat: nalgebra::UnitQuaternion<T>) -> [f32; 4]
137where
138    T: Copy + PartialEq + nalgebra::SimdValue + nalgebra::Scalar + nalgebra::RealField,
139    f32: From<T>,
140{
141    let quat = quat.quaternion();
142    [quat.i.into(), quat.j.into(), quat.k.into(), quat.w.into()]
143}
144
145#[cfg(feature = "rerun")]
146pub fn rr_log_cloud<T>(name: &str, pt_cloud: &PointCloud<T, 3>)
147where
148    T: Copy + Clone + PartialEq + nalgebra::Scalar,
149    f32: From<T>,
150{
151    crate::RR
152        .log(
153            name,
154            &rerun::Points3D::new(
155                pt_cloud
156                    .iter()
157                    .map(|pt| pt3_array(pt.pos))
158                    .collect::<Vec<_>>(),
159            ),
160        )
161        .unwrap();
162}
modern_icp/lib.rs

modern_icp/
lib.rs
