1use 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
48pub struct PostInput<'a> {
50 pub out: &'a Path,
52 pub span: usize,
54 pub ovl: usize,
56 pub events: Vec<(f64, String)>,
58 pub previews: &'a BTreeMap<usize, Reconstruction>,
60 pub refined: &'a BTreeMap<usize, Reconstruction>,
62 pub preview_ply: &'a BTreeMap<usize, PathBuf>,
64 pub refined_ply: &'a BTreeMap<usize, PathBuf>,
66 pub position: &'a (dyn Fn(&str) -> Option<usize> + Sync),
68}
69
70fn 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
75pub(crate) struct Fit {
77 pub(crate) sim3: Sim3,
78 pub(crate) median: f64,
79 pub(crate) inliers: usize,
80}
81
82pub(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
102pub(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
134fn 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#[derive(Clone, Copy, Debug, Default, PartialEq)]
153pub struct ZoneTimes {
154 pub arrived: Option<f64>,
156 pub preview_ready: Option<f64>,
158 pub refined_pose: Option<f64>,
160 pub refined_ready: Option<f64>,
162}
163
164pub struct ZoneCloud<'a> {
166 pub file: String,
168 pub cloud: &'a PointCloud,
170}
171
172pub struct PostZones<'a> {
174 pub out: &'a Path,
176 pub preview: BTreeMap<usize, ZoneCloud<'a>>,
178 pub refined: BTreeMap<usize, ZoneCloud<'a>>,
180 pub preview_models: BTreeMap<usize, &'a Reconstruction>,
182 pub refined_models: BTreeMap<usize, &'a Reconstruction>,
184 pub times: BTreeMap<usize, ZoneTimes>,
186 pub windows: BTreeMap<usize, (usize, usize)>,
188 pub position: &'a (dyn Fn(&str) -> Option<usize> + Sync),
190}
191
192pub 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
272pub const MERGED_STRIDE: usize = 2;
274
275#[derive(Clone, Debug, PartialEq)]
277pub struct LiveOptions {
278 pub voxel_factor: f64,
281 pub stride: usize,
283 pub min_pairs: usize,
285 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
296const MIN_VOXEL: f64 = 1e-9;
298
299type 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#[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
331fn 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
346pub 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
354fn 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#[derive(Default)]
378pub struct LiveDisplay {
379 opts: LiveOptions,
380 fits: HashMap<(usize, usize), Sim3>,
381 voxel: Option<f64>,
383 owners: HashMap<VoxelKey, usize, BuildHasherDefault<VoxelHasher>>,
385}
386
387impl LiveDisplay {
388 pub fn new(opts: LiveOptions) -> Self {
390 Self { opts, fits: HashMap::new(), voxel: None, owners: HashMap::default() }
391 }
392
393 pub fn set_relocate(&mut self, on: bool) {
395 self.opts.relocate = on;
396 }
397
398 pub fn reset_voxel(&mut self) {
400 self.voxel = None;
401 self.owners.clear();
402 }
403
404 pub fn forget(&mut self, k: usize) {
406 self.fits.retain(|(a, b), _| *a != k && *b != k);
407 }
408
409 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 pub fn compose(&mut self, clouds: &BTreeMap<usize, &PointCloud>, models: &BTreeMap<usize, &Reconstruction>) -> PointCloud {
428 self.compose_zoned(clouds, models).0
429 }
430
431 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 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 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 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
500pub 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
542pub 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 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 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 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 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 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 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; xs1[6] = 20.0; xs1[1] = 50.0; 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 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 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 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 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 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 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 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 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 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 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 assert!(fit_shared_points(&a, &b, None, 41).is_none());
1001 let none: NameFilter = Arc::new(|_: &str| false);
1003 assert!(fit_shared_points(&a, &b, Some(none), 1).is_none());
1004 }
1005}