spatialrust_registration/
fpfh.rs

1//! Fast Point Feature Histograms (FPFH) and FPFH-based global registration.
2//!
3//! Global registration recovers a coarse alignment *without* an initial guess,
4//! which the local refiners (ICP/GICP/NDT) need. It describes each point by an
5//! FPFH descriptor, matches descriptors between the clouds, and runs a RANSAC
6//! loop that samples correspondences, estimates a rigid transform, and keeps the
7//! pose with the most inliers.
8
9use spatialrust_core::{HasNormals3, HasPositions3, PointCloud, SpatialError, SpatialResult};
10use spatialrust_math::{Isometry3, TransformPoint, Vec3};
11use spatialrust_search::{KdTree, RadiusSearchIndex};
12
13use crate::kabsch::estimate_rigid_transform;
14use crate::registration::{PointCloudRegistration, RegistrationResult};
15
16/// Number of bins per angular feature; the full descriptor is `3 * BINS`.
17const BINS: usize = 11;
18/// FPFH descriptor dimensionality (`alpha`, `phi`, `theta` histograms).
19const FPFH_DIM: usize = 3 * BINS;
20
21/// Length of an FPFH descriptor (33: three 11-bin angular histograms).
22pub const FPFH_DESCRIPTOR_LEN: usize = FPFH_DIM;
23
24/// A single FPFH descriptor.
25pub type FpfhDescriptor = [f32; FPFH_DIM];
26
27/// Internal shorthand kept for the existing call sites.
28type Descriptor = FpfhDescriptor;
29
30/// Computes an FPFH descriptor for every point in `cloud`.
31///
32/// The cloud must carry normals. `feature_radius` is the neighborhood radius
33/// used to build each descriptor (≈5× the point spacing is a good start). This
34/// is the reusable building block behind [`FpfhRansacRegistration`]; compute it
35/// once on a keypoint cloud to drive descriptor-based matching cheaply.
36pub fn fpfh_descriptors(
37    cloud: &PointCloud,
38    feature_radius: f32,
39) -> SpatialResult<Vec<FpfhDescriptor>> {
40    if feature_radius <= 0.0 || feature_radius.is_nan() {
41        return Err(SpatialError::InvalidArgument("feature_radius must be positive".to_owned()));
42    }
43    let set = PointSet::from_cloud(cloud)?;
44    Ok(compute_fpfh(&set, feature_radius))
45}
46
47/// Configuration for [`FpfhRansacRegistration`].
48#[derive(Clone, Copy, Debug, PartialEq)]
49pub struct FpfhRansacConfig {
50    /// Radius used to gather neighbors when building FPFH descriptors. Should be
51    /// noticeably larger than the normal-estimation radius (≈5× point spacing).
52    pub feature_radius: f32,
53    /// Distance below which a transformed source point counts as an inlier.
54    pub max_correspondence_distance: f32,
55    /// Number of RANSAC sampling iterations.
56    pub ransac_iterations: usize,
57    /// Correspondences per RANSAC sample (minimum 3 for a rigid transform).
58    pub sample_size: usize,
59    /// Pairwise edge lengths in a sample must agree within this relative
60    /// tolerance, which cheaply rejects geometrically inconsistent samples.
61    pub edge_length_tolerance: f32,
62    /// Seed for the deterministic RNG, so runs are reproducible.
63    pub seed: u64,
64}
65
66impl Default for FpfhRansacConfig {
67    fn default() -> Self {
68        Self {
69            feature_radius: 0.25,
70            max_correspondence_distance: 0.075,
71            ransac_iterations: 4000,
72            sample_size: 3,
73            edge_length_tolerance: 0.9,
74            seed: 0x5eed,
75        }
76    }
77}
78
79impl FpfhRansacConfig {
80    /// Creates a config from the feature radius and inlier distance.
81    #[must_use]
82    pub fn with_radius(feature_radius: f32, max_correspondence_distance: f32) -> Self {
83        Self { feature_radius, max_correspondence_distance, ..Self::default() }
84    }
85}
86
87/// FPFH + RANSAC global registration.
88///
89/// Both `source` and `target` must carry normals (e.g. from normal estimation);
90/// FPFH is built from the angular relationships between a point's normal and its
91/// neighbors' normals, so normals are mandatory.
92#[derive(Clone, Copy, Debug, PartialEq)]
93pub struct FpfhRansacRegistration {
94    config: FpfhRansacConfig,
95}
96
97impl FpfhRansacRegistration {
98    /// Creates a registration from config.
99    #[must_use]
100    pub const fn new(config: FpfhRansacConfig) -> Self {
101        Self { config }
102    }
103
104    /// Returns the registration config.
105    #[must_use]
106    pub const fn config(&self) -> FpfhRansacConfig {
107        self.config
108    }
109}
110
111impl PointCloudRegistration for FpfhRansacRegistration {
112    fn name(&self) -> &'static str {
113        "FpfhRansacRegistration"
114    }
115
116    fn align(&self, source: &PointCloud, target: &PointCloud) -> SpatialResult<RegistrationResult> {
117        if self.config.feature_radius <= 0.0 || self.config.feature_radius.is_nan() {
118            return Err(SpatialError::InvalidArgument(
119                "feature_radius must be positive".to_owned(),
120            ));
121        }
122        if self.config.sample_size < 3 {
123            return Err(SpatialError::InvalidArgument("sample_size must be at least 3".to_owned()));
124        }
125        if source.is_empty() || target.is_empty() {
126            return Err(SpatialError::InvalidArgument(
127                "source and target must be non-empty".to_owned(),
128            ));
129        }
130
131        let src = PointSet::from_cloud(source)?;
132        let tgt = PointSet::from_cloud(target)?;
133
134        let src_features = compute_fpfh(&src, self.config.feature_radius);
135        let tgt_features = compute_fpfh(&tgt, self.config.feature_radius);
136
137        // For each source point, its best feature-space match in the target.
138        let matches: Vec<usize> =
139            src_features.iter().map(|feature| nearest_feature(feature, &tgt_features)).collect();
140
141        self.ransac(&src, &tgt, &matches)
142    }
143}
144
145impl FpfhRansacRegistration {
146    fn ransac(
147        &self,
148        src: &PointSet,
149        tgt: &PointSet,
150        matches: &[usize],
151    ) -> SpatialResult<RegistrationResult> {
152        let n = src.points.len();
153        let max_sq = self.config.max_correspondence_distance.powi(2);
154        let mut rng = Lcg::new(self.config.seed);
155
156        let mut best_inliers = 0_usize;
157        let mut best_error = f64::INFINITY;
158        let mut best_transform = Isometry3::<f32>::identity();
159
160        for _ in 0..self.config.ransac_iterations {
161            // Draw `sample_size` distinct source indices.
162            let mut sample = Vec::with_capacity(self.config.sample_size);
163            let mut attempts = 0;
164            while sample.len() < self.config.sample_size && attempts < self.config.sample_size * 8 {
165                let idx = (rng.next_u32() as usize) % n;
166                if !sample.contains(&idx) {
167                    sample.push(idx);
168                }
169                attempts += 1;
170            }
171            if sample.len() < self.config.sample_size {
172                continue;
173            }
174
175            let sample_src: Vec<Vec3<f32>> = sample.iter().map(|&i| src.points[i]).collect();
176            let sample_tgt: Vec<Vec3<f32>> =
177                sample.iter().map(|&i| tgt.points[matches[i]]).collect();
178
179            if !edge_lengths_consistent(&sample_src, &sample_tgt, self.config.edge_length_tolerance)
180            {
181                continue;
182            }
183
184            let Some(transform) = estimate_rigid_transform(&sample_src, &sample_tgt) else {
185                continue;
186            };
187
188            // Score the candidate over all feature matches.
189            let mut inliers = 0_usize;
190            let mut error = 0.0_f64;
191            for (i, &match_idx) in matches.iter().enumerate() {
192                let moved = transform.transform_point(src.points[i]);
193                let dist_sq = (moved - tgt.points[match_idx]).length_squared();
194                if dist_sq <= max_sq {
195                    inliers += 1;
196                    error += f64::from(dist_sq);
197                }
198            }
199
200            let mean_error = if inliers > 0 { error / inliers as f64 } else { f64::INFINITY };
201            // Prefer more inliers; break ties by lower mean inlier error.
202            if inliers > best_inliers || (inliers == best_inliers && mean_error < best_error) {
203                best_inliers = inliers;
204                best_error = mean_error;
205                best_transform = transform;
206            }
207        }
208
209        Ok(RegistrationResult {
210            transform: best_transform,
211            fitness: if best_inliers > 0 { best_error } else { f64::INFINITY },
212            iterations: self.config.ransac_iterations,
213            converged: best_inliers >= self.config.sample_size,
214        })
215    }
216}
217
218/// Positions + normals lifted out of a cloud into contiguous `Vec3` storage.
219struct PointSet {
220    points: Vec<Vec3<f32>>,
221    normals: Vec<Vec3<f32>>,
222    tree: KdTree,
223}
224
225impl PointSet {
226    fn from_cloud(cloud: &PointCloud) -> SpatialResult<Self> {
227        let (x, y, z) = cloud.positions3()?;
228        let (nx, ny, nz) = cloud.normals3()?;
229        let points: Vec<Vec3<f32>> = (0..x.len()).map(|i| Vec3::new(x[i], y[i], z[i])).collect();
230        let normals: Vec<Vec3<f32>> =
231            (0..nx.len()).map(|i| Vec3::new(nx[i], ny[i], nz[i])).collect();
232        let tree = KdTree::from_slices(x, y, z);
233        Ok(Self { points, normals, tree })
234    }
235}
236
237/// Computes the three Darboux-frame angular features between an anchor point
238/// `(p, n_p)` and a neighbor `(q, n_q)`: returns `(alpha, phi, theta)`.
239fn darboux(p: Vec3<f32>, np: Vec3<f32>, q: Vec3<f32>, nq: Vec3<f32>) -> Option<(f32, f32, f32)> {
240    let diff = q - p;
241    let dist = diff.length();
242    if dist < 1e-9 {
243        return None;
244    }
245    let u = np;
246    let pq = diff.normalize();
247    let v = pq.cross(u);
248    let v_len = v.length();
249    if v_len < 1e-9 {
250        return None;
251    }
252    let v = v.normalize();
253    let w = u.cross(v);
254
255    let alpha = v.dot(nq); // [-1, 1]
256    let phi = u.dot(pq); // [-1, 1]
257    let theta = w.dot(nq).atan2(u.dot(nq)); // [-pi, pi]
258    Some((alpha, phi, theta))
259}
260
261/// Bins a value from `[lo, hi]` into `BINS` buckets.
262fn bin_index(value: f32, lo: f32, hi: f32) -> usize {
263    let t = ((value - lo) / (hi - lo)).clamp(0.0, 0.999_999);
264    (t * BINS as f32) as usize
265}
266
267/// Builds the per-point Simplified Point Feature Histograms (SPFH).
268fn compute_spfh(set: &PointSet, radius: f32) -> Vec<Descriptor> {
269    let mut spfh = vec![[0.0_f32; FPFH_DIM]; set.points.len()];
270    for (i, spfh_i) in spfh.iter_mut().enumerate() {
271        let p = set.points[i];
272        let np = set.normals[i];
273        let neighbors = set.tree.radius_search(p.x, p.y, p.z, radius);
274        let mut count = 0_u32;
275        for neighbor in neighbors {
276            let j = neighbor.index;
277            if j == i {
278                continue;
279            }
280            let Some((alpha, phi, theta)) = darboux(p, np, set.points[j], set.normals[j]) else {
281                continue;
282            };
283            spfh_i[bin_index(alpha, -1.0, 1.0)] += 1.0;
284            spfh_i[BINS + bin_index(phi, -1.0, 1.0)] += 1.0;
285            spfh_i[2 * BINS + bin_index(theta, -std::f32::consts::PI, std::f32::consts::PI)] += 1.0;
286            count += 1;
287        }
288        if count > 0 {
289            // Normalize each sub-histogram to percentages.
290            for sub in 0..3 {
291                let slice = &mut spfh_i[sub * BINS..(sub + 1) * BINS];
292                let sum: f32 = slice.iter().sum();
293                if sum > 0.0 {
294                    for bin in slice {
295                        *bin = *bin / sum * 100.0;
296                    }
297                }
298            }
299        }
300    }
301    spfh
302}
303
304/// Builds FPFH descriptors by distance-weighting each point's neighbors' SPFH.
305fn compute_fpfh(set: &PointSet, radius: f32) -> Vec<Descriptor> {
306    let spfh = compute_spfh(set, radius);
307    let mut fpfh = vec![[0.0_f32; FPFH_DIM]; set.points.len()];
308    for (i, fpfh_i) in fpfh.iter_mut().enumerate() {
309        let p = set.points[i];
310        let neighbors = set.tree.radius_search(p.x, p.y, p.z, radius);
311        let mut sum_weight = 0.0_f32;
312        let mut acc = [0.0_f32; FPFH_DIM];
313        for neighbor in neighbors {
314            let j = neighbor.index;
315            if j == i {
316                continue;
317            }
318            let dist = neighbor.distance_squared.sqrt();
319            if dist < 1e-9 {
320                continue;
321            }
322            let weight = 1.0 / dist;
323            sum_weight += weight;
324            for bin in 0..FPFH_DIM {
325                acc[bin] += weight * spfh[j][bin];
326            }
327        }
328        for bin in 0..FPFH_DIM {
329            fpfh_i[bin] = spfh[i][bin] + if sum_weight > 0.0 { acc[bin] / sum_weight } else { 0.0 };
330        }
331    }
332    fpfh
333}
334
335/// Brute-force nearest descriptor (squared L2) in feature space.
336fn nearest_feature(feature: &Descriptor, candidates: &[Descriptor]) -> usize {
337    let mut best = 0_usize;
338    let mut best_dist = f32::INFINITY;
339    for (idx, candidate) in candidates.iter().enumerate() {
340        let mut dist = 0.0_f32;
341        for bin in 0..FPFH_DIM {
342            let d = feature[bin] - candidate[bin];
343            dist += d * d;
344            if dist >= best_dist {
345                break;
346            }
347        }
348        if dist < best_dist {
349            best_dist = dist;
350            best = idx;
351        }
352    }
353    best
354}
355
356/// Checks that pairwise edge lengths in the source sample match the target
357/// sample within `tolerance` (a relative ratio), rejecting distorted samples.
358fn edge_lengths_consistent(src: &[Vec3<f32>], tgt: &[Vec3<f32>], tolerance: f32) -> bool {
359    for a in 0..src.len() {
360        for b in (a + 1)..src.len() {
361            let ls = (src[a] - src[b]).length();
362            let lt = (tgt[a] - tgt[b]).length();
363            let lo = ls.min(lt);
364            let hi = ls.max(lt);
365            if hi < 1e-9 {
366                continue;
367            }
368            if lo / hi < tolerance {
369                return false;
370            }
371        }
372    }
373    true
374}
375
376/// Small deterministic xorshift-style PRNG (avoids a `rand` dependency).
377struct Lcg {
378    state: u64,
379}
380
381impl Lcg {
382    fn new(seed: u64) -> Self {
383        // Avoid a zero state, which xorshift cannot escape.
384        Self { state: seed ^ 0x9e37_79b9_7f4a_7c15 }
385    }
386
387    fn next_u32(&mut self) -> u32 {
388        let mut x = self.state;
389        x ^= x << 13;
390        x ^= x >> 7;
391        x ^= x << 17;
392        self.state = x;
393        (x >> 32) as u32
394    }
395}
396
397#[cfg(test)]
398mod tests {
399    use super::{FpfhRansacConfig, FpfhRansacRegistration};
400    use crate::registration::PointCloudRegistration;
401    use spatialrust_core::{DType, FieldSemantic, PointCloudBuilder, PointField, PointSchema};
402    use spatialrust_math::{Isometry3, Quat, TransformPoint, Vec3};
403
404    fn schema_with_normals() -> PointSchema {
405        PointSchema::new()
406            .with_field(PointField::scalar("x", FieldSemantic::PositionX, DType::F32))
407            .with_field(PointField::scalar("y", FieldSemantic::PositionY, DType::F32))
408            .with_field(PointField::scalar("z", FieldSemantic::PositionZ, DType::F32))
409            .with_field(PointField::scalar("normal_x", FieldSemantic::NormalX, DType::F32))
410            .with_field(PointField::scalar("normal_y", FieldSemantic::NormalY, DType::F32))
411            .with_field(PointField::scalar("normal_z", FieldSemantic::NormalZ, DType::F32))
412    }
413
414    /// Two angled planes meeting at an edge, with analytic normals. The corner
415    /// gives FPFH something distinctive to latch onto.
416    fn corner_cloud() -> (Vec<Vec3<f32>>, Vec<Vec3<f32>>) {
417        let mut points = Vec::new();
418        let mut normals = Vec::new();
419        for i in 0..20 {
420            for j in 0..20 {
421                let (a, b) = (i as f32 * 0.05, j as f32 * 0.05);
422                // floor (normal +z)
423                points.push(Vec3::new(a, b, 0.0));
424                normals.push(Vec3::new(0.0, 0.0, 1.0));
425                // wall (normal +y)
426                points.push(Vec3::new(a, 0.0, b + 0.05));
427                normals.push(Vec3::new(0.0, 1.0, 0.0));
428            }
429        }
430        (points, normals)
431    }
432
433    fn build_cloud(points: &[Vec3<f32>], normals: &[Vec3<f32>]) -> spatialrust_core::PointCloud {
434        let mut builder = PointCloudBuilder::new(schema_with_normals());
435        for (p, n) in points.iter().zip(normals) {
436            builder.push_point([p.x, p.y, p.z, n.x, n.y, n.z]).unwrap();
437        }
438        builder.build().unwrap()
439    }
440
441    #[test]
442    fn recovers_coarse_alignment_without_initial_guess() {
443        let (points, normals) = corner_cloud();
444        let target = build_cloud(&points, &normals);
445
446        // Apply a sizeable yaw + translation that ICP alone could not recover.
447        let misalign = Isometry3::new(
448            Quat::from_axis_angle(Vec3::new(0.0, 0.0, 1.0), 0.6),
449            Vec3::new(0.3, -0.2, 0.1),
450        );
451        let moved_points: Vec<Vec3<f32>> =
452            points.iter().map(|p| misalign.transform_point(*p)).collect();
453        let moved_normals: Vec<Vec3<f32>> =
454            normals.iter().map(|n| misalign.transform_point(*n) - misalign.translation()).collect();
455        let source = build_cloud(&moved_points, &moved_normals);
456
457        let config = FpfhRansacConfig {
458            feature_radius: 0.2,
459            max_correspondence_distance: 0.05,
460            ransac_iterations: 6000,
461            seed: 7,
462            ..FpfhRansacConfig::default()
463        };
464        let result = FpfhRansacRegistration::new(config).align(&source, &target).unwrap();
465        assert!(result.converged);
466
467        // Global registration yields a *coarse* pose meant to seed ICP/GICP, so
468        // the residual only needs to land within their convergence basin (a few
469        // point spacings, here 0.05), not at sub-voxel accuracy.
470        let probe = moved_points[123];
471        let restored = result.transform.transform_point(probe);
472        let expected = points[123];
473        let err = (restored - expected).length();
474        assert!(err < 0.15, "coarse alignment residual too large: {err}");
475    }
476
477    #[test]
478    fn rejects_bad_params() {
479        let (points, normals) = corner_cloud();
480        let cloud = build_cloud(&points, &normals);
481        let config = FpfhRansacConfig { feature_radius: 0.0, ..FpfhRansacConfig::default() };
482        assert!(FpfhRansacRegistration::new(config).align(&cloud, &cloud).is_err());
483    }
484
485    #[test]
486    fn public_descriptors_have_expected_shape() {
487        use super::{fpfh_descriptors, FPFH_DESCRIPTOR_LEN};
488        let (points, normals) = corner_cloud();
489        let cloud = build_cloud(&points, &normals);
490        let descriptors = fpfh_descriptors(&cloud, 0.2).unwrap();
491        assert_eq!(descriptors.len(), cloud.len());
492        assert_eq!(FPFH_DESCRIPTOR_LEN, 33);
493        // Each descriptor's three sub-histograms are normalized to ~100 each,
494        // so a non-degenerate point's bins should sum to a positive value.
495        let total: f32 = descriptors[descriptors.len() / 2].iter().sum();
496        assert!(total > 0.0);
497    }
498}
spatialrust_registration/fpfh.rs

spatialrust_registration/
fpfh.rs
