fast_mesh_scene/register.rs
1//! Rigid registration of one mesh to another: placements in and out of the
2//! [`fast_mesh_ops::icp()`] solver.
3//!
4//! The solver runs in the fixed mesh's frame: its stored positions, with its
5//! scale applied when it has one (on a baked copy), so that distances there
6//! are world distances.
7
8use fast_mesh_bvh::Bvh;
9use fast_mesh_ops::{IcpMotion, IcpOptions, IcpStatus, icp, sample_surface};
10use fast_mesh_query::Rigid;
11use fast_mesh_store::{Mesh, StoreError};
12
13use crate::baked::baked_mesh;
14use crate::placement::orthonormalize;
15use crate::{MeshId, Placement, Result, Scene, SceneError};
16
17/// Options for [`Scene::register`].
18#[derive(Debug, Clone, Copy, PartialEq)]
19pub struct RegisterOptions {
20 /// The solver's options: iterations, tolerance, overlap and maximum
21 /// distance, in world units.
22 pub icp: IcpOptions,
23 /// The most surface points of the moving mesh to use, evenly spaced over
24 /// its triangles' centroids and its vertices
25 /// ([`fast_mesh_ops::sample_surface`]); at least 1. The `rmse` is measured
26 /// on these. Default 100,000.
27 pub max_samples: usize,
28 /// Whether a good result becomes the moving mesh's placement, as one undo
29 /// step: it must converge and pass the guards in [`NotApplied`]. Default
30 /// true.
31 pub apply: bool,
32 /// The largest `rmse`, in world units, that may be applied; `None` means
33 /// [`DEFAULT_MAX_RMSE_FRACTION`] of the moving points' extent. Zero or
34 /// greater; `f64::INFINITY` turns the guard off.
35 pub max_rmse: Option<f64>,
36}
37
38/// The default [`RegisterOptions::max_rmse`], as a fraction of the moving
39/// points' extent. A wrong local minimum measured about 3%, and the same mesh
40/// with noise of 1% of its size correctly aligned at about 0.4% (`icp_survey`
41/// in `fast-mesh-ops`). Scans noisier than this need their own limit.
42pub const DEFAULT_MAX_RMSE_FRACTION: f64 = 0.01;
43
44/// Why a result was not applied.
45#[derive(Debug, Clone, Copy, PartialEq, Eq)]
46pub enum NotApplied {
47 /// [`RegisterOptions::apply`] was false.
48 NotRequested,
49 /// The status is not [`IcpStatus::Converged`].
50 NotConverged,
51 /// `rmse` is above [`RegisterOptions::max_rmse`]: most likely a wrong local
52 /// minimum, which a converged status does not rule out.
53 RmseAboveLimit,
54 /// Fewer than half of the points `overlap` expects were within
55 /// `max_distance`: the fit rests on a small patch.
56 TooFewInliers,
57}
58
59impl Default for RegisterOptions {
60 fn default() -> Self {
61 RegisterOptions {
62 icp: IcpOptions::default(),
63 max_samples: 100_000,
64 apply: true,
65 max_rmse: None,
66 }
67 }
68}
69
70/// What [`Scene::register`] found.
71#[derive(Debug, Clone, PartialEq)]
72pub struct RegisterReport {
73 /// The moving mesh's aligned placement, with its scale unchanged: the
74 /// converged result, or the best estimate when it did not converge.
75 pub placement: Placement,
76 /// The root mean square distance, in world units, from the used points to
77 /// the fixed mesh at `placement`; infinite when no point has a
78 /// correspondence.
79 pub rmse: f64,
80 /// Solver steps taken.
81 pub iterations: usize,
82 /// Whether the iteration settled and the data determine every motion
83 /// locally ([`fast_mesh_ops::icp()`] tests this by restarting from displaced
84 /// poses). That is not proof of the right alignment, since ICP is local and
85 /// can settle in a wrong minimum: judge that from `rmse`.
86 pub converged: bool,
87 /// Why iteration stopped.
88 pub status: IcpStatus,
89 /// Whether `placement` was set on the moving mesh.
90 pub applied: bool,
91 /// Why it was not; `None` exactly when `applied`.
92 pub not_applied: Option<NotApplied>,
93 /// The `rmse` limit that applied, in world units.
94 pub max_rmse: f64,
95 /// Surface points of the moving mesh sampled.
96 pub samples: usize,
97 /// Sampled points used at `placement`: the rest were trimmed or out of
98 /// range.
99 pub inliers: usize,
100 /// A warning of a weak motion, from geometry alone; see
101 /// [`fast_mesh_ops::IcpReport::conditioning`].
102 pub conditioning: f64,
103 /// The motions the data do not determine, in world coordinates; non-empty
104 /// only with [`IcpStatus::Underconstrained`]. The pose along them is
105 /// arbitrary.
106 pub undetermined: Vec<IcpMotion>,
107}
108
109/// `a` after `b`.
110fn compose(a: &Rigid, b: &Rigid) -> Rigid {
111 let (ra, rb) = (&a.rotation, &b.rotation);
112 let t = b.translation;
113 Rigid {
114 rotation: std::array::from_fn(|i| {
115 std::array::from_fn(|j| ra[i][0] * rb[0][j] + ra[i][1] * rb[1][j] + ra[i][2] * rb[2][j])
116 }),
117 translation: std::array::from_fn(|i| {
118 ra[i][0] * t[0] + ra[i][1] * t[1] + ra[i][2] * t[2] + a.translation[i]
119 }),
120 }
121}
122
123/// A motion in the frame `frame`, in world coordinates.
124fn motion_to_world(m: &IcpMotion, frame: &Rigid) -> IcpMotion {
125 let r = &frame.rotation;
126 let turn = |v: [f64; 3]| -> [f64; 3] {
127 std::array::from_fn(|i| r[i][0] * v[0] + r[i][1] * v[1] + r[i][2] * v[2])
128 };
129 let about = turn(m.about);
130 IcpMotion {
131 translation: turn(m.translation),
132 rotation: turn(m.rotation),
133 about: std::array::from_fn(|i| about[i] + frame.translation[i]),
134 alternative_rmse: m.alternative_rmse,
135 }
136}
137
138/// The moving mesh's sampled surface points as `scale * (origin + local)`.
139fn scaled_samples(mesh: &Mesh, placement: &Placement, max: usize) -> Result<Vec<[f64; 3]>> {
140 let origin = mesh.origin();
141 let (s, local) = (placement.scale, sample_surface(mesh, max)?);
142 let mut out = Vec::new();
143 out.try_reserve_exact(local.len())
144 .map_err(|_| SceneError::Store(StoreError::OutOfMemory))?;
145 out.extend(
146 local
147 .iter()
148 .map(|p| -> [f64; 3] { std::array::from_fn(|i| s[i] * (origin[i] + p[i])) }),
149 );
150 Ok(out)
151}
152
153impl Scene {
154 /// Rigidly aligns the mesh `moving` to the mesh `fixed` by point-to-plane
155 /// ICP ([`fast_mesh_ops::icp()`]), using points on `moving`'s surface and
156 /// `fixed`'s BVH.
157 ///
158 /// **The initial guess is the current placements.** ICP is local, so put
159 /// the meshes roughly in place first, with [`Scene::set_placement`]; a
160 /// good guess is off by a small part of the shape's features.
161 ///
162 /// **Applying.** With [`RegisterOptions::apply`] (the default) a good result
163 /// becomes `moving`'s placement as one undo step, or as part of the open
164 /// step. Good means converged, with `rmse` within
165 /// [`RegisterOptions::max_rmse`] and enough points in range ([`NotApplied`]
166 /// says which failed). Anything else changes nothing, whatever the option,
167 /// but its estimate is returned; set it with [`Scene::set_placement`] if it
168 /// will do.
169 ///
170 /// **Undetermined poses.** A sphere's spin, a cylinder's spin and a plane's
171 /// slide are not fixed by the data, though facets or noise can make the
172 /// iteration settle on some value with a tiny `rmse`. Those report
173 /// [`IcpStatus::Underconstrained`], not converged, and list the motions in
174 /// `undetermined`. Coarse regular meshes count too: a 16-sided sphere has
175 /// exact fits 22.5 degrees apart.
176 ///
177 /// **Scales.** Only rotation and translation are solved. `moving`'s scale
178 /// is applied to its points and kept as it is. `fixed`'s scale, uniform
179 /// or not, is followed exactly, on a copy of `fixed` baked through it with
180 /// its own BVH, so each such call costs a BVH build. Distances and options
181 /// are in world units either way.
182 ///
183 /// **Partial overlap.** Set [`IcpOptions::overlap`] a little below the
184 /// fraction of `moving` that lies over `fixed`, or nothing is rejected.
185 ///
186 /// Only `fixed`'s BVH is used, and only it is brought up to date.
187 ///
188 /// # Errors
189 /// [`SceneError::UnknownMesh`] for a stale id;
190 /// [`SceneError::InvalidInput`] when the ids are one mesh, `max_samples` is
191 /// zero or `max_rmse` is negative or NaN; [`SceneError::StaleBvh`] for a stale Manual `fixed`;
192 /// [`SceneError::Ops`] when a mesh has no triangles, an option is invalid,
193 /// or a placement takes the points out of `f32` range in `fixed`'s
194 /// frame. On error the scene is unchanged.
195 pub fn register(
196 &mut self,
197 moving: MeshId,
198 fixed: MeshId,
199 options: &RegisterOptions,
200 ) -> Result<RegisterReport> {
201 self.drop_field_unit();
202 self.entry(moving)?;
203 self.entry(fixed)?;
204 if moving == fixed {
205 return Err(SceneError::InvalidInput("cannot register a mesh to itself"));
206 }
207 if options.max_samples == 0 {
208 return Err(SceneError::InvalidInput("max_samples must be at least 1"));
209 }
210 if options.max_rmse.is_some_and(|v| v.is_nan() || v < 0.0) {
211 return Err(SceneError::InvalidInput("max_rmse must be zero or greater"));
212 }
213 self.prepare(Some(&[fixed]))?;
214 let (m, f) = (self.entry(moving)?, self.entry(fixed)?);
215 let points = scaled_samples(&m.mesh, &m.placement, options.max_samples)?;
216 let moving_scale = m.placement.scale;
217 // The moving vertices, once scaled, to the world.
218 let world = Rigid {
219 rotation: orthonormalize(m.placement.rotation),
220 translation: m.placement.translation,
221 };
222 // Frame to world: `R (S (o + p)) + t` with frame coordinates `S p`.
223 let (s, o) = (f.placement.scale, f.mesh.origin());
224 let r = orthonormalize(f.placement.rotation);
225 let scaled_origin: [f64; 3] = std::array::from_fn(|i| s[i] * o[i]);
226 let frame = Rigid {
227 rotation: r,
228 translation: std::array::from_fn(|i| {
229 r[i][0] * scaled_origin[0]
230 + r[i][1] * scaled_origin[1]
231 + r[i][2] * scaled_origin[2]
232 + f.placement.translation[i]
233 }),
234 };
235 let initial = compose(&frame.inverse(), &world);
236 let baked: Option<(Mesh, Bvh)> = if f.placement.is_scaled() {
237 Some(baked_mesh(
238 &f.mesh,
239 |p| std::array::from_fn(|i| s[i] * f64::from(p[i])),
240 f.bvh.options(),
241 )?)
242 } else {
243 None
244 };
245 let (mesh, bvh) = baked
246 .as_ref()
247 .map_or((&f.mesh, &f.bvh), |(mesh, bvh)| (mesh, bvh));
248 let solved = icp(&points, mesh, bvh, &initial, &options.icp)?;
249 let out = compose(&frame, &solved.transform);
250 let placement = Placement {
251 rotation: out.rotation,
252 translation: out.translation,
253 scale: moving_scale,
254 };
255 let converged = solved.converged();
256 let max_rmse = options
257 .max_rmse
258 .unwrap_or(DEFAULT_MAX_RMSE_FRACTION * solved.extent);
259 let expected = (options.icp.overlap * solved.samples as f64).ceil();
260 let not_applied = if !converged {
261 Some(NotApplied::NotConverged)
262 } else if !options.apply {
263 Some(NotApplied::NotRequested)
264 } else if solved.rmse > max_rmse {
265 Some(NotApplied::RmseAboveLimit)
266 } else if (solved.inliers as f64) * 2.0 < expected {
267 Some(NotApplied::TooFewInliers)
268 } else {
269 None
270 };
271 if not_applied.is_none() {
272 self.set_placement(moving, placement)?;
273 }
274 Ok(RegisterReport {
275 placement,
276 rmse: solved.rmse,
277 iterations: solved.iterations,
278 converged,
279 status: solved.status,
280 applied: not_applied.is_none(),
281 not_applied,
282 max_rmse,
283 samples: solved.samples,
284 inliers: solved.inliers,
285 conditioning: solved.conditioning,
286 undetermined: solved
287 .undetermined
288 .iter()
289 .map(|m| motion_to_world(m, &frame))
290 .collect(),
291 })
292 }
293}