cumulus3d_align/
cloud.rs

1/*
2 * cloud.rs
3 *
4 * Copyright (c) 2026 ParkSangWoo
5 *
6 * Author(s):
7 *
8 *      ParkSangWoo <dev.parksangwoo@gmail.com>
9 *
10 *
11 * This file is part of cumulus3d.
12 *
13 * Licensed under the Apache License, Version 2.0 (the "License");
14 * you may not use this file except in compliance with the License.
15 * You may obtain a copy of the License at
16 *
17 *      http://www.apache.org/licenses/LICENSE-2.0
18 *
19 * Unless required by applicable law or agreed to in writing, software
20 * distributed under the License is distributed on an "AS IS" BASIS,
21 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
22 * See the License for the specific language governing permissions and
23 * limitations under the License.
24 *
25 * SPDX-License-Identifier: Apache-2.0
26 */
27
28//! Point cloud post-processing: Sim3 transform, proximity masking against the refined cloud, 1/N thinning, snapshot composition.
29
30use crate::kdtree::KdTree;
31use cumulus3d_core::io::PointCloud;
32use cumulus3d_core::{Sim3, Vec3};
33use rayon::prelude::*;
34
35/// Applies a Sim3 to points and normals (points: sRx+t, normals: Rn). Computed in f64.
36pub fn transform_cloud(cloud: &mut PointCloud, t: &Sim3) {
37    let r = t.rotation.to_rotation_matrix();
38    let sr = r * t.scale;
39    cloud.positions.par_iter_mut().for_each(|p| {
40        let v = sr * Vec3::new(p[0] as f64, p[1] as f64, p[2] as f64) + t.translation;
41        *p = [v.x as f32, v.y as f32, v.z as f32];
42    });
43    cloud.normals.par_iter_mut().for_each(|n| {
44        let v = r * Vec3::new(n[0] as f64, n[1] as f64, n[2] as f64);
45        *n = [v.x as f32, v.y as f32, v.z as f32];
46    });
47}
48
49/// New point cloud keeping only the points where `mask[i]` is true.
50pub fn select(cloud: &PointCloud, keep: &[bool]) -> PointCloud {
51    let pick = |v: &Vec<[f32; 3]>| -> Vec<[f32; 3]> {
52        if v.is_empty() {
53            Vec::new()
54        } else {
55            v.iter().zip(keep).filter(|(_, k)| **k).map(|(x, _)| *x).collect()
56        }
57    };
58    PointCloud {
59        positions: pick(&cloud.positions),
60        normals: pick(&cloud.normals),
61        colors: if cloud.colors.is_empty() {
62            Vec::new()
63        } else {
64            cloud.colors.iter().zip(keep).filter(|(_, k)| **k).map(|(c, _)| *c).collect()
65        },
66        keys: if cloud.keys.is_empty() { Vec::new() } else { cloud.keys.iter().zip(keep).filter(|(_, k)| **k).map(|(c, _)| *c).collect() },
67    }
68}
69
70/// Point cloud without the points within distance ≤ radius of `reference` (removes preview points that overlap refined points).
71pub fn remove_near(cloud: &PointCloud, reference: &KdTree, radius: f64) -> PointCloud {
72    let near = reference.any_within_many_f32(&cloud.positions, radius);
73    let keep: Vec<bool> = near.iter().map(|b| !b).collect();
74    select(cloud, &keep)
75}
76
77/// Takes every stride-th point (indices 0, stride, 2·stride, …). Unchanged if stride ≤ 1.
78pub fn decimate(cloud: &PointCloud, stride: usize) -> PointCloud {
79    if stride <= 1 {
80        return cloud.clone();
81    }
82    let keep: Vec<bool> = (0..cloud.len()).map(|i| i % stride == 0).collect();
83    select(cloud, &keep)
84}
85
86/// Concatenation. If only some clouds have normals/colors, the missing ones are filled with (0,0,0) normals / white.
87/// Keys are kept only if every non-empty cloud has them.
88pub fn merge_clouds(clouds: &[&PointCloud]) -> PointCloud {
89    let total: usize = clouds.iter().map(|c| c.len()).sum();
90    let any_n = clouds.iter().any(|c| c.has_normals());
91    let any_c = clouds.iter().any(|c| c.has_colors());
92    let mut out = PointCloud {
93        positions: Vec::with_capacity(total),
94        normals: Vec::with_capacity(if any_n { total } else { 0 }),
95        colors: Vec::with_capacity(if any_c { total } else { 0 }),
96        keys: Vec::new(),
97    };
98    // Keys are concatenated only if every cloud has them (if only some do, alignment would break, so they are dropped).
99    let all_k = !clouds.is_empty() && clouds.iter().all(|c| c.is_empty() || c.has_keys());
100    if all_k && clouds.iter().any(|c| c.has_keys()) {
101        out.keys.reserve(total);
102    }
103    for c in clouds {
104        out.positions.extend_from_slice(&c.positions);
105        if all_k && c.has_keys() {
106            out.keys.extend_from_slice(&c.keys);
107        }
108        if any_n {
109            if c.has_normals() {
110                out.normals.extend_from_slice(&c.normals);
111            } else {
112                out.normals.extend(std::iter::repeat_n([0.0f32; 3], c.len()));
113            }
114        }
115        if any_c {
116            if c.has_colors() {
117                out.colors.extend_from_slice(&c.colors);
118            } else {
119                out.colors.extend(std::iter::repeat_n([255u8; 3], c.len()));
120            }
121        }
122    }
123    out
124}
125
126/// Snapshot composition options.
127#[derive(Clone, Debug, PartialEq)]
128pub struct SnapshotOptions {
129    /// Remove preview points within this distance (m) of a refined point.
130    pub mask_radius: f64,
131    /// Final 1/N thinning.
132    pub stride: usize,
133}
134
135impl Default for SnapshotOptions {
136    fn default() -> Self {
137        Self { mask_radius: 1.5, stride: 6 }
138    }
139}
140
141/// Snapshot = all refined zones + the preview zones (not overlapping refined points), then 1/N thinning.
142/// All inputs must already be in the same coordinate frame.
143pub fn compose_snapshot(fine: &[&PointCloud], coarse: &[&PointCloud], opts: &SnapshotOptions) -> PointCloud {
144    let fine_all = merge_clouds(fine);
145    let masked: Vec<PointCloud> = if fine_all.is_empty() {
146        coarse.iter().map(|c| (*c).clone()).collect()
147    } else {
148        let tree = KdTree::from_f32(&fine_all.positions);
149        coarse.iter().map(|c| remove_near(c, &tree, opts.mask_radius)).collect()
150    };
151    let mut parts: Vec<&PointCloud> = vec![&fine_all];
152    parts.extend(masked.iter());
153    // Design decision: timing of 1/N thinning. Applied once to the whole merged set after masking.
154    decimate(&merge_clouds(&parts), opts.stride)
155}
156
157#[cfg(test)]
158mod tests {
159    use super::*;
160    use cumulus3d_core::Quat;
161
162    fn grid(n: usize, step: f32, z: f32) -> PointCloud {
163        let mut c = PointCloud::default();
164        for i in 0..n {
165            for j in 0..n {
166                c.positions.push([i as f32 * step, j as f32 * step, z]);
167                c.normals.push([0.0, 0.0, 1.0]);
168                c.colors.push([i as u8, j as u8, 0]);
169            }
170        }
171        c
172    }
173
174    #[test]
175    fn mask_within_1_5m() {
176        // Refined: grid on x ∈ [0, 10] (z=0). Preview: points at various z heights, only those within 1.5 m are removed.
177        let fine = grid(11, 1.0, 0.0);
178        let mut coarse = PointCloud::default();
179        for z in [0.5f32, 1.4, 1.5, 1.6, 3.0] {
180            coarse.positions.push([5.0, 5.0, z]);
181        }
182        coarse.positions.push([20.0, 5.0, 0.0]); // far away horizontally
183        coarse.positions.push([11.4, 5.0, 0.0]); // 1.4 m from the edge
184        let tree = KdTree::from_f32(&fine.positions);
185        let out = remove_near(&coarse, &tree, 1.5);
186        assert_eq!(out.positions, vec![[5.0, 5.0, 1.6], [5.0, 5.0, 3.0], [20.0, 5.0, 0.0]]);
187    }
188
189    #[test]
190    fn transform_points_and_normals() {
191        let mut c = grid(3, 1.0, 2.0);
192        let t = Sim3::new(2.0, Quat::from_axis_angle(&Vec3::x(), std::f64::consts::FRAC_PI_2), Vec3::new(1.0, 0.0, 0.0));
193        let orig = c.clone();
194        transform_cloud(&mut c, &t);
195        for (a, b) in orig.positions.iter().zip(&c.positions) {
196            let e = t.transform_point(&Vec3::new(a[0] as f64, a[1] as f64, a[2] as f64));
197            assert!((e - Vec3::new(b[0] as f64, b[1] as f64, b[2] as f64)).norm() < 1e-5);
198        }
199        // z normal, rotate 90° about the x axis, giving −y.
200        let n = c.normals[0];
201        assert!((n[1] + 1.0).abs() < 1e-6 && n[2].abs() < 1e-6);
202    }
203
204    #[test]
205    fn decimate_and_snapshot() {
206        let c = grid(4, 1.0, 0.0);
207        assert_eq!(decimate(&c, 6).len(), 3); // 16 → 0, 6, 12
208        let fine = grid(5, 1.0, 0.0);
209        let mut coarse = grid(10, 1.0, 0.0); // 0..9; points with x,y ≤ 6 are within 1.5 m of the refined ones (0..4)
210        coarse.normals.clear();
211        let snap = compose_snapshot(&[&fine], &[&coarse], &SnapshotOptions { mask_radius: 1.5, stride: 1 });
212        let kept_coarse = coarse
213            .positions
214            .iter()
215            .filter(|p| {
216                let dx = (p[0] - 4.0).max(0.0);
217                let dy = (p[1] - 4.0).max(0.0);
218                dx * dx + dy * dy > 2.25
219            })
220            .count();
221        assert_eq!(snap.len(), 25 + kept_coarse);
222        assert_eq!(snap.normals.len(), snap.len());
223        assert_eq!(snap.normals[snap.len() - 1], [0.0; 3]);
224    }
225}
cumulus3d_align/cloud.rs

cumulus3d_align/
cloud.rs
