1use crate::densewrap::{dense_model, DenseConfig};
61use crate::events::{Command, Event, Frame, Meta, ZoneRange};
62use crate::intrinsics::IntrinsicsPrior;
63use crate::post::fit_shared_points;
64use crate::util::StageTimes;
65use cumulus3d_align::shared::NameFilter;
66use cumulus3d_align::{align_to_gps, EnuOrigin, ModelAlignerOptions};
67use cumulus3d_ba::{bundle_adjust_with, BaBackend, BaBackendRef, BaConfig};
68use cumulus3d_core::analyzer::ModelStats;
69use cumulus3d_core::io::ply::PointCloud;
70use cumulus3d_core::io::GpsRecord;
71use cumulus3d_core::{Camera, CameraId, FeatureStore, ImageId, MatchGraph, MatchGraphOptions, Reconstruction, Rigid3};
72use cumulus3d_features::{CameraMode, ExtractionOptions, FeatureExtractor, ImageReport, ImageStatus, SiftEngine};
73use cumulus3d_matching::{match_pairs_with, MatcherBackend, PairMatchingOptions, TwoViewBackend};
74use cumulus3d_sfm::{
75 global_mapper, register_images_with, triangulate_points_with, AbsPoseBackend, GlobalSfmOptions, PointTriangulatorOptions,
76 RegistrationOptions, TriangulationBackend, TriangulationScope,
77};
78use rayon::prelude::*;
79use std::collections::{BTreeMap, BTreeSet, HashMap, HashSet, VecDeque};
80use std::path::PathBuf;
81use std::sync::{Arc, Mutex, MutexGuard};
82use std::thread::JoinHandle;
83use std::time::{Instant, SystemTime};
84
85#[derive(Clone, Debug, PartialEq, Eq)]
87pub struct FrameImage {
88 pub camera: String,
90 pub name: String,
92}
93
94#[derive(Clone, Debug, Default)]
96pub struct FrameSet {
97 pub images: Vec<FrameImage>,
99 pub gps: Vec<GpsRecord>,
101}
102
103impl FrameSet {
104 pub fn new<C: Into<String>, N: Into<String>>(images: impl IntoIterator<Item = (C, N)>) -> Self {
106 Self { images: images.into_iter().map(|(c, n)| FrameImage { camera: c.into(), name: n.into() }).collect(), gps: Vec::new() }
107 }
108}
109
110#[derive(Clone)]
112pub struct SparseBackends {
113 pub two_view: Arc<dyn TwoViewBackend>,
115 pub abs_pose: Arc<dyn AbsPoseBackend>,
117 pub triangulation: Arc<dyn TriangulationBackend>,
119}
120
121impl Default for SparseBackends {
122 fn default() -> Self {
123 Self {
124 two_view: Arc::new(cumulus3d_matching::CpuTwoView),
125 abs_pose: Arc::new(cumulus3d_sfm::CpuAbsPose),
126 triangulation: Arc::new(cumulus3d_sfm::CpuTriangulation),
127 }
128 }
129}
130
131#[derive(Clone)]
133pub struct SessionConfig {
134 pub image_root: PathBuf,
136 pub gps: Vec<GpsRecord>,
138 pub span: usize,
140 pub overlap: usize,
142 pub total_positions: Option<usize>,
144 pub incremental_triangulation: bool,
146 pub fixed_enu_origin: bool,
148 pub seed: Option<u64>,
150 pub extraction: ExtractionOptions,
152 pub sift: Arc<dyn SiftEngine>,
154 pub matcher: Arc<dyn MatcherBackend>,
156 pub ba_backend: Arc<dyn BaBackend>,
158 pub sparse: SparseBackends,
160 pub dense: Option<DenseConfig>,
162 pub history: usize,
164 pub decode_frames: bool,
166 pub stable_display: bool,
171 pub gps_prior_sigma: f64,
174 pub settle_tol: f64,
178 pub settle_max_zone: usize,
180 pub intrinsics_prior: Option<Arc<IntrinsicsPrior>>,
183}
184
185impl SessionConfig {
186 pub fn new(image_root: impl Into<PathBuf>) -> Self {
188 let mut extraction = ExtractionOptions::default();
189 extraction.sift.max_num_features = 8192;
190 extraction.reader.max_image_size = 3200;
191 Self {
192 image_root: image_root.into(),
193 gps: Vec::new(),
194 span: 12,
195 overlap: 2,
196 total_positions: None,
197 incremental_triangulation: false,
198 fixed_enu_origin: false,
199 seed: None,
200 extraction,
201 sift: Arc::new(cumulus3d_features::CpuSift::default()),
202 matcher: Arc::new(cumulus3d_matching::CpuMatcher::default()),
203 ba_backend: Arc::new(cumulus3d_ba::CpuBaBackend),
204 sparse: SparseBackends::default(),
205 dense: None,
206 history: 8,
207 decode_frames: false,
208 stable_display: false,
209 gps_prior_sigma: 1.0,
210 settle_tol: 0.01,
211 settle_max_zone: 3,
212 intrinsics_prior: None,
213 }
214 }
215}
216
217#[derive(Clone, Debug)]
219pub enum Input {
220 Frames(FrameSet),
222 Command(Command),
224 Poll,
226 Log(String),
228 Finish,
230}
231
232impl From<FrameSet> for Input {
233 fn from(f: FrameSet) -> Self {
234 Input::Frames(f)
235 }
236}
237
238impl From<Command> for Input {
239 fn from(c: Command) -> Self {
240 Input::Command(c)
241 }
242}
243
244impl crate::pipeline::Reducer for Session {
247 type Input = Input;
248 fn step(&mut self, input: Input) -> Vec<Event> {
249 self.apply(input)
250 }
251 fn prelude(&mut self, input: &Input) -> Vec<Event> {
252 let Input::Frames(fs) = input else { return Vec::new() };
253 let sh = self.shared.clone();
254 let mut evs = Vec::new();
255 {
256 let mut out = Out { sh: &sh, fg: Some(&mut evs), guard: None };
257 self.announce(fs, &mut out);
258 }
259 evs
260 }
261 fn poll(&mut self) -> Vec<Event> {
262 self.apply(Input::Poll)
263 }
264 fn command(&mut self, c: Command) -> Vec<Event> {
265 self.apply(Input::Command(c))
266 }
267 fn finish(&mut self) -> Vec<Event> {
268 self.apply(Input::Finish)
269 }
270}
271
272pub type EventSink = Arc<dyn Fn(Event) + Send + Sync>;
274
275#[derive(Clone, Debug)]
277pub struct PreviewZone {
278 pub range: ZoneRange,
280 pub model: Arc<Reconstruction>,
282 pub cloud: Option<Arc<PointCloud>>,
284 pub frame: String,
286}
287
288#[derive(Clone, Debug)]
290pub struct RefinedZone {
291 pub range: ZoneRange,
293 pub model: Arc<Reconstruction>,
295 pub cloud: Option<Arc<PointCloud>>,
297 pub done: bool,
299 generation: u64,
300}
301
302struct EmitState {
305 version: u64,
306 sink: Option<EventSink>,
307 inbox: Vec<Event>,
308}
309
310struct Emitter(Mutex<EmitState>);
311
312impl Emitter {
313 fn lock(&self) -> MutexGuard<'_, EmitState> {
314 self.0.lock().unwrap_or_else(|e| e.into_inner())
315 }
316
317 fn emit(&self, make: impl FnOnce(Meta) -> Event, fg: Option<&mut Vec<Event>>) {
319 let mut st = self.lock();
320 st.version += 1;
321 let ev = make(Meta { version: st.version, at: SystemTime::now() });
322 match (&st.sink, fg) {
323 (Some(s), fg) => {
324 let s = s.clone();
325 if let Some(v) = fg {
326 v.push(ev.clone());
327 }
328 s(ev);
329 }
330 (None, Some(v)) => v.push(ev),
331 (None, None) => st.inbox.push(ev),
332 }
333 }
334}
335
336struct Shared {
338 cfg: SessionConfig,
339 extractor: FeatureExtractor,
340 emitter: Emitter,
341 times: StageTimes,
342 bg: Mutex<Background>,
343 start: Mutex<Option<Instant>>,
344}
345
346#[derive(Default)]
347struct Background {
348 jobs: Vec<JoinHandle<()>>,
349 ready: BTreeMap<usize, RefinedZone>,
350 generation: BTreeMap<usize, u64>,
351 settled: Option<usize>,
353 zone_keep: BTreeMap<usize, Arc<HashSet<String>>>,
355 dense_basis: BTreeMap<usize, Arc<Reconstruction>>,
357 finished: BTreeMap<usize, u64>,
359}
360
361struct FinishMark<'a> {
363 sh: &'a Shared,
364 zone: usize,
365 generation: u64,
366}
367
368impl Drop for FinishMark<'_> {
369 fn drop(&mut self) {
370 self.sh.bg().finished.insert(self.zone, self.generation);
371 }
372}
373
374impl Shared {
375 fn bg(&self) -> MutexGuard<'_, Background> {
376 self.bg.lock().unwrap_or_else(|e| e.into_inner())
377 }
378
379 fn generation(&self, zone: usize) -> u64 {
380 self.bg().generation.get(&zone).copied().unwrap_or(0)
381 }
382
383 fn invalidate(&self, zone: usize) {
385 let mut bg = self.bg();
386 *bg.generation.entry(zone).or_insert(0) += 1;
387 bg.ready.remove(&zone);
388 bg.zone_keep.remove(&zone);
389 bg.dense_basis.remove(&zone);
390 }
391
392 fn aligner_options(&self, gps: &[GpsRecord]) -> ModelAlignerOptions {
393 let mut o = ModelAlignerOptions { max_error: 3.0, ..Default::default() };
394 o.ransac.random_seed = self.cfg.seed;
395 if self.cfg.fixed_enu_origin {
396 if let Some(g) = gps.first() {
397 o.origin = EnuOrigin::Explicit { lat: g.lat, lon: g.lon, alt: g.alt };
398 }
399 }
400 o
401 }
402
403 fn wait_refined(&self, k: usize, generation: u64) -> Option<Arc<Reconstruction>> {
407 let t0 = Instant::now();
408 let me = k + 1;
409 loop {
410 if let Some(z) = self.bg().ready.get(&k) {
411 return Some(z.model.clone());
412 }
413 if self.generation(me) != generation || t0.elapsed().as_secs() > 1800 {
414 return None;
415 }
416 std::thread::sleep(std::time::Duration::from_millis(20));
417 }
418 }
419
420 fn wait_finished(&self, k: usize, me: usize, generation: u64) -> bool {
423 let t0 = Instant::now();
424 loop {
425 {
426 let bg = self.bg();
427 if bg.finished.get(&k).is_some_and(|g| *g == bg.generation.get(&k).copied().unwrap_or(0)) {
428 return true;
429 }
430 }
431 if self.generation(me) != generation || t0.elapsed().as_secs() > 1800 {
432 return false;
433 }
434 std::thread::sleep(std::time::Duration::from_millis(20));
435 }
436 }
437
438 fn replace_shown(&self, k: usize, generation: u64, model: &Arc<Reconstruction>, st: &ModelStats) {
443 if !self.wait_finished(k - 1, k, generation) {
444 return;
445 }
446 for j in 0..k {
447 let snap = {
448 let bg = self.bg();
449 let gj = bg.generation.get(&j).copied().unwrap_or(0);
450 bg.ready
451 .get(&j)
452 .filter(|z| z.generation == gj)
453 .map(|z| (z.clone(), gj, bg.zone_keep.get(&j).cloned(), bg.dense_basis.get(&j).cloned()))
454 };
455 let Some((z, gj, keep, basis)) = snap else { continue };
456 let mut out = Out { sh: self, fg: None, guard: Some((j, gj)) };
457 let change = basis.as_ref().map_or(0.0, |b| intrinsics_change(b.cameras(), model.cameras()));
458 let mut basis_new = None;
459 let cloud = z.cloud.as_ref().map(|old| {
460 if change >= self.cfg.settle_tol {
461 if let Some(c) = keep.as_ref().and_then(|keep| self.dense_region("refined", j, model, keep, &mut out)) {
462 out.log(format!("[settle] 구역 {j} 다시 조밀화 (내부값 변화 {:.2}%)", change * 100.0));
463 basis_new = Some(model.clone());
464 return c;
465 }
466 }
467 let by_points = keep
469 .as_ref()
470 .and_then(|keep| {
471 let keep = keep.clone();
472 let filter: NameFilter = Arc::new(move |n: &str| keep.contains(n));
473 fit_shared_points(&z.model, model, Some(filter), MIN_SHARED_PAIRS)
474 })
475 .or_else(|| fit_shared_points(&z.model, model, None, MIN_SHARED_PAIRS));
476 let mut c = (**old).clone();
477 if let Some((f, n)) = by_points {
478 let t = f.sim3;
479 cumulus3d_align::transform_cloud(&mut c, &t);
480 out.log(format!(
481 "[settle] 구역 {j} 점군 이동: 공유 점 {n}, 잔차 중앙 {:.3}m, 축척 {:.4}, 이동 {:.2}m",
482 f.median,
483 t.scale,
484 t.translation.norm()
485 ));
486 } else {
487 let t = keep.as_ref().and_then(|keep| sim3_between(&z.model, model, |n| keep.contains(n)));
488 match t {
489 Some(t) => {
490 cumulus3d_align::transform_cloud(&mut c, &t);
491 out.log(format!(
492 "[settle] 구역 {j} 점군 이동(카메라 중심 대체): 이동 {:.3}m 축척 {:.4}",
493 t.translation.norm(),
494 t.scale
495 ));
496 }
497 None => out.log(format!("[settle] 구역 {j} 점군 이동 생략: 공통 영상 부족")),
498 }
499 }
500 Arc::new(c)
501 });
502 {
503 let mut bg = self.bg();
504 if bg.generation.get(&j).copied().unwrap_or(0) != gj {
505 continue;
506 }
507 let Some(r) = bg.ready.get_mut(&j).filter(|r| r.generation == gj) else { continue };
508 r.model = model.clone();
509 r.cloud = cloud.clone();
510 r.done = true;
511 if let Some(b) = basis_new {
512 bg.dense_basis.insert(j, b);
513 }
514 }
515 let (range, m) = (z.range, model.clone());
516 let (registered, mean_reproj_px) = (st.registered_image_count, st.mean_reprojection_error);
517 out.emit(|meta| Event::ZoneRefinedPose { meta, range, registered, mean_reproj_px, model: m });
518 let m = model.clone();
519 out.emit(|meta| Event::ZoneRefined { meta, range, dense: cloud.is_some(), cloud: cloud.unwrap_or_default(), model: m });
520 }
521 }
522
523 fn align_enu(&self, rec: &mut Reconstruction, what: &str, gps_all: &[GpsRecord], out: &mut Out) -> bool {
524 let names: BTreeSet<String> = rec.registered_images().into_iter().filter_map(|i| rec.image(i).map(|im| im.name.clone())).collect();
525 let gps: Vec<GpsRecord> = gps_all.iter().filter(|g| names.contains(&g.name)).cloned().collect();
526 let opts = self.aligner_options(gps_all);
527 let t0 = Instant::now();
528 let r = align_to_gps(rec, &gps, &opts);
529 out.stage("align", what, t0);
530 match r {
531 Ok(a) => {
532 out.log(format!(
533 "[align] {what}: 공통 {} 인라이어 {} 오차 평균 {:.3}m 중앙 {:.3}m 축척 {:.4}",
534 a.common.len(),
535 a.num_inliers,
536 a.mean_error,
537 a.median_error,
538 a.sim3.scale
539 ));
540 true
541 }
542 Err(e) => {
543 out.warn(format!("[align] {what}: 정렬 실패({e}) — 정렬 없이 계속"));
544 false
545 }
546 }
547 }
548
549 fn dense_region(&self, kind: &str, k: usize, model: &Reconstruction, keep: &HashSet<String>, out: &mut Out) -> Option<Arc<PointCloud>> {
551 let dense = self.cfg.dense.as_ref()?;
552 let what = format!("{kind} {k}");
553 let t = Instant::now();
554 let r = dense_model(model, |n| keep.contains(n), &self.cfg.image_root, dense);
555 let total = t.elapsed();
556 match r {
557 Ok(run) => {
558 self.times.add("undistort", run.undistort_time);
559 self.times.add("densify", run.densify_time);
560 let tm = &run.output.timings;
561 out.stamped(format!(
562 "[time] dense {what}: 전체 {:.1}s (왜곡 보정+장면 {:.1}s, 대기 {:.1}s, 조밀화 {:.1}s = 이웃 {:.1} 준비 {:.1} 깊이 {:.1} 필터 {:.1} 융합 {:.1}, 캐시 적중 {})",
563 total.as_secs_f64(),
564 run.undistort_time.as_secs_f64(),
565 run.lock_wait.as_secs_f64(),
566 run.densify_time.as_secs_f64(),
567 tm.neighbors.as_secs_f64(),
568 tm.prepare.as_secs_f64(),
569 tm.depth().as_secs_f64(),
570 tm.filter.as_secs_f64(),
571 tm.fusion.as_secs_f64(),
572 run.output.cache_hits
573 ));
574 out.log(format!("[dense] {kind} {k} frames {} points {}", run.frames, run.output.cloud.len()));
575 let mut cloud = run.output.cloud;
576 if !cloud.is_empty() && cloud.colors.is_empty() {
578 cloud.colors = vec![[255, 255, 255]; cloud.len()];
579 }
580 Some(Arc::new(cloud))
581 }
582 Err(e) => {
583 out.warn(format!("[dense] {kind} {k} 실패: {e}"));
584 None
585 }
586 }
587 }
588
589 fn refine_job(
591 self: Arc<Self>,
592 range: ZoneRange,
593 mut rec: Reconstruction,
594 generation: u64,
595 gps: Arc<Vec<GpsRecord>>,
596 keep: Arc<HashSet<String>>,
597 cam_ids: Arc<BTreeMap<String, CameraId>>,
598 ) {
599 let k = range.zone;
600 let sh = &*self;
601 let mut out = Out { sh, fg: None, guard: Some((k, generation)) };
602 let _finish = FinishMark { sh, zone: k, generation };
603 out.emit(|meta| Event::RefineStarted { meta, zone: k });
604 let mut ba = BaConfig::default();
605 let mut prev: Option<Arc<Reconstruction>> = None;
608 let mut settling = false;
609 if sh.cfg.stable_display {
610 let mut bg = sh.bg();
611 if bg.generation.get(&k).copied().unwrap_or(0) == generation {
612 bg.zone_keep.insert(k, keep.clone());
613 }
614 }
615 if sh.cfg.stable_display {
616 let targets = if sh.cfg.gps_prior_sigma > 0.0 { enu_targets(&rec, &gps) } else { HashMap::new() };
617 if k == 0 {
618 let (src, dst): (Vec<_>, Vec<_>) = targets.iter().filter_map(|(id, g)| rec.projection_center(*id).map(|c| (c, *g))).unzip();
620 if src.len() >= 3 {
621 if let Some(t) = cumulus3d_align::umeyama(&src, &dst, true) {
622 rec.transform(&t);
623 out.log(format!("[stable] refined 0: ENU 초기 정렬 GPS {} 개, 축척 {:.4}", src.len(), t.scale));
624 }
625 }
626 settling = true;
627 } else {
628 prev = sh.wait_refined(k - 1, generation);
629 settling = sh.bg().settled.is_none();
630 match (&prev, settling) {
631 (Some(p), false) => {
632 let (n, scale) = anchor_to_shown(&mut rec, p, &mut ba);
633 out.log(format!("[stable] refined {k}: 고정 영상 {n}, 이전 구역에 맞춘 축척 {scale:.4}"));
634 }
635 (Some(p), true) => {
636 let scale = sim3_onto_shown(&mut rec, p).map_or(1.0, |(t, _)| t.scale);
637 out.log(format!("[stable] refined {k}: 정착 전 — 모든 자세·내부값 자유, 이전 구역에 맞춘 축척 {scale:.4}"));
638 }
639 (None, _) => out.warn(format!("[stable] refined {k}: 앞 구역 결과 없음 — 고정 없이 조정")),
640 }
641 }
642 let sigma = sh.cfg.gps_prior_sigma;
643 for (id, g) in &targets {
644 if !ba.constant_poses.contains(id) {
645 ba.position_priors.insert(*id, (*g, sigma));
646 }
647 }
648 if !ba.position_priors.is_empty() {
649 out.log(format!("[stable] refined {k}: GPS 위치 제약 {} 개 (σ {sigma} m)", ba.position_priors.len()));
650 }
651 }
652 if let Some(pr) = &sh.cfg.intrinsics_prior {
653 let pr = pr.resolve(&cam_ids);
654 for (cid, cam) in rec.cameras() {
655 if !ba.constant_cameras.contains(cid) {
656 if let Some(p) = pr.ba_prior(cam) {
657 ba.intrinsics_priors.insert(*cid, p);
658 }
659 }
660 }
661 if !ba.intrinsics_priors.is_empty() {
662 out.log(format!(
663 "[prior] refined {k}: 내부값 사전 카메라 {} 개 (초점 σ {} 상대, 왜곡 σ {})",
664 ba.intrinsics_priors.len(),
665 pr.focal_sigma,
666 pr.dist_sigma
667 ));
668 }
669 }
670 let t0 = Instant::now();
671 let r = bundle_adjust_with(&mut rec, &ba, &*sh.cfg.ba_backend);
672 out.stage("ba", &format!("refined {k}"), t0);
673 match r {
674 Ok(s) => out.log(format!(
675 "[ba] refined {k}: 반복 {} 비용 {:.4e} → {:.4e} ({:?}), RMS {:.3}px",
676 s.num_iterations,
677 s.initial_cost,
678 s.final_cost,
679 s.termination,
680 s.rms_reprojection_error()
681 )),
682 Err(e) => out.warn(format!("[ba] refined {k} 실패: {e}")),
683 }
684 if let (true, Some(p)) = (settling && k > 0, &prev) {
686 match fit_shared_points(&rec, p, None, MIN_SHARED_PAIRS) {
687 Some((f, n)) => {
688 let t = f.sim3;
689 rec.transform(&t);
690 out.log(format!(
691 "[settle] refined {k}: 화면 좌표계 재정렬 공유 점 {n}, 잔차 중앙 {:.3}m, 축척 {:.4}, 이동 {:.2}m",
692 f.median,
693 t.scale,
694 t.translation.norm()
695 ));
696 }
697 None => match sim3_onto_shown(&mut rec, p) {
698 Some((t, _)) => out.log(format!(
699 "[settle] refined {k}: 화면 좌표계 재정렬 (카메라 중심 대체) 축척 {:.4}, 이동 {:.2}m",
700 t.scale,
701 t.translation.norm()
702 )),
703 None => out.warn(format!("[settle] refined {k}: 화면 좌표계 재정렬 생략: 공통 영상 부족")),
704 },
705 }
706 }
707 let adjusted = Arc::new(rec.clone());
708 out.emit(|meta| Event::ZoneAdjusted { meta, zone: k, model: adjusted });
709 if !sh.cfg.stable_display {
710 sh.align_enu(&mut rec, &format!("refined {k}"), &gps, &mut out);
711 }
712 let st = ModelStats::compute(&rec);
713 let mut settle_now = false;
715 if settling {
716 let change = prev.as_ref().map(|p| intrinsics_change(p.cameras(), rec.cameras()));
717 let forced = k >= sh.cfg.settle_max_zone;
718 settle_now = forced || change.is_some_and(|c| c < sh.cfg.settle_tol);
719 let verdict = match (settle_now, forced) {
720 (true, true) => "강제 고정",
721 (true, false) => "고정",
722 _ => "정착 중",
723 };
724 match change {
725 Some(c) => out.log(format!("[settle] refined {k}: 내부값 변화 {:.2}% → {verdict}", c * 100.0)),
726 None => out.log(format!("[settle] refined {k}: → {verdict}")),
727 }
728 }
729 let model = Arc::new(rec);
730 {
731 let mut bg = sh.bg();
732 if bg.generation.get(&k).copied().unwrap_or(0) == generation {
733 if settle_now {
734 bg.settled = Some(k);
735 }
736 bg.ready.insert(k, RefinedZone { range, model: model.clone(), cloud: None, done: false, generation });
737 }
738 }
739 let m = model.clone();
740 out.emit(|meta| Event::ZoneRefinedPose {
741 meta,
742 range,
743 registered: st.registered_image_count,
744 mean_reproj_px: st.mean_reprojection_error,
745 model: m,
746 });
747 let cloud = sh.dense_region("refined", k, &model, &keep, &mut out);
748 {
749 let mut bg = sh.bg();
750 if bg.generation.get(&k).copied().unwrap_or(0) == generation {
751 if let Some(z) = bg.ready.get_mut(&k).filter(|z| z.generation == generation) {
752 z.cloud = cloud.clone();
753 z.done = true;
754 }
755 bg.dense_basis.insert(k, model.clone());
756 }
757 }
758 let m = model.clone();
759 out.emit(|meta| Event::ZoneRefined { meta, range, dense: cloud.is_some(), cloud: cloud.unwrap_or_default(), model: m });
760 if settling && k > 0 {
762 sh.replace_shown(k, generation, &model, &st);
763 }
764 }
765}
766
767struct Out<'a> {
769 sh: &'a Shared,
770 fg: Option<&'a mut Vec<Event>>,
771 guard: Option<(usize, u64)>,
772}
773
774impl Out<'_> {
775 fn emit(&mut self, make: impl FnOnce(Meta) -> Event) {
776 if let Some((k, g)) = self.guard {
777 if self.sh.generation(k) != g {
778 return;
779 }
780 }
781 self.sh.emitter.emit(make, self.fg.as_deref_mut());
782 }
783
784 fn log(&mut self, line: impl Into<String>) {
785 let line = line.into();
786 self.emit(|meta| Event::Log { meta, line, stamped: false });
787 }
788
789 fn stamped(&mut self, line: impl Into<String>) {
790 let line = line.into();
791 self.emit(|meta| Event::Log { meta, line, stamped: true });
792 }
793
794 fn warn(&mut self, line: String) {
796 self.log(line.clone());
797 self.emit(|meta| Event::Warning { meta, message: line });
798 }
799
800 fn stage(&mut self, stage: &str, what: &str, t0: Instant) {
802 let d = t0.elapsed();
803 self.sh.times.add(stage, d);
804 self.stamped(format!("[time] {stage} {what}: {:.3}s", d.as_secs_f64()));
805 }
806}
807
808#[derive(Clone)]
811struct Checkpoint {
812 position: usize,
814 chain: Option<Reconstruction>,
815 adopted: Option<usize>,
816 cam_ids: Arc<BTreeMap<String, CameraId>>,
817 graph: Arc<MatchGraph>,
818 gps_len: usize,
819}
820
821#[derive(Clone)]
823pub struct Session {
824 shared: Arc<Shared>,
825 store: Arc<FeatureStore>,
826 graph: Arc<MatchGraph>,
827 frames: Arc<Vec<Vec<FrameImage>>>,
828 position_of: Arc<HashMap<String, usize>>,
829 ingested: usize,
830 cam_ids: Arc<BTreeMap<String, CameraId>>,
831 gps: Arc<Vec<GpsRecord>>,
832 chain: Option<Reconstruction>,
833 adopted: Option<usize>,
834 arrived: BTreeMap<usize, ZoneRange>,
835 previews: Arc<BTreeMap<usize, PreviewZone>>,
836 checkpoints: VecDeque<Checkpoint>,
837 started: bool,
838 finished: bool,
839 failed: Option<String>,
840 announced: Option<usize>,
842}
843
844pub fn step(session: &Session, frames: FrameSet) -> (Session, Vec<Event>) {
846 session.clone().advance(Input::Frames(frames))
847}
848
849pub fn poll(session: &Session) -> (Session, Vec<Event>) {
851 session.clone().advance(Input::Poll)
852}
853
854pub fn command(session: &Session, cmd: Command) -> (Session, Vec<Event>) {
856 session.clone().advance(Input::Command(cmd))
857}
858
859pub fn finish(session: &Session) -> (Session, Vec<Event>) {
861 session.clone().advance(Input::Finish)
862}
863
864impl Session {
865 pub fn new(cfg: SessionConfig) -> Self {
867 let extractor = FeatureExtractor::with_backend(cfg.sift.clone());
868 let gps = Arc::new(cfg.gps.clone());
869 let shared = Arc::new(Shared {
870 cfg,
871 extractor,
872 emitter: Emitter(Mutex::new(EmitState { version: 0, sink: None, inbox: Vec::new() })),
873 times: StageTimes::default(),
874 bg: Mutex::new(Background::default()),
875 start: Mutex::new(None),
876 });
877 Self {
878 shared,
879 store: Arc::new(FeatureStore::new()),
880 graph: Arc::new(MatchGraph::new()),
881 frames: Arc::new(Vec::new()),
882 position_of: Arc::new(HashMap::new()),
883 ingested: 0,
884 cam_ids: Arc::new(BTreeMap::new()),
885 gps,
886 chain: None,
887 adopted: None,
888 arrived: BTreeMap::new(),
889 previews: Arc::new(BTreeMap::new()),
890 checkpoints: VecDeque::new(),
891 started: false,
892 announced: None,
893 finished: false,
894 failed: None,
895 }
896 }
897
898 pub fn set_sink(&self, sink: Option<EventSink>) {
901 let mut st = self.shared.emitter.lock();
902 if let Some(s) = &sink {
903 for ev in std::mem::take(&mut st.inbox) {
904 s(ev);
905 }
906 }
907 st.sink = sink;
908 }
909
910 pub fn emit_external(&self, make: impl FnOnce(Meta) -> Event) {
913 self.shared.emitter.emit(make, None);
914 }
915
916 pub fn advance(mut self, input: Input) -> (Session, Vec<Event>) {
918 let evs = self.apply(input);
919 (self, evs)
920 }
921
922 pub fn apply(&mut self, input: Input) -> Vec<Event> {
925 let sh = self.shared.clone();
926 let mut evs: Vec<Event> = Vec::new();
927 {
928 let mut out = Out { sh: &sh, fg: Some(&mut evs), guard: None };
929 if self.finished {
930 if !matches!(input, Input::Poll) {
931 out.emit(|meta| Event::Warning { meta, message: "세션이 이미 끝남 — 입력 무시".into() });
932 }
933 } else {
934 if !matches!(input, Input::Poll) {
935 self.ensure_started(&mut out);
936 }
937 match input {
938 Input::Frames(fs) => {
939 self.announce(&fs, &mut out);
940 if let Some(e) = &self.failed {
941 let message = format!("이전 오류로 중단된 세션 — 프레임 무시 ({e})");
942 out.emit(|meta| Event::Warning { meta, message });
943 } else if let Err(e) = self.process_frames(fs, &mut out) {
944 self.failed = Some(e.clone());
945 out.emit(|meta| Event::Error { meta, message: e });
946 }
947 }
948 Input::Command(c) => self.apply_command(c, &mut out),
949 Input::Poll => {}
950 Input::Log(line) => out.log(line),
951 Input::Finish => {
952 if self.failed.is_none() {
953 self.finish_session(&mut out);
954 }
955 self.finished = true;
956 }
957 }
958 }
959 }
960 let bg = std::mem::take(&mut sh.emitter.lock().inbox);
962 if !bg.is_empty() {
963 evs.extend(bg);
964 evs.sort_by_key(|e| e.meta().version);
965 }
966 evs
967 }
968
969 pub fn positions(&self) -> usize {
973 self.frames.len()
974 }
975 pub fn frames(&self) -> &[Vec<FrameImage>] {
977 &self.frames
978 }
979 pub fn position_of(&self, name: &str) -> Option<usize> {
981 self.position_of.get(name).copied()
982 }
983 pub fn model(&self) -> Option<&Reconstruction> {
985 self.chain.as_ref()
986 }
987 pub fn adopted(&self) -> Option<usize> {
989 self.adopted
990 }
991 pub fn store(&self) -> &FeatureStore {
993 &self.store
994 }
995 pub fn graph(&self) -> &MatchGraph {
997 &self.graph
998 }
999 pub fn config(&self) -> &SessionConfig {
1001 &self.shared.cfg
1002 }
1003 pub fn arrived_zones(&self) -> Vec<ZoneRange> {
1005 self.arrived.values().copied().collect()
1006 }
1007 pub fn previews(&self) -> &BTreeMap<usize, PreviewZone> {
1009 &self.previews
1010 }
1011 pub fn refined(&self) -> BTreeMap<usize, RefinedZone> {
1013 self.shared.bg().ready.clone()
1014 }
1015 pub fn pending_jobs(&self) -> usize {
1017 self.shared.bg().jobs.iter().filter(|j| !j.is_finished()).count()
1018 }
1019 pub fn version(&self) -> u64 {
1021 self.shared.emitter.lock().version
1022 }
1023 pub fn failed(&self) -> Option<&str> {
1025 self.failed.as_deref()
1026 }
1027 pub fn is_finished(&self) -> bool {
1029 self.finished
1030 }
1031
1032 fn ensure_started(&mut self, out: &mut Out) {
1035 if self.started {
1036 return;
1037 }
1038 self.started = true;
1039 *self.shared.start.lock().unwrap_or_else(|e| e.into_inner()) = Some(Instant::now());
1040 let positions = self.shared.cfg.total_positions;
1041 out.emit(|meta| Event::Started { meta, positions });
1042 }
1043
1044 fn announce(&mut self, fs: &FrameSet, out: &mut Out) {
1046 let sh = self.shared.clone();
1047 let p = self.frames.len();
1048 if !sh.cfg.decode_frames || self.finished || self.failed.is_some() || self.announced == Some(p) {
1049 return;
1050 }
1051 self.ensure_started(out);
1052 self.announced = Some(p);
1053 let decoded: Vec<Result<Frame, String>> = fs
1054 .images
1055 .par_iter()
1056 .map(|im| {
1057 let path = sh.cfg.image_root.join(&im.name);
1058 let img = image::ImageReader::open(&path)
1059 .and_then(|r| r.with_guessed_format())
1060 .map_err(|e| format!("{}: {e}", path.display()))?
1061 .decode()
1062 .map_err(|e| format!("{}: 디코딩 실패: {e}", path.display()))?
1063 .into_rgb8();
1064 let gps = fs.gps.iter().chain(self.gps.iter()).find(|g| g.name == im.name).cloned();
1065 let (width, height) = img.dimensions();
1066 Ok(Frame {
1067 position: p,
1068 camera: im.camera.clone(),
1069 name: im.name.clone(),
1070 path,
1071 width,
1072 height,
1073 rgb: Arc::from(img.into_raw()),
1074 gps,
1075 })
1076 })
1077 .collect();
1078 for d in decoded {
1079 match d {
1080 Ok(f) => {
1081 let frame = Arc::new(f);
1082 out.emit(|meta| Event::FrameDecoded { meta, frame });
1083 }
1084 Err(e) => out.emit(|meta| Event::Warning { meta, message: format!("[frame] {e}") }),
1085 }
1086 }
1087 }
1088
1089 fn process_frames(&mut self, fs: FrameSet, out: &mut Out) -> Result<(), String> {
1090 let sh = self.shared.clone();
1091 let cfg = &sh.cfg;
1092 let p = self.frames.len();
1093 let names: Vec<String> = fs.images.iter().map(|i| i.name.clone()).collect();
1094 {
1095 let pos = Arc::make_mut(&mut self.position_of);
1096 for n in &names {
1097 pos.insert(n.clone(), p);
1098 }
1099 }
1100 Arc::make_mut(&mut self.frames).push(fs.images.clone());
1101 self.ingested += fs.images.len();
1102 if !fs.gps.is_empty() {
1103 Arc::make_mut(&mut self.gps).extend(fs.gps);
1104 }
1105 out.emit(|meta| Event::FrameIngested { meta, position: p, images: names });
1106
1107 let t0 = Instant::now();
1109 let r = self.extract(p, &fs.images, out);
1110 out.stage("feature", &format!("pos {p}"), t0);
1111 r?;
1112
1113 let pairs: Vec<(ImageId, ImageId)> = pairs_for(&self.frames, p)
1115 .into_iter()
1116 .filter_map(|(a, b)| Some((self.store.image_id_by_name(&a)?, self.store.image_id_by_name(&b)?)))
1117 .collect();
1118 let mut mopts = PairMatchingOptions::default();
1119 mopts.geometry.ransac.random_seed = cfg.seed;
1120 let t0 = Instant::now();
1121 let st = match_pairs_with(&self.store, &pairs, &mopts, cfg.matcher.as_ref(), cfg.sparse.two_view.as_ref());
1122 out.stage("matching", &format!("pos {p} ({} pairs)", pairs.len()), t0);
1123 let st = st.map_err(|e| e.to_string())?;
1124 Arc::make_mut(&mut self.graph).update_from_store(&self.store, &MatchGraphOptions::default());
1125 out.log(format!(
1126 "[match] pos {p}: 짝 {} 매칭 {} 유효 기하 {} (그래프 영상 {})",
1127 pairs.len(),
1128 st.num_matched,
1129 st.num_valid_geometries,
1130 self.graph.image_ids().len()
1131 ));
1132 let (np, nv, nm) = (pairs.len(), st.num_valid_geometries, st.num_matched);
1133 out.emit(|meta| Event::PairsMatched { meta, pairs: np, verified: nv, position: p, matched: nm });
1134
1135 let (span, ovl) = (cfg.span, cfg.overlap);
1137 if p + 1 < span + ovl {
1138 self.checkpoint(p);
1139 return Ok(());
1140 }
1141 if p + 1 == span + ovl {
1142 let t0 = Instant::now();
1143 let gopts = GlobalSfmOptions { ba_backend: BaBackendRef::new(cfg.ba_backend.clone()), ..GlobalSfmOptions::script() };
1144 let o = global_mapper(&self.store, &self.graph, &gopts);
1145 out.stage("mapper", &format!("pos {p}"), t0);
1146 let o = o.map_err(|e| e.to_string())?;
1147 if let Some(f) = &o.failure {
1148 out.warn(format!("[mapper] 실패 보고: {f}"));
1149 }
1150 out.log(format!("[mapper] {:?}", o.summary));
1151 let rec = o.reconstruction;
1152 let model = Arc::new(rec.clone());
1153 let registered = rec.registered_image_count();
1154 out.emit(|meta| Event::ModelInitialized { meta, registered, position: p, model });
1155 let ids = rec.registered_images();
1156 self.emit_registered(&rec, &ids, out);
1157 self.chain = Some(rec);
1158 } else {
1159 let mut k = self.adopted.map_or(0, |a| a + 1);
1161 loop {
1162 let r = sh.bg().ready.get(&k).map(|z| z.model.clone());
1163 let Some(r) = r else { break };
1164 self.chain = Some((*r).clone());
1165 self.adopted = Some(k);
1166 out.emit(|meta| Event::BaseAdopted { meta, zone: k });
1167 k += 1;
1168 }
1169 if let Some(base) = &self.chain {
1170 let mut reg = base.clone();
1171 let t0 = Instant::now();
1172 let rr =
1173 register_images_with(&mut reg, &self.store, &self.graph, &RegistrationOptions::default(), cfg.sparse.abs_pose.as_ref());
1174 out.stage("registration", &format!("pos {p}"), t0);
1175 match rr {
1176 Ok(rep) => {
1177 let new_ids = rep.registered();
1178 out.log(format!("[register] pos {p}: 시도 {} 등록 {}", rep.attempts.len(), new_ids.len()));
1179 let scope = if cfg.incremental_triangulation {
1180 TriangulationScope::Images(new_ids.clone())
1181 } else {
1182 TriangulationScope::AllRegistered
1183 };
1184 let topts = PointTriangulatorOptions { clear_points: false, scope, ..Default::default() };
1185 let t0 = Instant::now();
1186 let tr = triangulate_points_with(&mut reg, &self.graph, &topts, cfg.sparse.triangulation.as_ref());
1187 out.stage("triangulation", &format!("pos {p}"), t0);
1188 match tr {
1189 Ok(t) => {
1190 out.log(format!(
1191 "[triangulate] pos {p}: 생성 {} 연장 {} 완성 {} 병합 {} 필터 {} (점 {})",
1192 t.num_created,
1193 t.num_continued,
1194 t.num_completed,
1195 t.num_merged,
1196 t.num_filtered,
1197 reg.num_points3d()
1198 ));
1199 self.emit_registered(®, &new_ids, out);
1200 self.chain = Some(reg);
1201 }
1202 Err(e) => out.warn(format!("[triangulate] pos {p} 실패: {e} (이전 모델 유지)")),
1203 }
1204 }
1205 Err(e) => out.warn(format!("[register] pos {p} 실패: {e}")),
1206 }
1207 }
1208 }
1209 let registered = self.chain.as_ref().map_or(0, |r| r.registered_image_count());
1210 let expected = self.ingested;
1211 out.emit(|meta| Event::PositionDone { meta, position: p, registered, expected });
1212
1213 for z in zones_ending_at(p, span, ovl, cfg.total_positions) {
1215 if !self.arrived.contains_key(&z.zone) {
1216 self.process_zone(z, out);
1217 }
1218 }
1219 self.checkpoint(p);
1220 Ok(())
1221 }
1222
1223 fn extract(&mut self, p: usize, images: &[FrameImage], out: &mut Out) -> Result<(), String> {
1224 let sh = self.shared.clone();
1225 let cfg = &sh.cfg;
1226 if self.cam_ids.is_empty() {
1227 let names: Vec<String> = images.iter().map(|i| i.name.clone()).collect();
1228 let mut o = cfg.extraction.clone();
1229 o.reader.camera_mode = CameraMode::PerFolder;
1230 let rep = sh.extractor.extract_files(&self.store, &cfg.image_root, &names, &o).map_err(|e| e.to_string())?;
1231 report_extraction(&rep, p, out);
1232 let cams = Arc::make_mut(&mut self.cam_ids);
1233 for im in images {
1234 let s = self.store.image_by_name(&im.name).ok_or_else(|| format!("{} 첫 영상 특징 추출 실패", im.camera))?;
1235 cams.insert(im.camera.clone(), s.camera_id);
1236 }
1237 self.apply_intrinsics_prior(out)?;
1238 } else {
1239 for im in images {
1240 let mut o = cfg.extraction.clone();
1241 let known = self.cam_ids.get(&im.camera).copied();
1242 o.reader.camera_mode = known.map_or(CameraMode::PerFolder, CameraMode::Existing);
1243 let rep = sh
1244 .extractor
1245 .extract_files(&self.store, &cfg.image_root, std::slice::from_ref(&im.name), &o)
1246 .map_err(|e| e.to_string())?;
1247 report_extraction(&rep, p, out);
1248 if known.is_none() {
1249 if let Some(s) = self.store.image_by_name(&im.name) {
1250 Arc::make_mut(&mut self.cam_ids).insert(im.camera.clone(), s.camera_id);
1251 }
1252 self.apply_intrinsics_prior(out)?;
1253 }
1254 }
1255 }
1256 Ok(())
1257 }
1258
1259 fn apply_intrinsics_prior(&self, out: &mut Out) -> Result<(), String> {
1262 let Some(pr) = &self.shared.cfg.intrinsics_prior else { return Ok(()) };
1263 let pr = pr.resolve(&self.cam_ids);
1264 for mut cam in self.store.cameras() {
1265 let before = cam.params.clone();
1266 if pr.apply(&mut cam)? && cam.params != before {
1267 out.log(format!("[prior] 카메라 {}: 내부값 초기화 {} → {}", cam.camera_id, fmt_params(&before), fmt_params(&cam.params)));
1268 self.store.update_camera(cam).map_err(|e| e.to_string())?;
1269 }
1270 }
1271 Ok(())
1272 }
1273
1274 fn emit_registered(&self, rec: &Reconstruction, ids: &[ImageId], out: &mut Out) {
1275 for &id in ids {
1276 let (Some(im), Some(pose)) = (rec.image(id), rec.world_to_cam(id)) else { continue };
1277 let image = im.name.clone();
1278 let position = self.position_of(&image).unwrap_or(usize::MAX);
1279 out.emit(|meta| Event::FrameRegistered { meta, position, image, image_id: id, cam_from_world: pose });
1280 }
1281 }
1282
1283 fn process_zone(&mut self, range: ZoneRange, out: &mut Out) {
1285 let sh = self.shared.clone();
1286 let k = range.zone;
1287 let snap = self.chain.clone().unwrap_or_default();
1288 let m = Arc::new(snap.clone());
1289 out.emit(|meta| Event::ZoneArrived { meta, range, model: m });
1290 if sh.cfg.stable_display {
1291 let keep: Arc<HashSet<String>> =
1292 Arc::new(self.position_of.iter().filter(|(_, p)| **p >= range.lo && **p < range.hi).map(|(n, _)| n.clone()).collect());
1293 let generation = sh.generation(k);
1294 let (sh2, gps, cam_ids) = (sh.clone(), self.gps.clone(), self.cam_ids.clone());
1295 let h = std::thread::spawn(move || sh2.refine_job(range, snap, generation, gps, keep, cam_ids));
1296 sh.bg().jobs.push(h);
1297 self.arrived.insert(k, range);
1298 return;
1299 }
1300 let mut preview = snap.clone();
1301 let mut frame = match self.adopted {
1302 Some(j) => format!("refined_{j}"),
1303 None => "unaligned".to_string(),
1304 };
1305 if k == 0 && sh.align_enu(&mut preview, "preview 0", &self.gps, out) {
1306 frame = "gps".to_string();
1307 }
1308 let keep: Arc<HashSet<String>> =
1309 Arc::new(self.position_of.iter().filter(|(_, p)| **p >= range.lo && **p < range.hi).map(|(n, _)| n.clone()).collect());
1310 let generation = sh.generation(k);
1311 {
1312 let (sh2, gps, keep, cam_ids) = (sh.clone(), self.gps.clone(), keep.clone(), self.cam_ids.clone());
1313 let h = std::thread::spawn(move || sh2.refine_job(range, snap, generation, gps, keep, cam_ids));
1314 sh.bg().jobs.push(h);
1315 }
1316 let cloud = sh.dense_region("preview", k, &preview, &keep, out);
1317 let model = Arc::new(preview);
1318 self.arrived.insert(k, range);
1319 Arc::make_mut(&mut self.previews)
1320 .insert(k, PreviewZone { range, model: model.clone(), cloud: cloud.clone(), frame: frame.clone() });
1321 out.emit(|meta| Event::ZonePreview { meta, range, dense: cloud.is_some(), cloud: cloud.unwrap_or_default(), model, frame });
1322 }
1323
1324 fn checkpoint(&mut self, position: usize) {
1325 let n = self.shared.cfg.history;
1326 if n == 0 {
1327 return;
1328 }
1329 self.checkpoints.push_back(Checkpoint {
1330 position,
1331 chain: self.chain.clone(),
1332 adopted: self.adopted,
1333 cam_ids: self.cam_ids.clone(),
1334 graph: self.graph.clone(),
1335 gps_len: self.gps.len(),
1336 });
1337 while self.checkpoints.len() > n {
1338 self.checkpoints.pop_front();
1339 }
1340 }
1341
1342 fn drop_zone(&mut self, k: usize, out: &mut Out) {
1343 self.arrived.remove(&k);
1344 Arc::make_mut(&mut self.previews).remove(&k);
1345 self.shared.invalidate(k);
1346 out.emit(|meta| Event::ZoneInvalidated { meta, zone: k });
1347 }
1348
1349 fn apply_command(&mut self, cmd: Command, out: &mut Out) {
1350 match cmd {
1351 Command::ResetFrom(p) => self.reset_from(p, out),
1352 Command::InvalidateZone(k) => {
1353 let Some(range) = self.arrived.get(&k).copied() else {
1354 out.emit(|meta| Event::Warning {
1355 meta, message: format!("구역 {k} 은 아직 도착하지 않음 — 무효화할 것 없음")
1356 });
1357 return;
1358 };
1359 self.drop_zone(k, out);
1360 self.process_zone(range, out);
1362 }
1363 }
1364 }
1365
1366 fn reset_from(&mut self, p: usize, out: &mut Out) {
1369 let npos = self.frames.len();
1370 if p > npos {
1371 out.emit(|meta| Event::Warning { meta, message: format!("ResetFrom({p}): 처리한 위치 {npos} 보다 큼 — 무시") });
1372 return;
1373 }
1374 let cp = if p == 0 {
1375 None
1376 } else {
1377 match self.checkpoints.iter().find(|c| c.position + 1 == p) {
1378 Some(c) => Some(c.clone()),
1379 None => {
1380 out.emit(|meta| Event::Warning {
1381 meta, message: format!("ResetFrom({p}): 되감기 지점 없음(history 부족) — 무시")
1382 });
1383 return;
1384 }
1385 }
1386 };
1387 let gone: Vec<usize> = self.arrived.iter().filter(|(_, r)| r.hi.saturating_sub(1) >= p).map(|(k, _)| *k).collect();
1388 for k in gone {
1389 self.drop_zone(k, out);
1390 }
1391 let pos = self.position_of.clone();
1392 self.store = Arc::new(self.store.retain_copy(|im| pos.get(&im.name).is_some_and(|q| *q < p)));
1393 Arc::make_mut(&mut self.frames).truncate(p);
1394 Arc::make_mut(&mut self.position_of).retain(|_, q| *q < p);
1395 self.ingested = self.frames.iter().map(|f| f.len()).sum();
1396 self.checkpoints.retain(|c| c.position < p);
1397 match cp {
1398 Some(c) => {
1399 self.chain = c.chain;
1400 self.adopted = c.adopted;
1401 self.cam_ids = c.cam_ids;
1402 self.graph = c.graph;
1403 Arc::make_mut(&mut self.gps).truncate(c.gps_len);
1404 }
1405 None => {
1406 self.chain = None;
1407 self.adopted = None;
1408 self.cam_ids = Arc::new(BTreeMap::new());
1409 self.graph = Arc::new(MatchGraph::new());
1410 self.gps = Arc::new(self.shared.cfg.gps.clone());
1411 }
1412 }
1413 out.emit(|meta| Event::Reset { meta, position: p });
1414 }
1415
1416 fn finish_session(&mut self, out: &mut Out) {
1417 let sh = self.shared.clone();
1418 let (span, ovl) = (sh.cfg.span, sh.cfg.overlap);
1419 let npos = self.frames.len();
1421 if npos > 0 && npos >= span + ovl {
1422 for z in zones_upto(npos, span, ovl) {
1423 if !self.arrived.contains_key(&z.zone) {
1424 self.process_zone(z, out);
1425 }
1426 }
1427 }
1428 out.emit(|meta| Event::AllPositionsDone { meta });
1429 loop {
1430 let jobs = std::mem::take(&mut sh.bg().jobs);
1431 if jobs.is_empty() {
1432 break;
1433 }
1434 for j in jobs {
1435 let _ = j.join();
1436 }
1437 }
1438 let chain = match (&self.chain, sh.cfg.stable_display) {
1439 (Some(c), true) => {
1440 let mut enu = c.clone();
1444 if sh.align_enu(&mut enu, "final (stable display)", &self.gps, out) {
1445 match display_to_enu_scale(c, &enu) {
1448 Some(s) => out.log(format!("[stable] 표시→ENU 축척 {s:.4} (1.0 이면 표시 좌표계가 ENU 와 같음)")),
1449 None => out.log("[stable] 표시→ENU 축척: 공통 영상 부족".to_string()),
1450 }
1451 Some(Arc::new(enu))
1452 } else {
1453 out.warn(
1454 "[align] final (stable display): GPS 정렬 실패 — 표시 좌표계 모델을 그대로 넘김(final_frame/ 은 표시 좌표계)"
1455 .to_string(),
1456 );
1457 Some(Arc::new(c.clone()))
1458 }
1459 }
1460 (c, _) => c.clone().map(Arc::new),
1461 };
1462 out.emit(|meta| Event::AllRefinedDone { meta, chain });
1463 let pipeline_secs = sh.start.lock().unwrap_or_else(|e| e.into_inner()).map_or(0.0, |t| t.elapsed().as_secs_f64());
1464 let stage_times = sh.times.snapshot().into_iter().map(|(k, (d, n))| (k, d, n)).collect();
1465 let chain_stats = self.chain.as_ref().map(|c| ModelStats::compute(c).lines()).unwrap_or_default();
1466 out.emit(|meta| Event::Finished { meta, pipeline_secs, stage_times, chain_stats });
1467 }
1468}
1469
1470fn report_extraction(rep: &[ImageReport], position: usize, out: &mut Out) {
1471 for r in rep {
1472 match &r.status {
1473 ImageStatus::Extracted { image_id, camera_id, num_features } => {
1474 out.log(format!("[feature] {} id {image_id} cam {camera_id} 특징 {num_features}", r.name));
1475 let (image, image_id, count) = (r.name.clone(), *image_id, *num_features);
1476 out.emit(|meta| Event::FeaturesExtracted { meta, image, count, position, image_id });
1477 }
1478 ImageStatus::AlreadyExists { image_id } => out.log(format!("[feature] {} id {image_id} 이미 있음", r.name)),
1479 ImageStatus::Failed { error } => out.warn(format!("[feature] {} 실패: {error}", r.name)),
1480 }
1481 }
1482}
1483
1484fn fmt_params(p: &[f64]) -> String {
1485 format!("[{}]", p.iter().map(|x| format!("{x:.5}")).collect::<Vec<_>>().join(" "))
1486}
1487
1488fn display_to_enu_scale(display: &Reconstruction, enu: &Reconstruction) -> Option<f64> {
1490 let (mut src, mut dst) = (Vec::new(), Vec::new());
1491 for id in display.registered_images() {
1492 let name = display.image(id)?.name.clone();
1493 let Some(eid) = enu.image_by_name(&name).map(|im| im.image_id) else { continue };
1494 if let (Some(a), Some(b)) = (display.projection_center(id), enu.projection_center(eid)) {
1495 src.push(a);
1496 dst.push(b);
1497 }
1498 }
1499 if src.len() < 3 {
1500 return None;
1501 }
1502 cumulus3d_align::umeyama(&src, &dst, true).map(|t| t.scale)
1503}
1504
1505fn enu_targets(rec: &Reconstruction, gps: &[GpsRecord]) -> HashMap<ImageId, cumulus3d_core::Vec3> {
1507 let Some(o) = gps.first() else { return HashMap::new() };
1508 let frame = cumulus3d_align::EnuFrame::new(o.lat, o.lon, o.alt);
1509 let by_name: HashMap<&str, &GpsRecord> = gps.iter().map(|g| (g.name.as_str(), g)).collect();
1510 rec.registered_images()
1511 .into_iter()
1512 .filter_map(|id| {
1513 let g = by_name.get(rec.image(id)?.name.as_str())?;
1514 Some((id, frame.lla_to_enu(g.lat, g.lon, g.alt)))
1515 })
1516 .collect()
1517}
1518
1519fn sim3_onto_shown(rec: &mut Reconstruction, shown: &Reconstruction) -> Option<(cumulus3d_core::Sim3, Vec<(ImageId, Rigid3)>)> {
1522 let mut pairs = Vec::new();
1523 for id in rec.registered_images() {
1524 let Some(name) = rec.image(id).map(|im| im.name.clone()) else { continue };
1525 let Some(sid) = shown.image_by_name(&name).map(|im| im.image_id) else { continue };
1526 if let (Some(a), Some(b), Some(pose)) = (rec.projection_center(id), shown.projection_center(sid), shown.world_to_cam(sid)) {
1527 pairs.push((id, a, b, pose));
1528 }
1529 }
1530 let (src, dst): (Vec<_>, Vec<_>) = pairs.iter().map(|(_, a, b, _)| (*a, *b)).unzip();
1531 let t = cumulus3d_align::umeyama(&src, &dst, true)?;
1532 rec.transform(&t);
1533 Some((t, pairs.into_iter().map(|(id, _, _, pose)| (id, pose)).collect()))
1534}
1535
1536fn anchor_to_shown(rec: &mut Reconstruction, shown: &Reconstruction, ba: &mut BaConfig) -> (usize, f64) {
1539 let Some((t, poses)) = sim3_onto_shown(rec, shown) else { return (0, 1.0) };
1540 for (id, pose) in &poses {
1541 if rec.set_world_to_cam(*id, *pose).is_ok() {
1542 ba.constant_poses.insert(*id);
1543 }
1544 }
1545 for (cid, cam) in shown.cameras() {
1546 if let Some(c) = rec.camera_mut(*cid) {
1547 *c = cam.clone();
1548 ba.constant_cameras.insert(*cid);
1549 }
1550 }
1551 (ba.constant_poses.len(), t.scale)
1552}
1553
1554const MIN_SHARED_PAIRS: usize = 20;
1556
1557fn sim3_between(from: &Reconstruction, to: &Reconstruction, keep: impl Fn(&str) -> bool) -> Option<cumulus3d_core::Sim3> {
1560 let (mut src, mut dst) = (Vec::new(), Vec::new());
1561 for id in from.registered_images() {
1562 let Some(name) = from.image(id).map(|im| im.name.as_str()).filter(|n| keep(n)) else { continue };
1563 let Some(tid) = to.image_by_name(name).map(|im| im.image_id) else { continue };
1564 if let (Some(a), Some(b)) = (from.projection_center(id), to.projection_center(tid)) {
1565 src.push(a);
1566 dst.push(b);
1567 }
1568 }
1569 if src.len() < 3 {
1570 return None;
1571 }
1572 cumulus3d_align::umeyama(&src, &dst, true)
1573}
1574
1575pub fn intrinsics_change(a: &BTreeMap<CameraId, Camera>, b: &BTreeMap<CameraId, Camera>) -> f64 {
1578 let mut worst = 0.0f64;
1579 for (id, ca) in a {
1580 let Some(cb) = b.get(id) else { continue };
1581 if ca.model != cb.model {
1582 return f64::INFINITY;
1583 }
1584 for &s in ca.model.focal_slots() {
1585 let (fa, fb) = (ca.params[s], cb.params[s]);
1586 worst = worst.max(if fa != 0.0 {
1587 ((fb - fa) / fa).abs()
1588 } else if fb != 0.0 {
1589 f64::INFINITY
1590 } else {
1591 0.0
1592 });
1593 }
1594 }
1595 worst
1596}
1597
1598pub fn pairs_for(frames: &[Vec<FrameImage>], p: usize) -> Vec<(String, String)> {
1601 let mut pr = BTreeSet::new();
1602 let Some(cur) = frames.get(p) else { return Vec::new() };
1603 for im in cur {
1604 for d in [1usize, 2, 3, 4, 5, 8, 16] {
1605 if p >= d {
1606 if let Some(o) = frames[p - d].iter().find(|x| x.camera == im.camera) {
1607 pr.insert((o.name.clone(), im.name.clone()));
1608 }
1609 }
1610 }
1611 }
1612 for a in cur {
1613 for fq in &frames[p.saturating_sub(4)..=p] {
1614 for b in fq {
1615 if a.camera == b.camera {
1616 continue;
1617 }
1618 let (x, y) = (a.name.clone(), b.name.clone());
1619 pr.insert(if x <= y { (x, y) } else { (y, x) });
1620 }
1621 }
1622 }
1623 pr.into_iter().collect()
1624}
1625
1626pub fn zones_upto(total: usize, span: usize, ovl: usize) -> Vec<ZoneRange> {
1628 let mut v = Vec::new();
1629 let mut start = 0;
1630 let mut k = 0;
1631 while start < total {
1632 v.push(ZoneRange { zone: k, lo: start.saturating_sub(ovl), hi: (start + span + ovl).min(total) });
1633 k += 1;
1634 start += span.max(1);
1635 }
1636 v
1637}
1638
1639fn zones_ending_at(p: usize, span: usize, ovl: usize, total: Option<usize>) -> Vec<ZoneRange> {
1641 let mut v = Vec::new();
1642 let mut k = 0;
1643 loop {
1644 let start = k * span.max(1);
1645 if start > p || total.is_some_and(|n| start >= n) {
1646 break;
1647 }
1648 let hi = total.map_or(start + span + ovl, |n| (start + span + ovl).min(n));
1649 if hi == p + 1 {
1650 v.push(ZoneRange { zone: k, lo: start.saturating_sub(ovl), hi });
1651 }
1652 k += 1;
1653 }
1654 v
1655}
1656
1657#[cfg(test)]
1658mod tests {
1659 use super::*;
1660
1661 fn frames(n: usize) -> Vec<Vec<FrameImage>> {
1662 (0..n)
1663 .map(|p| {
1664 ["camF", "camR", "camL"]
1665 .iter()
1666 .map(|c| FrameImage { camera: c.to_string(), name: format!("{c}/{c}_{:04}.jpg", p * 3) })
1667 .collect()
1668 })
1669 .collect()
1670 }
1671
1672 #[test]
1673 fn zones_match_regions() {
1674 let z = |v: Vec<ZoneRange>| v.into_iter().map(|r| (r.zone, r.lo, r.hi)).collect::<Vec<_>>();
1675 assert_eq!(z(zones_upto(26, 12, 2)), vec![(0, 0, 14), (1, 10, 26), (2, 22, 26)]);
1676 let mut all = Vec::new();
1678 for p in 0..26 {
1679 all.extend(zones_ending_at(p, 12, 2, Some(26)));
1680 }
1681 all.sort_by_key(|r| r.zone);
1682 assert_eq!(z(all), z(zones_upto(26, 12, 2)));
1683 assert_eq!(z(zones_ending_at(13, 12, 2, None)), vec![(0, 0, 14)]);
1685 assert_eq!(z(zones_ending_at(25, 12, 2, None)), vec![(1, 10, 26)]);
1686 assert!(zones_ending_at(25, 12, 2, Some(40)).iter().any(|r| r.zone == 1));
1687 }
1688
1689 #[test]
1690 fn intrinsics_change_is_max_relative_focal_change() {
1691 use cumulus3d_core::CameraModelKind;
1692 let cam = |id: CameraId, model: CameraModelKind, params: Vec<f64>| Camera::new(id, model, 4000, 3000, params).unwrap();
1693 let single = |id, f: f64| cam(id, CameraModelKind::SingleFocalRadial, vec![f, 2000.0, 1500.0, 0.1]);
1694 let pinhole = |id, fx: f64, fy: f64| cam(id, CameraModelKind::Pinhole, vec![fx, fy, 2000.0, 1500.0]);
1695 let map = |v: Vec<Camera>| v.into_iter().map(|c| (c.camera_id, c)).collect::<BTreeMap<_, _>>();
1696 let a = map(vec![single(1, 1000.0), pinhole(2, 2000.0, 2000.0)]);
1697 assert_eq!(intrinsics_change(&a, &a), 0.0);
1698 let b = map(vec![single(1, 1005.0), pinhole(2, 2000.0, 2040.0), single(3, 1.0)]);
1700 assert!((intrinsics_change(&a, &b) - 0.02).abs() < 1e-12);
1701 let c = map(vec![single(1, 990.0), pinhole(2, 2000.0, 2000.0)]);
1702 assert!((intrinsics_change(&a, &c) - 0.01).abs() < 1e-12);
1703 assert_eq!(intrinsics_change(&a, &map(vec![single(9, 5.0)])), 0.0);
1705 assert!(intrinsics_change(&a, &map(vec![pinhole(1, 1000.0, 1000.0)])).is_infinite());
1706 }
1707
1708 #[test]
1709 fn pair_rule() {
1710 let f = frames(20);
1711 assert_eq!(pairs_for(&f, 0).len(), 3);
1712 let p17 = pairs_for(&f, 17);
1713 assert_eq!(p17.len(), 21 + 27);
1714 assert!(p17.contains(&("camF/camF_0003.jpg".to_string(), "camF/camF_0051.jpg".to_string())));
1715 }
1716}