1use crate::events::{Event, EventKind, ZoneRange};
32use crate::pipeline::{Pipeline, Reducer};
33use crate::post::{self, PostZones, ZoneCloud, ZoneTimes};
34use crate::util::Logger;
35use cumulus3d_core::interop::{write_model_binary, ImageOrder};
36use cumulus3d_core::io::{write_ply, PlyLayout, PointCloud};
37use cumulus3d_core::Reconstruction;
38use std::collections::{BTreeMap, HashMap};
39use std::fs::File;
40use std::io::Write;
41use std::path::{Path, PathBuf};
42use std::sync::{Arc, Mutex};
43use std::time::{Duration, Instant, SystemTime, UNIX_EPOCH};
44
45pub type Hook = Box<dyn FnMut(&Event) + Send>;
47
48pub const TIMELINE_TABLE: [(EventKind, &str); 11] = [
50 (EventKind::Started, "start NPOS={positions}"),
51 (EventKind::ModelInitialized, "init_model Registered images: {registered}"),
52 (EventKind::BaseAdopted, "adopt refined {zone} as base"),
53 (EventKind::PositionDone, "pos {position} registered {registered}/{expected}"),
54 (EventKind::ZoneArrived, "region {zone} arrived pos[{lo},{hi})"),
55 (EventKind::ZonePreview, "preview {zone} ready"),
56 (EventKind::RefineStarted, "refined {zone} ba_start"),
57 (EventKind::ZoneRefinedPose, "refined {zone} pose_ready Registered images: {registered} Mean reprojection error: {mean_reproj_px:.6}px "),
58 (EventKind::ZoneRefined, "refined {zone} ready"),
59 (EventKind::AllPositionsDone, "all positions done"),
60 (EventKind::AllRefinedDone, "all refined done"),
61];
62
63pub trait Sink: Send {
65 fn kinds(&self) -> Vec<EventKind>;
67 fn handle(&mut self, e: &Event);
69}
70
71pub fn hooks<S: Sink + 'static>(sink: S) -> Vec<(EventKind, Hook)> {
73 let kinds = sink.kinds();
74 let shared = Arc::new(Mutex::new(sink));
75 kinds
76 .into_iter()
77 .map(|k| {
78 let s = Arc::clone(&shared);
79 let h: Hook = Box::new(move |e: &Event| s.lock().unwrap_or_else(|p| p.into_inner()).handle(e));
80 (k, h)
81 })
82 .collect()
83}
84
85pub fn any_hook<S: Sink + 'static>(mut sink: S) -> Hook {
87 let kinds = sink.kinds();
88 Box::new(move |e: &Event| {
89 if kinds.contains(&e.kind()) {
90 sink.handle(e)
91 }
92 })
93}
94
95fn epoch_secs(t: SystemTime) -> f64 {
96 t.duration_since(UNIX_EPOCH).unwrap_or_default().as_secs_f64()
97}
98
99fn zone_file(kind: &str, r: &ZoneRange) -> String {
100 format!("{kind}_{:02}_pos{}-{}.ply", r.zone, r.lo, r.hi)
101}
102
103#[derive(Clone)]
107pub struct RunLog {
108 inner: Arc<RunLogInner>,
109}
110
111struct RunLogInner {
112 logger: Logger,
113 extra_times: Mutex<Vec<(String, Duration)>>,
115}
116
117impl RunLog {
118 pub fn open(path: Option<&Path>, echo: bool) -> std::io::Result<Self> {
120 Ok(Self { inner: Arc::new(RunLogInner { logger: Logger::new(path, echo)?, extra_times: Mutex::new(Vec::new()) }) })
121 }
122 pub fn logger(&self) -> &Logger {
124 &self.inner.logger
125 }
126 pub fn line(&self, s: &str) {
128 self.inner.logger.line(s)
129 }
130 pub fn stamped_at(&self, at: SystemTime, s: &str) {
132 self.inner.logger.stamped_at(at, s)
133 }
134 pub fn add_time(&self, stage: &str, d: Duration) {
136 if let Ok(mut v) = self.inner.extra_times.lock() {
137 v.push((stage.to_string(), d));
138 }
139 }
140 fn take_times(&self) -> Vec<(String, Duration)> {
141 self.inner.extra_times.lock().map(|mut v| std::mem::take(&mut *v)).unwrap_or_default()
142 }
143}
144
145pub struct TimelineSink {
149 file: File,
150}
151
152impl TimelineSink {
153 pub fn create(path: &Path) -> std::io::Result<Self> {
155 Ok(Self { file: File::create(path)? })
156 }
157
158 pub fn line(e: &Event) -> Option<String> {
160 let s = e.timeline_text()?;
161 let t = e.meta().at.duration_since(UNIX_EPOCH).unwrap_or_default();
162 Some(format!("{}.{:09} {s}", t.as_secs(), t.subsec_nanos()))
163 }
164}
165
166impl Sink for TimelineSink {
167 fn kinds(&self) -> Vec<EventKind> {
168 TIMELINE_TABLE.iter().map(|(k, _)| *k).collect()
169 }
170 fn handle(&mut self, e: &Event) {
171 if let Some(l) = Self::line(e) {
172 let _ = writeln!(self.file, "{l}");
173 let _ = self.file.flush();
174 }
175 }
176}
177
178pub struct RunLogSink {
184 log: RunLog,
185 done: Option<PathBuf>,
186}
187
188impl RunLogSink {
189 pub fn new(log: RunLog, done: Option<PathBuf>) -> Self {
191 Self { log, done }
192 }
193}
194
195impl Sink for RunLogSink {
196 fn kinds(&self) -> Vec<EventKind> {
197 let mut v: Vec<EventKind> = TIMELINE_TABLE.iter().map(|(k, _)| *k).collect();
198 v.extend([EventKind::Log, EventKind::Finished]);
199 v
200 }
201 fn handle(&mut self, e: &Event) {
202 if let Some(s) = e.timeline_text() {
203 self.log.stamped_at(e.meta().at, &s);
204 return;
205 }
206 match e {
207 Event::Log { meta, line, stamped } => {
208 if *stamped {
209 self.log.stamped_at(meta.at, line)
210 } else {
211 self.log.line(line)
212 }
213 }
214 Event::Finished { pipeline_secs, stage_times, chain_stats, .. } => {
215 let mut m: BTreeMap<String, (Duration, usize)> = BTreeMap::new();
216 for (s, d, n) in stage_times {
217 let x = m.entry(s.clone()).or_default();
218 x.0 += *d;
219 x.1 += n;
220 }
221 for (s, d) in self.log.take_times() {
222 let x = m.entry(s).or_default();
223 x.0 += d;
224 x.1 += 1;
225 }
226 self.log.line(&format!("== 단계별 시간 (파이프라인 {pipeline_secs:.1}s, 배경 스레드와 겹친 누적)"));
227 for (stage, (d, n)) in m {
228 self.log.line(&format!("{stage:<14} {:>9.2}s ({n}회)", d.as_secs_f64()));
229 }
230 if !chain_stats.is_empty() {
231 self.log.line("== 최종 체인 모델");
232 for l in chain_stats {
233 self.log.line(l);
234 }
235 }
236 if let Some(p) = &self.done {
237 if let Err(err) = std::fs::write(p, b"") {
238 self.log.line(&format!("{}: {err}", p.display()));
239 }
240 }
241 }
242 _ => {}
243 }
244 }
245}
246
247pub struct ZonePlySink {
252 full: PathBuf,
253 log: Option<RunLog>,
254}
255
256impl ZonePlySink {
257 pub fn new(full: PathBuf, log: Option<RunLog>) -> Self {
259 Self { full, log }
260 }
261
262 fn write(&self, kind: &str, r: &ZoneRange, c: &PointCloud) {
263 let path = self.full.join(kind).join(zone_file(kind, r));
264 let res = path
265 .parent()
266 .map_or(Ok(()), std::fs::create_dir_all)
267 .map_err(|e| e.to_string())
268 .and_then(|_| write_ply(&path, c, PlyLayout::XyzNormalRgb).map_err(|e| e.to_string()));
269 if let (Err(e), Some(l)) = (res, &self.log) {
270 l.line(&format!("[dense] {kind} {} PLY 쓰기 실패: {e}", r.zone));
271 }
272 }
273}
274
275impl Sink for ZonePlySink {
276 fn kinds(&self) -> Vec<EventKind> {
277 vec![EventKind::ZonePreview, EventKind::ZoneRefined]
278 }
279 fn handle(&mut self, e: &Event) {
280 match e {
281 Event::ZonePreview { range, cloud, dense: true, .. } => self.write("preview", range, cloud),
282 Event::ZoneRefined { range, cloud, dense: true, .. } => self.write("refined", range, cloud),
283 _ => {}
284 }
285 }
286}
287
288pub struct ModelSink {
294 dir: PathBuf,
295 log: Option<RunLog>,
296}
297
298impl ModelSink {
299 pub fn new(dir: PathBuf, log: Option<RunLog>) -> Self {
301 Self { dir, log }
302 }
303
304 fn save(&self, rec: &Reconstruction, name: &str) {
305 let d = self.dir.join(name);
306 let _ = std::fs::create_dir_all(&d);
307 if let (Err(e), Some(l)) = (write_model_binary(rec, &d, ImageOrder::Registration), &self.log) {
308 l.line(&format!("모델 저장 실패 {}: {e}", d.display()));
309 }
310 }
311}
312
313impl Sink for ModelSink {
314 fn kinds(&self) -> Vec<EventKind> {
315 vec![EventKind::ZoneArrived, EventKind::ZonePreview, EventKind::ZoneAdjusted, EventKind::ZoneRefinedPose, EventKind::AllRefinedDone]
316 }
317 fn handle(&mut self, e: &Event) {
318 match e {
319 Event::ZoneArrived { range, model, .. } => self.save(model, &format!("snap_{}", range.zone)),
320 Event::ZonePreview { range, model, .. } => self.save(model, &format!("preview_{}", range.zone)),
321 Event::ZoneAdjusted { zone, model, .. } => self.save(model, &format!("ba_{zone}")),
322 Event::ZoneRefinedPose { range, model, .. } => self.save(model, &format!("refined_{}", range.zone)),
323 Event::AllRefinedDone { chain: Some(c), .. } => self.save(c, "chain"),
324 _ => {}
325 }
326 }
327}
328
329pub struct SnapshotSink {
338 out: PathBuf,
339 log: RunLog,
340 t0: Option<SystemTime>,
341 ranges: BTreeMap<usize, ZoneRange>,
342 times: BTreeMap<usize, Stamps>,
343 preview: BTreeMap<usize, Arc<PointCloud>>,
344 refined: BTreeMap<usize, Arc<PointCloud>>,
345 preview_models: BTreeMap<usize, Arc<Reconstruction>>,
346 refined_models: BTreeMap<usize, Arc<Reconstruction>>,
347 positions: HashMap<String, usize>,
348}
349
350#[derive(Clone, Copy, Default)]
352struct Stamps {
353 arrived: Option<SystemTime>,
354 preview: Option<SystemTime>,
355 pose: Option<SystemTime>,
356 refined: Option<SystemTime>,
357}
358
359impl SnapshotSink {
360 pub fn new(out: PathBuf, log: RunLog) -> Self {
362 Self {
363 out,
364 log,
365 t0: None,
366 ranges: BTreeMap::new(),
367 times: BTreeMap::new(),
368 preview: BTreeMap::new(),
369 refined: BTreeMap::new(),
370 preview_models: BTreeMap::new(),
371 refined_models: BTreeMap::new(),
372 positions: HashMap::new(),
373 }
374 }
375
376 fn drop_zone(&mut self, k: usize) {
377 self.ranges.remove(&k);
378 self.times.remove(&k);
379 self.preview.remove(&k);
380 self.refined.remove(&k);
381 self.preview_models.remove(&k);
382 self.refined_models.remove(&k);
383 }
384
385 pub fn finish(&mut self) {
387 let t0 = epoch_secs(self.t0.unwrap_or(UNIX_EPOCH));
388 let rel = |t: Option<SystemTime>| t.map(|t| epoch_secs(t) - t0);
389 let times: BTreeMap<usize, ZoneTimes> = self
390 .times
391 .iter()
392 .map(|(k, s)| {
393 (*k, ZoneTimes { arrived: rel(s.arrived), preview_ready: rel(s.preview), refined_pose: rel(s.pose), refined_ready: rel(s.refined) })
394 })
395 .collect();
396 let windows = self
397 .ranges
398 .iter()
399 .map(|(k, r)| {
400 let hi = k.checked_sub(1).and_then(|j| self.ranges.get(&j)).map_or(r.hi, |p| p.hi);
401 (*k, (r.lo, hi))
402 })
403 .collect();
404 let zc = |kind: &str, m: &BTreeMap<usize, Arc<PointCloud>>| -> BTreeMap<usize, (String, Arc<PointCloud>)> {
405 m.iter()
406 .filter_map(|(k, c)| self.ranges.get(k).map(|r| (*k, (zone_file(kind, r), Arc::clone(c)))))
407 .collect()
408 };
409 let (pv, rf) = (zc("preview", &self.preview), zc("refined", &self.refined));
410 let positions = &self.positions;
411 let pos = move |n: &str| positions.get(n).copied();
412 let z = PostZones {
413 out: &self.out,
414 preview: pv.iter().map(|(k, (f, c))| (*k, ZoneCloud { file: f.clone(), cloud: c })).collect(),
415 refined: rf.iter().map(|(k, (f, c))| (*k, ZoneCloud { file: f.clone(), cloud: c })).collect(),
416 preview_models: self.preview_models.iter().map(|(k, m)| (*k, &**m)).collect(),
417 refined_models: self.refined_models.iter().map(|(k, m)| (*k, &**m)).collect(),
418 times,
419 windows,
420 position: &pos,
421 };
422 let t = Instant::now();
423 let r = post::run_zones(&z, self.log.logger());
424 self.log.add_time("post", t.elapsed());
425 if let Err(e) = r {
426 self.log.line(&format!("[post] 실패: {e}"));
427 }
428 let _ = std::fs::copy(self.out.join("timeline.txt"), self.out.join("snapshots").join("timeline.txt"));
429 }
430}
431
432impl Sink for SnapshotSink {
433 fn kinds(&self) -> Vec<EventKind> {
434 vec![
435 EventKind::Started,
436 EventKind::FrameIngested,
437 EventKind::FeaturesExtracted,
438 EventKind::ZoneArrived,
439 EventKind::ZonePreview,
440 EventKind::ZoneRefinedPose,
441 EventKind::ZoneRefined,
442 EventKind::Reset,
443 EventKind::ZoneInvalidated,
444 EventKind::AllRefinedDone,
445 ]
446 }
447
448 fn handle(&mut self, e: &Event) {
449 let at = e.meta().at;
450 if matches!(e, Event::Started { .. }) || self.t0.is_none() {
451 self.t0 = Some(at);
452 }
453 match e {
454 Event::FrameIngested { position, images, .. } => {
455 for n in images {
456 self.positions.insert(n.clone(), *position);
457 }
458 }
459 Event::FeaturesExtracted { image, position, .. } => {
460 self.positions.insert(image.clone(), *position);
461 }
462 Event::ZoneArrived { range, .. } => {
463 self.ranges.insert(range.zone, *range);
464 self.times.entry(range.zone).or_default().arrived.get_or_insert(at);
465 }
466 Event::ZonePreview { range, cloud, model, dense, .. } => {
467 self.ranges.entry(range.zone).or_insert(*range);
468 self.times.entry(range.zone).or_default().preview.get_or_insert(at);
469 self.preview_models.insert(range.zone, Arc::clone(model));
470 if *dense {
471 self.preview.insert(range.zone, Arc::clone(cloud));
472 }
473 }
474 Event::ZoneRefinedPose { range, model, .. } => {
475 self.ranges.entry(range.zone).or_insert(*range);
476 self.times.entry(range.zone).or_default().pose.get_or_insert(at);
477 self.refined_models.insert(range.zone, Arc::clone(model));
478 }
479 Event::ZoneRefined { range, cloud, dense, .. } => {
480 self.ranges.entry(range.zone).or_insert(*range);
481 self.times.entry(range.zone).or_default().refined.get_or_insert(at);
482 if *dense {
483 self.refined.insert(range.zone, Arc::clone(cloud));
484 }
485 }
486 Event::Reset { position, .. } => {
487 let ks: Vec<usize> = self.ranges.values().filter(|r| r.hi > *position).map(|r| r.zone).collect();
488 for k in ks {
489 self.drop_zone(k);
490 }
491 }
492 Event::ZoneInvalidated { zone, .. } => self.drop_zone(*zone),
493 Event::AllRefinedDone { .. } => self.finish(),
494 _ => {}
495 }
496 }
497}
498
499#[derive(Clone, Debug, Default)]
503pub struct SinkOptions {
504 pub echo: bool,
506 pub save_models: bool,
508}
509
510pub struct DefaultSinks {
512 sinks: Vec<Box<dyn Sink>>,
513 log: RunLog,
514}
515
516impl DefaultSinks {
517 pub fn new(out: &Path, opts: &SinkOptions) -> std::io::Result<Self> {
520 for d in ["full/preview", "full/refined", "work"] {
521 std::fs::create_dir_all(out.join(d))?;
522 }
523 let log = RunLog::open(Some(&out.join("run.log")), opts.echo)?;
524 let mut sinks: Vec<Box<dyn Sink>> = vec![
525 Box::new(TimelineSink::create(&out.join("timeline.txt"))?),
526 Box::new(RunLogSink::new(log.clone(), Some(out.join("DONE")))),
527 Box::new(ZonePlySink::new(out.join("full"), Some(log.clone()))),
528 ];
529 if opts.save_models {
530 sinks.push(Box::new(ModelSink::new(out.join("work").join("models"), Some(log.clone()))));
531 }
532 sinks.push(Box::new(SnapshotSink::new(out.to_path_buf(), log.clone())));
533 Ok(Self { sinks, log })
534 }
535
536 pub fn run_log(&self) -> RunLog {
538 self.log.clone()
539 }
540
541 pub fn into_hook(self) -> Hook {
543 any_hook(self)
544 }
545}
546
547impl Sink for DefaultSinks {
548 fn kinds(&self) -> Vec<EventKind> {
549 let mut v: Vec<EventKind> = Vec::new();
550 for s in &self.sinks {
551 for k in s.kinds() {
552 if !v.contains(&k) {
553 v.push(k);
554 }
555 }
556 }
557 v
558 }
559 fn handle(&mut self, e: &Event) {
560 let k = e.kind();
561 for s in &mut self.sinks {
562 if s.kinds().contains(&k) {
563 s.handle(e);
564 }
565 }
566 }
567}
568
569pub fn default_sinks(out: &Path, opts: &SinkOptions) -> std::io::Result<Vec<(EventKind, Hook)>> {
571 Ok(hooks(DefaultSinks::new(out, opts)?))
572}
573
574pub fn attach<R: Reducer>(p: Pipeline<R>, out: &Path, opts: &SinkOptions) -> std::io::Result<Pipeline<R>> {
576 Ok(p.on_any(DefaultSinks::new(out, opts)?.into_hook()))
577}
578
579#[cfg(test)]
580mod tests {
581 use super::*;
582 use crate::events::Meta;
583
584 fn meta(s: u64, n: u32) -> Meta {
585 Meta { version: 0, at: UNIX_EPOCH + Duration::new(s, n) }
586 }
587 fn r(zone: usize, lo: usize, hi: usize) -> ZoneRange {
588 ZoneRange { zone, lo, hi }
589 }
590
591 #[test]
592 fn timeline_table() {
593 let m = || meta(1_700_000_000, 5);
594 let model = Arc::new(Reconstruction::new());
595 let cloud = Arc::new(PointCloud::default());
596 let cases: Vec<(Event, &str)> = vec![
597 (Event::Started { meta: m(), positions: Some(26) }, "start NPOS=26"),
598 (Event::ModelInitialized { meta: m(), registered: 42, position: 13, model: model.clone() }, "init_model Registered images: 42"),
599 (Event::BaseAdopted { meta: m(), zone: 0 }, "adopt refined 0 as base"),
600 (Event::PositionDone { meta: m(), position: 14, registered: 44, expected: 45 }, "pos 14 registered 44/45"),
601 (Event::ZoneArrived { meta: m(), range: r(1, 10, 26), model: model.clone() }, "region 1 arrived pos[10,26)"),
602 (
603 Event::ZonePreview { meta: m(), range: r(1, 10, 26), cloud: cloud.clone(), model: model.clone(), dense: true, frame: "gps".into() },
604 "preview 1 ready",
605 ),
606 (Event::RefineStarted { meta: m(), zone: 1 }, "refined 1 ba_start"),
607 (
608 Event::ZoneRefinedPose { meta: m(), range: r(1, 10, 26), registered: 48, mean_reproj_px: 0.25095, model: model.clone() },
609 "refined 1 pose_ready Registered images: 48 Mean reprojection error: 0.250950px ",
610 ),
611 (Event::ZoneRefined { meta: m(), range: r(1, 10, 26), cloud, model, dense: true }, "refined 1 ready"),
612 (Event::AllPositionsDone { meta: m() }, "all positions done"),
613 (Event::AllRefinedDone { meta: m(), chain: None }, "all refined done"),
614 ];
615 assert_eq!(cases.len(), TIMELINE_TABLE.len());
616 for ((e, want), (k, _)) in cases.iter().zip(TIMELINE_TABLE.iter()) {
617 assert_eq!(e.kind(), *k);
618 assert_eq!(TimelineSink::line(e).unwrap(), format!("1700000000.000000005 {want}"));
619 }
620 assert!(TimelineSink::line(&Event::Warning { meta: m(), message: "x".into() }).is_none());
621 }
622}