Skip to main content

cumulus3d_cli/
post.rs

1/*
2 * post.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//! Post-processing: performs the Python part at the end of the script (preview alignment, per-event snapshots, manifest) and
29//! incremental_reanchor.py
30//! with the align crate. The model uses the in-memory Reconstruction as is.
31
32use crate::util::{py_float, Json, Logger};
33use cumulus3d_align::shared::NameFilter;
34use cumulus3d_align::RobustUmeyamaOptions;
35use cumulus3d_align::{
36    compose_snapshot, decimate, merge_clouds, robust_umeyama, select, shared_point_correspondences, transform_cloud, KdTree,
37    SharedPointOptions, SnapshotOptions,
38};
39use cumulus3d_core::io::{read_ply, write_ply, PlyLayout, PointCloud};
40use cumulus3d_core::{Reconstruction, Sim3, Vec3};
41use rayon::prelude::*;
42use std::collections::HashSet;
43use std::collections::{BTreeMap, BTreeSet, HashMap};
44use std::hash::{BuildHasherDefault, Hasher};
45use std::path::{Path, PathBuf};
46use std::sync::Arc;
47
48/// Post-processing input (based on PLY paths and event strings).
49pub struct PostInput<'a> {
50    /// Output folder.
51    pub out: &'a Path,
52    /// Zone size (number of positions).
53    pub span: usize,
54    /// Zone overlap (number of positions).
55    pub ovl: usize,
56    /// (seconds from the first event, event string).
57    pub events: Vec<(f64, String)>,
58    /// Per-zone preview models.
59    pub previews: &'a BTreeMap<usize, Reconstruction>,
60    /// Per-zone refined models.
61    pub refined: &'a BTreeMap<usize, Reconstruction>,
62    /// Per-zone preview PLY paths.
63    pub preview_ply: &'a BTreeMap<usize, PathBuf>,
64    /// Per-zone refined PLY paths.
65    pub refined_ply: &'a BTreeMap<usize, PathBuf>,
66    /// Image name -> position.
67    pub position: &'a (dyn Fn(&str) -> Option<usize> + Sync),
68}
69
70/// Python `tof`: the time of the first event whose first words match or which shares the prefix.
71fn tof(events: &[(f64, String)], pat: &str) -> Option<f64> {
72    events.iter().find(|(_, s)| s.split(" pos[").next() == Some(pat) || s.starts_with(pat)).map(|(t, _)| *t)
73}
74
75/// Robust Umeyama result (s, R, t, residual median, inlier count).
76pub(crate) struct Fit {
77    pub(crate) sim3: Sim3,
78    pub(crate) median: f64,
79    pub(crate) inliers: usize,
80}
81
82/// Fit a_to_b using the shared 3D points of two models (same-name images, same 2D index).
83pub(crate) fn corr_points(a: &Reconstruction, b: &Reconstruction, filter: Option<NameFilter>) -> (Vec<Vec3>, Vec<Vec3>) {
84    let opts = SharedPointOptions { position_range: None, name_filter: filter };
85    let pairs = shared_point_correspondences(a, b, &opts);
86    let mut pa = Vec::with_capacity(pairs.len());
87    let mut pb = Vec::with_capacity(pairs.len());
88    for (i, j) in pairs {
89        if let (Some(x), Some(y)) = (a.point3d(i), b.point3d(j)) {
90            pa.push(x.xyz);
91            pb.push(y.xyz);
92        }
93    }
94    (pa, pb)
95}
96
97pub(crate) fn fit(pa: &[Vec3], pb: &[Vec3]) -> Option<Fit> {
98    let r = robust_umeyama(pa, pb, &RobustUmeyamaOptions::default())?;
99    Some(Fit { sim3: r.sim3, median: r.median_residual, inliers: r.num_inliers })
100}
101
102/// Robust Sim3 taking `a` into the frame of `b` from their shared 3D points, restricted by `filter` when given. Returns the
103/// fit and the number of shared pairs, or None with fewer than `min_pairs` pairs (or when the fit fails).
104pub(crate) fn fit_shared_points(
105    a: &Reconstruction,
106    b: &Reconstruction,
107    filter: Option<NameFilter>,
108    min_pairs: usize,
109) -> Option<(Fit, usize)> {
110    let (pa, pb) = corr_points(a, b, filter);
111    if pa.len() < min_pairs {
112        return None;
113    }
114    fit(&pa, &pb).map(|f| (f, pa.len()))
115}
116
117fn transformed(c: &PointCloud, t: &Sim3) -> PointCloud {
118    let mut c = c.clone();
119    transform_cloud(&mut c, t);
120    c
121}
122
123fn write(path: &Path, c: &PointCloud) -> Result<(), String> {
124    if let Some(d) = path.parent() {
125        std::fs::create_dir_all(d).map_err(|e| e.to_string())?;
126    }
127    write_ply(path, c, PlyLayout::XyzRgbNormal).map_err(|e| format!("{}: {e}", path.display()))
128}
129
130fn basename(p: &Path) -> String {
131    p.file_name().map(|s| s.to_string_lossy().to_string()).unwrap_or_default()
132}
133
134/// Median of the nearest-neighbor distances < 5 m from the points of the earlier cloud to the later cloud (1/20 subsampled).
135fn overlap_median(prev: &PointCloud, cur: &PointCloud) -> Option<(f64, usize)> {
136    if prev.is_empty() {
137        return None;
138    }
139    let tree = KdTree::from_f32(&prev.positions);
140    let q: Vec<[f64; 3]> = cur.positions.iter().step_by(20).map(|p| [p[0] as f64, p[1] as f64, p[2] as f64]).collect();
141    let mut d: Vec<f64> = tree.nearest_many(&q).into_iter().flatten().map(|(_, d2)| d2.sqrt()).filter(|d| *d < 5.0).collect();
142    if d.is_empty() {
143        return None;
144    }
145    let n = d.len();
146    d.sort_by(f64::total_cmp);
147    let m = if n % 2 == 1 { d[n / 2] } else { 0.5 * (d[n / 2 - 1] + d[n / 2]) };
148    Some((m, n))
149}
150
151/// Per-zone event time (seconds from the first event). None if absent.
152#[derive(Clone, Copy, Debug, Default, PartialEq)]
153pub struct ZoneTimes {
154    /// `region K arrived`.
155    pub arrived: Option<f64>,
156    /// `preview K ready`.
157    pub preview_ready: Option<f64>,
158    /// `refined K pose_ready`.
159    pub refined_pose: Option<f64>,
160    /// `refined K ready`.
161    pub refined_ready: Option<f64>,
162}
163
164/// One zone point cloud: the output file name (same name as in full/) and the point cloud.
165pub struct ZoneCloud<'a> {
166    /// Output file name.
167    pub file: String,
168    /// Point cloud.
169    pub cloud: &'a PointCloud,
170}
171
172/// Post-processing with in-memory input (zone results collected from events). Shared by [`run`] and the default hook.
173pub struct PostZones<'a> {
174    /// Output folder.
175    pub out: &'a Path,
176    /// Preview zones that have a dense point cloud.
177    pub preview: BTreeMap<usize, ZoneCloud<'a>>,
178    /// Refined zones that have a dense point cloud.
179    pub refined: BTreeMap<usize, ZoneCloud<'a>>,
180    /// Preview models (zone 0 is the GPS-aligned one).
181    pub preview_models: BTreeMap<usize, &'a Reconstruction>,
182    /// Refined models (BA + GPS alignment).
183    pub refined_models: BTreeMap<usize, &'a Reconstruction>,
184    /// Per-zone event time.
185    pub times: BTreeMap<usize, ZoneTimes>,
186    /// Position range [lo, hi) used for aligning the preview of zone K (>= 1) = the overlap with the previous zone.
187    pub windows: BTreeMap<usize, (usize, usize)>,
188    /// Image name -> position.
189    pub position: &'a (dyn Fn(&str) -> Option<usize> + Sync),
190}
191
192/// Stable-display outputs: every refined zone exactly as shown (`snapshots/event_NN_TTTT.Ts_refined_K.ply`, display
193/// frame, in order of arrival) and the same clouds in ENU (`final_frame/refined/`). Each zone gets its own Sim3 from its
194/// refined model (`models`) to the GPS-aligned final model (`enu`), fitted on shared 3D points; with too few pairs (or no
195/// model for the zone) one global Sim3 from the display model to `enu` (projection centers of shared images) is used.
196/// `final_frame/merged.ply` is the thinned (1/[`MERGED_STRIDE`]) and overlap-deduplicated union of the ENU clouds.
197pub fn run_stable(
198    out: &Path,
199    refined: &BTreeMap<usize, ZoneCloud<'_>>,
200    models: &BTreeMap<usize, &Reconstruction>,
201    times: &BTreeMap<usize, ZoneTimes>,
202    display: Option<&Reconstruction>,
203    enu: Option<&Reconstruction>,
204    log: &Logger,
205) -> Result<(), String> {
206    let snapdir = out.join("snapshots");
207    std::fs::create_dir_all(&snapdir).map_err(|e| e.to_string())?;
208    let mut order: Vec<(f64, usize)> = refined.keys().map(|k| (times.get(k).and_then(|t| t.refined_ready).unwrap_or(0.0), *k)).collect();
209    order.sort_by(|a, b| a.0.total_cmp(&b.0));
210    for (i, (t, k)) in order.iter().enumerate() {
211        write(&snapdir.join(format!("event_{:02}_{t:06.1}s_refined_{k}.ply", i + 1)), refined[k].cloud)?;
212    }
213    log.line(&format!("[post] stable display: 구역 {} 개를 표시한 그대로 snapshots/ 에 기록", order.len()));
214    let tf = match (display, enu) {
215        (Some(d), Some(e)) => {
216            let (mut src, mut dst) = (Vec::new(), Vec::new());
217            for id in d.registered_images() {
218                let Some(name) = d.image(id).map(|im| im.name.clone()) else { continue };
219                let Some(eid) = e.image_by_name(&name).map(|im| im.image_id) else { continue };
220                if let (Some(a), Some(b)) = (d.projection_center(id), e.projection_center(eid)) {
221                    src.push(a);
222                    dst.push(b);
223                }
224            }
225            cumulus3d_align::umeyama(&src, &dst, true)
226        }
227        _ => None,
228    };
229    if let Some(t) = &tf {
230        log.line(&format!("[post] stable display: ENU 변환 축척 {:.4}", t.scale));
231    }
232    let Some(enu) = enu else {
233        log.line("[post] stable display: ENU 변환 없음(최종 GPS 정렬 실패) — final_frame/ 생략");
234        return Ok(());
235    };
236    let mut placed: BTreeMap<usize, PointCloud> = BTreeMap::new();
237    for (k, zc) in refined {
238        let own = models.get(k).and_then(|m| {
239            let (pa, pb) = corr_points(m, enu, None);
240            if pa.len() >= LiveOptions::default().min_pairs {
241                fit(&pa, &pb).map(|f| (f, pa.len()))
242            } else {
243                None
244            }
245        });
246        let t = match (own, tf) {
247            (Some((f, n)), _) => {
248                log.line(&format!("[post] final_frame 구역 {k}: 공유 점 {n}, 잔차 중앙 {:.3}m, 축척 {:.4}", f.median, f.sim3.scale));
249                f.sim3
250            }
251            (None, Some(t)) => {
252                log.line(&format!("[post] final_frame 구역 {k}: 공유 점 부족 — 전체 변환 사용(축척 {:.4})", t.scale));
253                t
254            }
255            (None, None) => {
256                log.line(&format!("[post] final_frame 구역 {k}: 변환 없음 — 건너뜀"));
257                continue;
258            }
259        };
260        let c = transformed(zc.cloud, &t);
261        write(&out.join("final_frame/refined").join(&zc.file), &c)?;
262        placed.insert(*k, c);
263    }
264    let clouds: BTreeMap<usize, &PointCloud> = placed.iter().map(|(k, c)| (*k, c)).collect();
265    let merged = LiveDisplay::new(LiveOptions { stride: MERGED_STRIDE, relocate: false, ..LiveOptions::default() })
266        .compose(&clouds, &BTreeMap::new());
267    write(&out.join("final_frame/merged.ply"), &merged)?;
268    log.line(&format!("[post] stable display: final_frame/refined/ {} 개, final_frame/merged.ply 점 {}", placed.len(), merged.len()));
269    Ok(())
270}
271
272/// Thinning of `final_frame/merged.ply` in stable display (1/2 of each zone's points, then overlap dedupe).
273pub const MERGED_STRIDE: usize = 2;
274
275/// Settings for [`LiveDisplay`].
276#[derive(Clone, Debug, PartialEq)]
277pub struct LiveOptions {
278    /// Overlap dedupe voxel edge = this x the median nearest-neighbour spacing of the first thinned zone cloud (so it does
279    /// not depend on the scale of the frame). A point of a zone is dropped when an earlier zone already owns its voxel.
280    pub voxel_factor: f64,
281    /// Every zone is thinned to 1/N before merging.
282    pub stride: usize,
283    /// Fewer shared 3D points than this: the zone is not moved.
284    pub min_pairs: usize,
285    /// Place every zone into the newest refined model's frame (fit on shared 3D points). Off in stable display, where all
286    /// zone clouds are already in one frame: then [`LiveDisplay::compose`] only thins and drops the overlap duplicates.
287    pub relocate: bool,
288}
289
290impl Default for LiveOptions {
291    fn default() -> Self {
292        Self { voxel_factor: 2.0, stride: 6, min_pairs: 20, relocate: true }
293    }
294}
295
296/// Smallest voxel edge of the overlap dedupe (guards against a zero spacing from duplicate points).
297const MIN_VOXEL: f64 = 1e-9;
298
299/// Integer voxel of a point: `floor(p / edge)` per axis.
300type VoxelKey = [i64; 3];
301
302fn voxel_key(p: &[f32; 3], inv: f64) -> VoxelKey {
303    [(p[0] as f64 * inv).floor() as i64, (p[1] as f64 * inv).floor() as i64, (p[2] as f64 * inv).floor() as i64]
304}
305
306/// Multiplicative hasher for [`VoxelKey`] (the default SipHash is too slow for millions of keys).
307#[derive(Default)]
308struct VoxelHasher(u64);
309
310impl Hasher for VoxelHasher {
311    fn finish(&self) -> u64 {
312        self.0
313    }
314
315    fn write(&mut self, bytes: &[u8]) {
316        for b in bytes {
317            self.write_u64(*b as u64);
318        }
319    }
320
321    fn write_i64(&mut self, v: i64) {
322        self.write_u64(v as u64);
323    }
324
325    fn write_u64(&mut self, v: u64) {
326        let h = (self.0.rotate_left(26) ^ v).wrapping_mul(0x9E37_79B9_7F4A_7C15);
327        self.0 = h ^ (h >> 29);
328    }
329}
330
331/// Median nearest-neighbour distance of a strided sample (about 4000 points) against the whole cloud. The sample points
332/// are left out of the tree so they do not find themselves.
333fn median_spacing(pts: &[[f32; 3]]) -> f64 {
334    const SAMPLE: usize = 4000;
335    let step = (pts.len() / SAMPLE).max(2);
336    let q: Vec<[f64; 3]> = pts.iter().step_by(step).map(|p| [p[0] as f64, p[1] as f64, p[2] as f64]).collect();
337    let rest: Vec<[f32; 3]> = pts.iter().enumerate().filter(|(i, _)| i % step != 0).map(|(_, p)| *p).collect();
338    let mut d: Vec<f64> = KdTree::from_f32(&rest).nearest_many(&q).into_iter().flatten().map(|(_, d2)| d2.sqrt()).collect();
339    if d.is_empty() {
340        return 0.0;
341    }
342    d.sort_by(f64::total_cmp);
343    d[d.len() / 2]
344}
345
346/// splitmix64 finalizer: the fixed integer hash behind the key-based thinning.
347pub fn splitmix64(mut x: u64) -> u64 {
348    x = x.wrapping_add(0x9E37_79B9_7F4A_7C15);
349    x = (x ^ (x >> 30)).wrapping_mul(0xBF58_476D_1CE4_E5B9);
350    x = (x ^ (x >> 27)).wrapping_mul(0x94D0_49BB_1331_11EB);
351    x ^ (x >> 31)
352}
353
354/// 1/stride thinning. With point keys a point stays iff `splitmix64(key) % stride == 0`, so the same depth-map pixel
355/// survives (or not) in every re-densification of a zone; without keys the index rule of [`decimate`] (index % stride == 0).
356fn thin(cloud: &PointCloud, stride: usize) -> PointCloud {
357    if stride <= 1 {
358        return cloud.clone();
359    }
360    if !cloud.has_keys() || cloud.keys.len() != cloud.len() {
361        return decimate(cloud, stride);
362    }
363    let keep: Vec<bool> = cloud.keys.par_iter().map(|&k| splitmix64(k).is_multiple_of(stride as u64)).collect();
364    select(cloud, &keep)
365}
366
367/// Display cloud built the same way as the final output: every refined zone is placed in the frame of the newest
368/// refined model with its own Sim3 (robust fit on shared 3D points; skipped with `relocate` off), so zones meet without seams; the
369/// overlap of zone k
370/// with the earlier zones (voxel ownership, no empty strip at the seam) is dropped so it is not drawn twice. Fits are cached per (zone,
371/// reference) pair.
372///
373/// Ownership is sticky: the first compose in which a voxel holds points assigns it to the lowest zone that has points there,
374/// and it keeps that owner for the session, so re-densifying or moving a shown zone does not hand its voxels to another zone
375/// (the one exception: when the owner has no points left in the voxel, the lowest zone that has some takes it over, so no
376/// hole opens). Thinning is key-based when the clouds carry point keys (`splitmix64(key) % stride == 0`).
377#[derive(Default)]
378pub struct LiveDisplay {
379    opts: LiveOptions,
380    fits: HashMap<(usize, usize), Sim3>,
381    /// Voxel edge, estimated on the first compose with data and kept for the session so that ownership never flips.
382    voxel: Option<f64>,
383    /// Voxel → owner zone (sticky, see above).
384    owners: HashMap<VoxelKey, usize, BuildHasherDefault<VoxelHasher>>,
385}
386
387impl LiveDisplay {
388    /// With the given settings.
389    pub fn new(opts: LiveOptions) -> Self {
390        Self { opts, fits: HashMap::new(), voxel: None, owners: HashMap::default() }
391    }
392
393    /// Turns relocation (see [`LiveOptions::relocate`]) on or off.
394    pub fn set_relocate(&mut self, on: bool) {
395        self.opts.relocate = on;
396    }
397
398    /// Forgets the voxel size (only when the whole session restarts); the next compose estimates it again.
399    pub fn reset_voxel(&mut self) {
400        self.voxel = None;
401        self.owners.clear();
402    }
403
404    /// Forgets every fit that involves zone `k` (its model changed or was dropped).
405    pub fn forget(&mut self, k: usize) {
406        self.fits.retain(|(a, b), _| *a != k && *b != k);
407    }
408
409    /// Sim3 from zone `j`'s model to zone `cur`'s model (identity when they are the same, a model is missing or too few
410    /// points are shared).
411    fn fit(&mut self, j: usize, cur: usize, models: &BTreeMap<usize, &Reconstruction>) -> Sim3 {
412        if j == cur {
413            return Sim3::identity();
414        }
415        if let Some(t) = self.fits.get(&(j, cur)) {
416            return *t;
417        }
418        let (Some(a), Some(b)) = (models.get(&j), models.get(&cur)) else { return Sim3::identity() };
419        let (pa, pb) = corr_points(a, b, None);
420        let t = if pa.len() >= self.opts.min_pairs { fit(&pa, &pb).map(|f| f.sim3) } else { None };
421        let t = t.unwrap_or_else(Sim3::identity);
422        self.fits.insert((j, cur), t);
423        t
424    }
425
426    /// Merged display cloud of `clouds` (zone → refined cloud) in the frame of the newest zone's model.
427    pub fn compose(&mut self, clouds: &BTreeMap<usize, &PointCloud>, models: &BTreeMap<usize, &Reconstruction>) -> PointCloud {
428        self.compose_zoned(clouds, models).0
429    }
430
431    /// [`compose`](Self::compose) plus the owner zone of every point of the result (same order). Points with equal keys
432    /// (the overlap images of two zones densify the same pixel) are kept once, in the lowest zone.
433    pub fn compose_zoned(
434        &mut self,
435        clouds: &BTreeMap<usize, &PointCloud>,
436        models: &BTreeMap<usize, &Reconstruction>,
437    ) -> (PointCloud, Vec<usize>) {
438        let Some(&cur) = clouds.keys().next_back() else { return (PointCloud::default(), Vec::new()) };
439        // 1) Thin and place every zone; voxel of every point.
440        struct Part {
441            zone: usize,
442            cloud: PointCloud,
443            vox: Vec<VoxelKey>,
444        }
445        let mut parts: Vec<Part> = Vec::with_capacity(clouds.len());
446        for (&j, c) in clouds {
447            let t = if self.opts.relocate { self.fit(j, cur, models) } else { Sim3::identity() };
448            let mut d = thin(c, self.opts.stride);
449            transform_cloud(&mut d, &t);
450            if d.is_empty() {
451                continue;
452            }
453            let factor = self.opts.voxel_factor;
454            let v = *self.voxel.get_or_insert_with(|| (factor * median_spacing(&d.positions)).max(MIN_VOXEL));
455            let inv = 1.0 / v;
456            let vox: Vec<VoxelKey> = d.positions.par_iter().map(|p| voxel_key(p, inv)).collect();
457            parts.push(Part { zone: j, cloud: d, vox });
458        }
459        // 2) Ownership: keep an owner that still has points in the voxel, else the lowest zone present takes it.
460        type VoxSet = HashSet<VoxelKey, BuildHasherDefault<VoxelHasher>>;
461        let sets: Vec<VoxSet> = parts.par_iter().map(|p| p.vox.iter().copied().collect()).collect();
462        let idx: HashMap<usize, usize> = parts.iter().enumerate().map(|(i, p)| (p.zone, i)).collect();
463        let mut lowest: HashMap<VoxelKey, usize, BuildHasherDefault<VoxelHasher>> = HashMap::default();
464        for p in &parts {
465            for &v in &p.vox {
466                lowest.entry(v).or_insert(p.zone);
467            }
468        }
469        for (v, low) in lowest {
470            match self.owners.get(&v) {
471                Some(o) if idx.get(o).is_some_and(|&i| sets[i].contains(&v)) => {}
472                _ => {
473                    self.owners.insert(v, low);
474                }
475            }
476        }
477        // 3) Keep each zone's points in the voxels it owns, then drop repeated keys (lowest zone first).
478        let owners = &self.owners;
479        let mut seen: HashSet<u64, BuildHasherDefault<VoxelHasher>> = HashSet::default();
480        let all_keys = parts.iter().all(|p| p.cloud.has_keys());
481        let mut out_parts: Vec<PointCloud> = Vec::with_capacity(parts.len());
482        let mut zones: Vec<usize> = Vec::new();
483        for p in parts {
484            let mut keep: Vec<bool> = p.vox.par_iter().map(|v| owners.get(v) == Some(&p.zone)).collect();
485            if all_keys {
486                for (k, kp) in p.cloud.keys.iter().zip(keep.iter_mut()) {
487                    if *kp && !seen.insert(*k) {
488                        *kp = false;
489                    }
490                }
491            }
492            let c = select(&p.cloud, &keep);
493            zones.extend(std::iter::repeat_n(p.zone, c.len()));
494            out_parts.push(c);
495        }
496        (merge_clouds(&out_parts.iter().collect::<Vec<_>>()), zones)
497    }
498}
499
500/// Whole post-processing (input: PLY paths and event strings). The summary goes to run.log (+ standard output).
501pub fn run(inp: &PostInput, log: &Logger) -> Result<(), String> {
502    let ev = &inp.events;
503    let mut pv: BTreeMap<usize, PointCloud> = BTreeMap::new();
504    for (k, p) in inp.preview_ply {
505        pv.insert(*k, read_ply(p).map_err(|e| format!("{}: {e}", p.display()))?);
506    }
507    let mut rf: BTreeMap<usize, PointCloud> = BTreeMap::new();
508    for (k, p) in inp.refined_ply {
509        rf.insert(*k, read_ply(p).map_err(|e| format!("{}: {e}", p.display()))?);
510    }
511    let mut times: BTreeMap<usize, ZoneTimes> = BTreeMap::new();
512    for k in pv.keys() {
513        let t = times.entry(*k).or_default();
514        t.preview_ready = tof(ev, &format!("preview {k} ready"));
515        t.arrived = tof(ev, &format!("region {k} arrived"));
516    }
517    for k in rf.keys() {
518        let t = times.entry(*k).or_default();
519        t.refined_ready = tof(ev, &format!("refined {k} ready"));
520        t.refined_pose = tof(ev, &format!("refined {k} pose_ready"));
521    }
522    let windows = pv
523        .keys()
524        .map(|k| {
525            let start = k * inp.span;
526            (*k, (start.saturating_sub(inp.ovl), start + inp.ovl))
527        })
528        .collect();
529    let z = PostZones {
530        out: inp.out,
531        preview: pv.iter().map(|(k, c)| (*k, ZoneCloud { file: basename(&inp.preview_ply[k]), cloud: c })).collect(),
532        refined: rf.iter().map(|(k, c)| (*k, ZoneCloud { file: basename(&inp.refined_ply[k]), cloud: c })).collect(),
533        preview_models: inp.previews.iter().map(|(k, m)| (*k, m)).collect(),
534        refined_models: inp.refined.iter().map(|(k, m)| (*k, m)).collect(),
535        times,
536        windows,
537        position: inp.position,
538    };
539    run_zones(&z, log)
540}
541
542/// Post-processing body (in-memory input): preview alignment -> aligned/ -> per-event snapshots and manifest -> re-anchoring
543/// (final_frame/).
544pub fn run_zones(z: &PostZones, log: &Logger) -> Result<(), String> {
545    let out = z.out;
546    let pv: BTreeMap<usize, &PointCloud> = z.preview.iter().map(|(k, c)| (*k, c.cloud)).collect();
547    let rf: BTreeMap<usize, &PointCloud> = z.refined.iter().map(|(k, c)| (*k, c.cloud)).collect();
548    let tm = |k: &usize| z.times.get(k).copied().unwrap_or_default();
549    let t_p: BTreeMap<usize, Option<f64>> = pv.keys().map(|k| (*k, tm(k).preview_ready)).collect();
550    let t_r: BTreeMap<usize, Option<f64>> = rf.keys().map(|k| (*k, tm(k).refined_ready)).collect();
551    let t_rp: BTreeMap<usize, Option<f64>> = rf.keys().map(|k| (*k, tm(k).refined_pose)).collect();
552    let t_a: BTreeMap<usize, Option<f64>> = pv.keys().map(|k| (*k, tm(k).arrived)).collect();
553
554    // --- Preview alignment: onto the latest refined model whose pose was ready at that time (preview_0, GPS-aligned, if none), using
555    // shared points.
556    let mut align_rows: Vec<Json> = Vec::new();
557    let mut pframe: BTreeMap<usize, String> = BTreeMap::new();
558    let mut pv_aligned: BTreeMap<usize, PointCloud> = BTreeMap::new();
559    for (&k, cloud) in &pv {
560        let (s, med, nk, reference, t): (f64, f64, usize, String, Sim3);
561        if k == 0 {
562            (s, med, nk, reference, t) = (1.0, 0.0, 0, "GPS(preview_0)".into(), Sim3::identity());
563            pframe.insert(k, "preview_0".into());
564        } else {
565            let tpk = t_p[&k].unwrap_or(f64::INFINITY);
566            let cand = rf.keys().copied().filter(|j| t_rp.get(j).copied().flatten().is_some_and(|x| x <= tpk)).max();
567            let (b, refname, frame) = match cand.and_then(|j| z.refined_models.get(&j).copied().map(|m| (m, j))) {
568                Some((m, j)) => (Some(m), format!("refined_{j}"), format!("refined_{j}")),
569                None => (z.preview_models.get(&0).copied(), "preview_0(GPS)".to_string(), "preview_0".to_string()),
570            };
571            let a = z.preview_models.get(&k).copied();
572            let res = match (a, b) {
573                (Some(a), Some(b)) => {
574                    let (lo, hi) = z.windows.get(&k).copied().unwrap_or((0, 0));
575                    let pos = z.position;
576                    // The position function is not 'static, so build the set of range names in advance.
577                    let names: BTreeSet<String> =
578                        a.images().map(|im| im.name.clone()).filter(|n| pos(n).is_some_and(|p| p >= lo && p < hi)).collect();
579                    let filt: NameFilter = Arc::new(move |n: &str| names.contains(n));
580                    let (mut pa, mut pb) = corr_points(a, b, Some(filt));
581                    if pa.len() < 20 {
582                        (pa, pb) = corr_points(a, b, None);
583                    }
584                    fit(&pa, &pb)
585                }
586                _ => None,
587            };
588            match res {
589                Some(f) => {
590                    (s, med, nk, reference, t) = (f.sim3.scale, f.median, f.inliers, refname, f.sim3);
591                    pframe.insert(k, frame);
592                }
593                None => {
594                    // Design decision: if there is no correspondence, do not fail; leave it unaligned and mark it.
595                    log.line(&format!("[post] 초벌 {k} 정렬 실패(공유 점 부족) — 원래 좌표 유지"));
596                    (s, med, nk, reference, t) = (1.0, 0.0, 0, format!("{refname}(실패)"), Sim3::identity());
597                    pframe.insert(k, frame);
598                }
599            }
600        }
601        align_rows.push(Json::obj(vec![
602            ("region", Json::Int(k as i64)),
603            ("reference", Json::Str(reference)),
604            ("pairs", Json::Int(nk as i64)),
605            ("fit_median_m", Json::f(med, 2)),
606            ("scale", Json::f(s, 3)),
607        ]));
608        let c = transformed(cloud, &t);
609        write(&out.join("aligned/preview").join(z.preview[&k].file.clone()), &c)?;
610        pv_aligned.insert(k, c);
611    }
612    for (k, c) in &rf {
613        write(&out.join("aligned/refined").join(z.refined[k].file.clone()), c)?;
614    }
615
616    // --- Snapshots per event order.
617    let mut evs: Vec<(f64, &str, usize)> = Vec::new();
618    for k in pv.keys() {
619        evs.push((t_p[k].unwrap_or(f64::INFINITY), "preview", *k));
620    }
621    for k in rf.keys() {
622        evs.push((t_r[k].unwrap_or(f64::INFINITY), "refined", *k));
623    }
624    evs.sort_by(|a, b| a.0.total_cmp(&b.0).then(a.1.cmp(b.1)).then(a.2.cmp(&b.2)));
625    let snap_opts = SnapshotOptions::default();
626    let snapdir = out.join("snapshots");
627    std::fs::create_dir_all(&snapdir).map_err(|e| e.to_string())?;
628    let mut shown_p: BTreeSet<usize> = BTreeSet::new();
629    let mut shown_r: BTreeSet<usize> = BTreeSet::new();
630    struct Ev {
631        i: usize,
632        t: f64,
633        kind: &'static str,
634        k: usize,
635        n: usize,
636        refined: Vec<usize>,
637        preview_only: Vec<usize>,
638    }
639    let mut man: Vec<Ev> = Vec::new();
640    let snap_name = |i: usize, t: f64, kind: &str, k: usize| snapdir.join(format!("event_{i:02}_{t:06.1}s_{kind}_{k}.ply"));
641    for (i, &(t, kind, k)) in evs.iter().enumerate() {
642        let i = i + 1;
643        if kind == "preview" {
644            shown_p.insert(k);
645        } else {
646            shown_r.insert(k);
647        }
648        let fine: Vec<&PointCloud> = shown_r.iter().map(|j| rf[j]).collect();
649        let po: Vec<usize> = shown_p.difference(&shown_r).copied().collect();
650        let coarse: Vec<&PointCloud> = po.iter().map(|j| &pv_aligned[j]).collect();
651        let a = compose_snapshot(&fine, &coarse, &snap_opts);
652        write(&snap_name(i, t, kind, k), &a)?;
653        let kind: &'static str = if kind == "preview" { "preview" } else { "refined" };
654        man.push(Ev { i, t, kind, k, n: a.len(), refined: shown_r.iter().copied().collect(), preview_only: po });
655    }
656    let r1 = |x: Option<f64>| Json::of(x, 1);
657    let rows: Vec<Json> = pv
658        .keys()
659        .map(|k| {
660            let ta = t_a[k].unwrap_or(0.0);
661            let tp = t_p[k].unwrap_or(0.0);
662            let trp = t_rp.get(k).copied().flatten();
663            let tr = t_r.get(k).copied().flatten();
664            Json::obj(vec![
665                ("region", Json::Int(*k as i64)),
666                ("arrived_s", Json::f(ta, 1)),
667                ("preview_ready_s", Json::f(tp, 1)),
668                ("preview_latency_s", Json::f(tp - ta, 1)),
669                ("refined_pose_s", r1(trp)),
670                ("refined_ready_s", r1(tr)),
671                ("refined_latency_s", r1(tr.map(|x| x - ta))),
672            ])
673        })
674        .collect();
675    let ev_json = |e: &Ev, frame: Option<&str>, n: usize| {
676        let mut v = vec![
677            ("event", Json::Int(e.i as i64)),
678            ("time_s", Json::f(e.t, 1)),
679            ("kind", Json::Str(e.kind.into())),
680            ("region", Json::Int(e.k as i64)),
681            ("points_1of6", Json::Int(n as i64)),
682            ("refined", Json::ints(&e.refined)),
683            ("preview_only", Json::ints(&e.preview_only)),
684        ];
685        if let Some(f) = frame {
686            v.push(("frame", Json::Str(f.into())));
687        }
688        Json::obj(v)
689    };
690    let manifest = |events: Vec<Json>, reanchor: Option<Json>| {
691        let mut v = vec![("timeline", Json::Arr(rows.clone())), ("events", Json::Arr(events)), ("align", Json::Arr(align_rows.clone()))];
692        if let Some(r) = reanchor {
693            v.push(("reanchor", r));
694        }
695        Json::obj(v)
696    };
697    let mpath = snapdir.join("manifest.json");
698    std::fs::write(&mpath, manifest(man.iter().map(|e| ev_json(e, None, e.n)).collect(), None).dump()).map_err(|e| e.to_string())?;
699    log.line("== 구역별 시간(초, 시작 기준)");
700    for r in &rows {
701        log.line(&r.py_repr());
702    }
703    log.line("== 정렬");
704    for a in &align_rows {
705        log.line(&a.py_repr());
706    }
707    log.line("== 사건 순서");
708    for m in &man {
709        log.line(&format!("{} {} {} {} {}", m.i, py_float(m.t, 1), m.kind, m.k, m.n));
710    }
711    let ks: Vec<usize> = rf.keys().copied().collect();
712    for w in ks.windows(2) {
713        match overlap_median(rf[&w[0]], rf[&w[1]]) {
714            Some((m, _)) => log.line(&format!("정밀 {}-{} 겹침 차 중앙 {m:.2}m", w[0], w[1])),
715            None => log.line(&format!("정밀 {}-{} 겹침 없음", w[0], w[1])),
716        }
717    }
718
719    // --- Re-anchoring (incremental_reanchor.py): every time a new refined model appears, bring the already sent zones into its
720    // coordinate frame.
721    if rf.is_empty() {
722        return Ok(());
723    }
724    let mut models: BTreeMap<String, &Reconstruction> = BTreeMap::new();
725    if let Some(m) = z.preview_models.get(&0).copied() {
726        models.insert("preview_0".into(), m);
727    }
728    for (k, m) in &z.refined_models {
729        models.insert(format!("refined_{k}"), m);
730    }
731    let mut cache: HashMap<(String, String), Sim3> = HashMap::new();
732    let mut order: Vec<(String, String)> = Vec::new();
733    let mut tf = |a: &str, b: &str| -> Sim3 {
734        if a == b {
735            return Sim3::identity();
736        }
737        let key = (a.to_string(), b.to_string());
738        if let Some(t) = cache.get(&key) {
739            return *t;
740        }
741        let t = match (models.get(a), models.get(b)) {
742            (Some(ma), Some(mb)) => {
743                let (pa, pb) = corr_points(ma, mb, None);
744                match fit(&pa, &pb) {
745                    Some(f) => {
746                        log.line(&format!(
747                            "{a} -> {b}: 점쌍 {}, 잔차 중앙 {:.3}m, 축척 {:.4}, 이동 {:.2}m",
748                            f.inliers,
749                            f.median,
750                            f.sim3.scale,
751                            f.sim3.translation.norm()
752                        ));
753                        f.sim3
754                    }
755                    None => {
756                        log.line(&format!("{a} -> {b}: 점쌍 부족({}), 항등 사용", pa.len()));
757                        Sim3::identity()
758                    }
759                }
760            }
761            _ => Sim3::identity(),
762        };
763        cache.insert(key.clone(), t);
764        order.push(key);
765        t
766    };
767    for f in std::fs::read_dir(&snapdir).map_err(|e| e.to_string())?.flatten() {
768        let n = f.file_name().to_string_lossy().to_string();
769        if n.starts_with("event_") && n.ends_with(".ply") {
770            let _ = std::fs::remove_file(f.path());
771        }
772    }
773    let mut cur = "preview_0".to_string();
774    let mut ev2: Vec<Json> = Vec::new();
775    for e in &man {
776        if e.kind == "refined" {
777            cur = format!("refined_{}", e.k);
778        }
779        let rs: Vec<PointCloud> = e.refined.iter().map(|j| transformed(rf[j], &tf(&format!("refined_{j}"), &cur))).collect();
780        let ps: Vec<PointCloud> = e
781            .preview_only
782            .iter()
783            .map(|j| transformed(&pv_aligned[j], &tf(pframe.get(j).map(|s| s.as_str()).unwrap_or("preview_0"), &cur)))
784            .collect();
785        let a = compose_snapshot(&rs.iter().collect::<Vec<_>>(), &ps.iter().collect::<Vec<_>>(), &snap_opts);
786        write(&snap_name(e.i, e.t, e.kind, e.k), &a)?;
787        log.line(&format!("{} {} {} {} 좌표계 {cur} 점 {}", e.i, py_float(e.t, 1), e.kind, e.k, a.len()));
788        ev2.push(ev_json(e, Some(&cur), a.len()));
789    }
790    let last = format!("refined_{}", rf.keys().max().copied().unwrap_or(0));
791    let mut final_rf: BTreeMap<usize, PointCloud> = BTreeMap::new();
792    for (k, c) in &rf {
793        let c2 = transformed(c, &tf(&format!("refined_{k}"), &last));
794        write(&out.join("final_frame/refined").join(z.refined[k].file.clone()), &c2)?;
795        final_rf.insert(*k, c2);
796    }
797    for (k, c) in &pv_aligned {
798        let fr = pframe.get(k).cloned().unwrap_or_else(|| "preview_0".into());
799        write(&out.join("final_frame/preview").join(z.preview[k].file.clone()), &transformed(c, &tf(&fr, &last)))?;
800    }
801    for w in ks.windows(2) {
802        match overlap_median(&final_rf[&w[0]], &final_rf[&w[1]]) {
803            Some((m, n)) => log.line(&format!("최종 좌표계 정밀 {}-{} 겹침 차 중앙 {m:.2}m (겹침 점 {n})", w[0], w[1])),
804            None => log.line(&format!("최종 좌표계 정밀 {}-{} 겹침 없음", w[0], w[1])),
805        }
806    }
807    let re = Json::Obj(
808        order
809            .iter()
810            .map(|(a, b)| {
811                let t = cache[&(a.clone(), b.clone())];
812                (format!("{a}->{b}"), Json::obj(vec![("scale", Json::f(t.scale, 4)), ("shift_m", Json::f(t.translation.norm(), 2))]))
813            })
814            .collect(),
815    );
816    std::fs::write(&mpath, manifest(ev2, Some(re)).dump()).map_err(|e| e.to_string())?;
817    Ok(())
818}
819
820#[cfg(test)]
821mod tests {
822    use super::*;
823
824    fn cloud(xs: &[f32]) -> PointCloud {
825        PointCloud { positions: xs.iter().map(|x| [*x, 0.0, 0.0]).collect(), ..Default::default() }
826    }
827
828    #[test]
829    fn live_compose_thins_and_drops_overlap_without_models() {
830        // Stride 6 keeps indices 0 and 6 of each zone. Zone 0's kept points are 6 apart -> voxel edge 12 (factor 2).
831        let mut xs0 = vec![0.0f32; 12];
832        xs0[6] = 6.0;
833        let mut xs1 = vec![100.0f32; 12];
834        xs1[0] = 1.0; // same voxel as zone 0's point at x = 0 -> dropped
835        xs1[6] = 20.0; // free voxel -> kept
836        xs1[1] = 50.0; // removed by thinning
837        let (c0, c1) = (cloud(&xs0), cloud(&xs1));
838        let clouds: BTreeMap<usize, &PointCloud> = [(0, &c0), (1, &c1)].into_iter().collect();
839        let models: BTreeMap<usize, &Reconstruction> = BTreeMap::new();
840        let mut live = LiveDisplay::default();
841        let out = live.compose(&clouds, &models);
842        let mut xs: Vec<f32> = out.positions.iter().map(|p| p[0]).collect();
843        xs.sort_by(f32::total_cmp);
844        assert_eq!(xs, vec![0.0, 6.0, 20.0]);
845        assert!(live.compose(&BTreeMap::new(), &models).is_empty());
846    }
847
848    #[test]
849    fn live_compose_keeps_seam_without_empty_strip() {
850        // Two grids of spacing 1 meeting at x = 20; zone 1 starts 4 rows early and is offset by 0.3.
851        let grid = |x0: f32, x1: f32, off: f32| PointCloud {
852            positions: (x0 as i32..x1 as i32).flat_map(|i| (0..30).map(move |j| [i as f32 + off, j as f32 + off, 0.0])).collect(),
853            ..Default::default()
854        };
855        let (c0, c1) = (grid(0.0, 20.0, 0.0), grid(16.0, 40.0, 0.3));
856        let clouds: BTreeMap<usize, &PointCloud> = [(0, &c0), (1, &c1)].into_iter().collect();
857        let models: BTreeMap<usize, &Reconstruction> = BTreeMap::new();
858        let mut live = LiveDisplay::new(LiveOptions { stride: 1, relocate: false, ..LiveOptions::default() });
859        let out = live.compose(&clouds, &models);
860        assert!(out.len() < c0.len() + c1.len(), "overlap duplicates are removed");
861        assert!(out.len() > 40 * 30 / 2);
862        // Every cell of width 2 along x (and every row band) still holds points: no empty strip at the seam.
863        for cx in 0..20 {
864            for cy in 0..15 {
865                let n = out.positions.iter().filter(|p| (p[0] / 2.0).floor() as i32 == cx && (p[1] / 2.0).floor() as i32 == cy).count();
866                assert!(n > 0, "empty cell ({cx}, {cy})");
867            }
868        }
869    }
870
871    #[test]
872    fn live_compose_voxel_size_and_ownership_are_stable_across_calls() {
873        let grid = |x0: i32, x1: i32, off: f32| PointCloud {
874            positions: (x0..x1).flat_map(|i| (0..10).map(move |j| [i as f32 + off, j as f32 + off, 0.0])).collect(),
875            ..Default::default()
876        };
877        let (c0, c1, c2) = (grid(0, 12, 0.0), grid(8, 20, 0.3), grid(16, 28, 0.6));
878        let models: BTreeMap<usize, &Reconstruction> = BTreeMap::new();
879        let mut live = LiveDisplay::new(LiveOptions { stride: 1, relocate: false, ..LiveOptions::default() });
880        let a = live.compose(&[(0, &c0), (1, &c1)].into_iter().collect(), &models);
881        let voxel = live.voxel;
882        assert!(voxel.is_some());
883        // A very different first zone would give another estimate if it were redone.
884        let b = live.compose(&[(0, &c0), (1, &c1), (2, &c2)].into_iter().collect(), &models);
885        assert_eq!(live.voxel, voxel);
886        let kept: BTreeSet<[u32; 3]> = b.positions.iter().map(|p| p.map(f32::to_bits)).collect();
887        assert!(a.positions.iter().all(|p| kept.contains(&p.map(f32::to_bits))));
888        assert!(b.len() > a.len());
889    }
890
891    /// Cloud on a lattice (spacing 1) with keys = lattice index + `key0` (so equal lattice cells of two zones get equal keys).
892    fn keyed_grid(x0: i32, x1: i32, off: f32, key0: u64) -> PointCloud {
893        let mut c = PointCloud::default();
894        for i in x0..x1 {
895            for j in 0..10 {
896                c.positions.push([i as f32 + off, j as f32 + off, 0.0]);
897                c.keys.push(key0 + (i as u64) * 100 + j as u64);
898            }
899        }
900        c
901    }
902
903    #[test]
904    fn key_thinning_keeps_the_same_pixels_after_reordering() {
905        let c = keyed_grid(0, 40, 0.0, 7);
906        let mut r = c.clone();
907        let n = r.len();
908        // A re-densification returns other points in another order and with other positions, but the same keys.
909        r.positions.reverse();
910        r.keys.reverse();
911        for p in r.positions.iter_mut() {
912            p[2] += 0.5;
913        }
914        let (a, b) = (thin(&c, 6), thin(&r, 6));
915        let ka: BTreeSet<u64> = a.keys.iter().copied().collect();
916        let kb: BTreeSet<u64> = b.keys.iter().copied().collect();
917        assert_eq!(ka, kb);
918        assert!(a.len() > n / 12 && a.len() < n / 3, "{} of {n}", a.len());
919        assert!(a.keys.iter().all(|&k| splitmix64(k).is_multiple_of(6)));
920        // Without keys the index rule applies.
921        let mut nk = c.clone();
922        nk.keys.clear();
923        assert_eq!(thin(&nk, 6).len(), n.div_ceil(6));
924    }
925
926    #[test]
927    fn live_compose_ownership_is_sticky_when_a_zone_is_replaced() {
928        let models: BTreeMap<usize, &Reconstruction> = BTreeMap::new();
929        let mut live = LiveDisplay::new(LiveOptions { stride: 1, relocate: false, ..LiveOptions::default() });
930        // Zone 0 covers x 0..12, zone 1 x 8..20; the overlap 8..12 belongs to zone 0.
931        let (c0, c1) = (keyed_grid(0, 12, 0.0, 0), keyed_grid(8, 20, 0.2, 5000));
932        let (a, za) = live.compose_zoned(&[(0, &c0), (1, &c1)].into_iter().collect(), &models);
933        assert_eq!(a.len(), za.len());
934        let owner_at = |c: &PointCloud, z: &[usize], x: f32| -> Vec<usize> {
935            c.positions.iter().zip(z).filter(|(p, _)| (p[0] - x).abs() < 0.5).map(|(_, z)| *z).collect()
936        };
937        assert!(owner_at(&a, &za, 10.0).iter().all(|&z| z == 0));
938        // Zone 0 is re-densified in a different place (now only x 0..9): zone 1 must not take over the voxels zone 0 still
939        // has points in, while the voxels zone 0 lost go to the lowest zone present (no hole).
940        let c0b = keyed_grid(0, 9, 0.1, 0);
941        let (b, zb) = live.compose_zoned(&[(0, &c0b), (1, &c1)].into_iter().collect(), &models);
942        assert!(owner_at(&b, &zb, 8.5).iter().all(|&z| z == 0), "{:?}", owner_at(&b, &zb, 8.5));
943        assert!(owner_at(&b, &zb, 10.0).iter().all(|&z| z == 1));
944        // Once zone 0 grows back, the voxels that went to zone 1 in between stay with zone 1 (no flicker back), and the
945        // ones zone 0 kept stay with zone 0.
946        let (c, zc) = live.compose_zoned(&[(0, &c0), (1, &c1)].into_iter().collect(), &models);
947        assert!(owner_at(&c, &zc, 11.0).iter().all(|&z| z == 1));
948        assert!(owner_at(&c, &zc, 8.5).iter().all(|&z| z == 0));
949        assert_eq!(c.len(), zc.len());
950        live.reset_voxel();
951        assert!(live.owners.is_empty());
952    }
953
954    #[test]
955    fn live_compose_drops_repeated_keys_keeping_the_lowest_zone() {
956        let models: BTreeMap<usize, &Reconstruction> = BTreeMap::new();
957        let mut live = LiveDisplay::new(LiveOptions { stride: 1, relocate: false, voxel_factor: 0.01, ..LiveOptions::default() });
958        // Same keys in both zones at far apart positions (never the same voxel): only zone 0's survive.
959        let (c0, c1) = (keyed_grid(0, 5, 0.0, 0), keyed_grid(100, 105, 0.0, 0));
960        let c1 = PointCloud { keys: c0.keys.clone(), ..c1 };
961        let (out, z) = live.compose_zoned(&[(0, &c0), (1, &c1)].into_iter().collect(), &models);
962        assert_eq!(out.len(), c0.len());
963        assert!(z.iter().all(|&z| z == 0));
964        let uniq: BTreeSet<u64> = out.keys.iter().copied().collect();
965        assert_eq!(uniq.len(), out.len());
966    }
967
968    fn tiny_model(shift: f64, scale: f64) -> Reconstruction {
969        use cumulus3d_core::{Camera, CameraModelKind, Image, Point3D, Rigid3, TrackEntry, Vec2};
970        let mut rec = Reconstruction::new();
971        let mut cam = Camera::from_focal(CameraModelKind::Pinhole, 500.0, 640, 480);
972        cam.camera_id = 1;
973        rec.add_camera_own_rig(cam).unwrap();
974        for i in 1..=2u32 {
975            let pts: Vec<Vec2> = (0..40).map(|k| Vec2::new(k as f64, 0.0)).collect();
976            rec.add_image_own_frame(Image::new(i, format!("camF/camF_{:04}.jpg", i - 1), 1, pts), Some(Rigid3::identity())).unwrap();
977            rec.register_image(i).unwrap();
978        }
979        for k in 0..40u32 {
980            let x = Vec3::new(k as f64, (k * k % 7) as f64, (k % 5) as f64 * 2.0);
981            let track = vec![TrackEntry::new(1, k), TrackEntry::new(2, k)];
982            rec.add_point3d_with_id(
983                k as u64 + 1,
984                Point3D { xyz: x * scale + Vec3::new(shift, 0.0, -shift), color: [0; 3], error: -1.0, track },
985            )
986            .unwrap();
987        }
988        rec
989    }
990
991    #[test]
992    fn shared_point_fit_recovers_known_sim3() {
993        let (a, b) = (tiny_model(0.0, 1.0), tiny_model(3.0, 1.5));
994        let (f, n) = fit_shared_points(&a, &b, None, 20).unwrap();
995        assert_eq!(n, 40);
996        assert!((f.sim3.scale - 1.5).abs() < 1e-9);
997        assert!((f.sim3.transform_point(&Vec3::new(1.0, 2.0, 3.0)) - Vec3::new(4.5, 3.0, 4.5 - 3.0)).norm() < 1e-9);
998        assert!(f.median < 1e-9);
999        // Too few pairs for the requested minimum.
1000        assert!(fit_shared_points(&a, &b, None, 41).is_none());
1001        // A name filter that accepts nothing leaves no pairs.
1002        let none: NameFilter = Arc::new(|_: &str| false);
1003        assert!(fit_shared_points(&a, &b, Some(none), 1).is_none());
1004    }
1005}