use crate::events::{Event, EventKind, ZoneRange};
use crate::pipeline::{Pipeline, Reducer};
use crate::post::{self, PostZones, ZoneCloud, ZoneTimes};
use crate::util::Logger;
use cumulus3d_core::interop::{write_model_binary, ImageOrder};
use cumulus3d_core::io::{write_ply, PlyLayout, PointCloud};
use cumulus3d_core::Reconstruction;
use std::collections::{BTreeMap, HashMap};
use std::fs::File;
use std::io::Write;
use std::path::{Path, PathBuf};
use std::sync::{Arc, Mutex};
use std::time::{Duration, Instant, SystemTime, UNIX_EPOCH};
pub type Hook = Box<dyn FnMut(&Event) + Send>;
pub const TIMELINE_TABLE: [(EventKind, &str); 11] = [
(EventKind::Started, "start NPOS={positions}"),
(EventKind::ModelInitialized, "init_model Registered images: {registered}"),
(EventKind::BaseAdopted, "adopt refined {zone} as base"),
(EventKind::PositionDone, "pos {position} registered {registered}/{expected}"),
(EventKind::ZoneArrived, "region {zone} arrived pos[{lo},{hi})"),
(EventKind::ZonePreview, "preview {zone} ready"),
(EventKind::RefineStarted, "refined {zone} ba_start"),
(EventKind::ZoneRefinedPose, "refined {zone} pose_ready Registered images: {registered} Mean reprojection error: {mean_reproj_px:.6}px "),
(EventKind::ZoneRefined, "refined {zone} ready"),
(EventKind::AllPositionsDone, "all positions done"),
(EventKind::AllRefinedDone, "all refined done"),
];
pub trait Sink: Send {
fn kinds(&self) -> Vec<EventKind>;
fn handle(&mut self, e: &Event);
}
pub fn hooks<S: Sink + 'static>(sink: S) -> Vec<(EventKind, Hook)> {
let kinds = sink.kinds();
let shared = Arc::new(Mutex::new(sink));
kinds
.into_iter()
.map(|k| {
let s = Arc::clone(&shared);
let h: Hook = Box::new(move |e: &Event| s.lock().unwrap_or_else(|p| p.into_inner()).handle(e));
(k, h)
})
.collect()
}
pub fn any_hook<S: Sink + 'static>(mut sink: S) -> Hook {
let kinds = sink.kinds();
Box::new(move |e: &Event| {
if kinds.contains(&e.kind()) {
sink.handle(e)
}
})
}
fn epoch_secs(t: SystemTime) -> f64 {
t.duration_since(UNIX_EPOCH).unwrap_or_default().as_secs_f64()
}
fn zone_file(kind: &str, r: &ZoneRange) -> String {
format!("{kind}_{:02}_pos{}-{}.ply", r.zone, r.lo, r.hi)
}
#[derive(Clone)]
pub struct RunLog {
inner: Arc<RunLogInner>,
}
struct RunLogInner {
logger: Logger,
extra_times: Mutex<Vec<(String, Duration)>>,
}
impl RunLog {
pub fn open(path: Option<&Path>, echo: bool) -> std::io::Result<Self> {
Ok(Self { inner: Arc::new(RunLogInner { logger: Logger::new(path, echo)?, extra_times: Mutex::new(Vec::new()) }) })
}
pub fn logger(&self) -> &Logger {
&self.inner.logger
}
pub fn line(&self, s: &str) {
self.inner.logger.line(s)
}
pub fn stamped_at(&self, at: SystemTime, s: &str) {
self.inner.logger.stamped_at(at, s)
}
pub fn add_time(&self, stage: &str, d: Duration) {
if let Ok(mut v) = self.inner.extra_times.lock() {
v.push((stage.to_string(), d));
}
}
fn take_times(&self) -> Vec<(String, Duration)> {
self.inner.extra_times.lock().map(|mut v| std::mem::take(&mut *v)).unwrap_or_default()
}
}
pub struct TimelineSink {
file: File,
}
impl TimelineSink {
pub fn create(path: &Path) -> std::io::Result<Self> {
Ok(Self { file: File::create(path)? })
}
pub fn line(e: &Event) -> Option<String> {
let s = e.timeline_text()?;
let t = e.meta().at.duration_since(UNIX_EPOCH).unwrap_or_default();
Some(format!("{}.{:09} {s}", t.as_secs(), t.subsec_nanos()))
}
}
impl Sink for TimelineSink {
fn kinds(&self) -> Vec<EventKind> {
TIMELINE_TABLE.iter().map(|(k, _)| *k).collect()
}
fn handle(&mut self, e: &Event) {
if let Some(l) = Self::line(e) {
let _ = writeln!(self.file, "{l}");
let _ = self.file.flush();
}
}
}
pub struct RunLogSink {
log: RunLog,
done: Option<PathBuf>,
}
impl RunLogSink {
pub fn new(log: RunLog, done: Option<PathBuf>) -> Self {
Self { log, done }
}
}
impl Sink for RunLogSink {
fn kinds(&self) -> Vec<EventKind> {
let mut v: Vec<EventKind> = TIMELINE_TABLE.iter().map(|(k, _)| *k).collect();
v.extend([EventKind::Log, EventKind::Finished]);
v
}
fn handle(&mut self, e: &Event) {
if let Some(s) = e.timeline_text() {
self.log.stamped_at(e.meta().at, &s);
return;
}
match e {
Event::Log { meta, line, stamped } => {
if *stamped {
self.log.stamped_at(meta.at, line)
} else {
self.log.line(line)
}
}
Event::Finished { pipeline_secs, stage_times, chain_stats, .. } => {
let mut m: BTreeMap<String, (Duration, usize)> = BTreeMap::new();
for (s, d, n) in stage_times {
let x = m.entry(s.clone()).or_default();
x.0 += *d;
x.1 += n;
}
for (s, d) in self.log.take_times() {
let x = m.entry(s).or_default();
x.0 += d;
x.1 += 1;
}
self.log.line(&format!("== 단계별 시간 (파이프라인 {pipeline_secs:.1}s, 배경 스레드와 겹친 누적)"));
for (stage, (d, n)) in m {
self.log.line(&format!("{stage:<14} {:>9.2}s ({n}회)", d.as_secs_f64()));
}
if !chain_stats.is_empty() {
self.log.line("== 최종 체인 모델");
for l in chain_stats {
self.log.line(l);
}
}
if let Some(p) = &self.done {
if let Err(err) = std::fs::write(p, b"") {
self.log.line(&format!("{}: {err}", p.display()));
}
}
}
_ => {}
}
}
}
pub struct ZonePlySink {
full: PathBuf,
log: Option<RunLog>,
}
impl ZonePlySink {
pub fn new(full: PathBuf, log: Option<RunLog>) -> Self {
Self { full, log }
}
fn write(&self, kind: &str, r: &ZoneRange, c: &PointCloud) {
let path = self.full.join(kind).join(zone_file(kind, r));
let res = path
.parent()
.map_or(Ok(()), std::fs::create_dir_all)
.map_err(|e| e.to_string())
.and_then(|_| write_ply(&path, c, PlyLayout::XyzNormalRgb).map_err(|e| e.to_string()));
if let (Err(e), Some(l)) = (res, &self.log) {
l.line(&format!("[dense] {kind} {} PLY 쓰기 실패: {e}", r.zone));
}
}
}
impl Sink for ZonePlySink {
fn kinds(&self) -> Vec<EventKind> {
vec![EventKind::ZonePreview, EventKind::ZoneRefined]
}
fn handle(&mut self, e: &Event) {
match e {
Event::ZonePreview { range, cloud, dense: true, .. } => self.write("preview", range, cloud),
Event::ZoneRefined { range, cloud, dense: true, .. } => self.write("refined", range, cloud),
_ => {}
}
}
}
pub struct ModelSink {
dir: PathBuf,
log: Option<RunLog>,
}
impl ModelSink {
pub fn new(dir: PathBuf, log: Option<RunLog>) -> Self {
Self { dir, log }
}
fn save(&self, rec: &Reconstruction, name: &str) {
let d = self.dir.join(name);
let _ = std::fs::create_dir_all(&d);
if let (Err(e), Some(l)) = (write_model_binary(rec, &d, ImageOrder::Registration), &self.log) {
l.line(&format!("모델 저장 실패 {}: {e}", d.display()));
}
}
}
impl Sink for ModelSink {
fn kinds(&self) -> Vec<EventKind> {
vec![EventKind::ZoneArrived, EventKind::ZonePreview, EventKind::ZoneAdjusted, EventKind::ZoneRefinedPose, EventKind::AllRefinedDone]
}
fn handle(&mut self, e: &Event) {
match e {
Event::ZoneArrived { range, model, .. } => self.save(model, &format!("snap_{}", range.zone)),
Event::ZonePreview { range, model, .. } => self.save(model, &format!("preview_{}", range.zone)),
Event::ZoneAdjusted { zone, model, .. } => self.save(model, &format!("ba_{zone}")),
Event::ZoneRefinedPose { range, model, .. } => self.save(model, &format!("refined_{}", range.zone)),
Event::AllRefinedDone { chain: Some(c), .. } => self.save(c, "chain"),
_ => {}
}
}
}
pub struct SnapshotSink {
out: PathBuf,
log: RunLog,
t0: Option<SystemTime>,
ranges: BTreeMap<usize, ZoneRange>,
times: BTreeMap<usize, Stamps>,
preview: BTreeMap<usize, Arc<PointCloud>>,
refined: BTreeMap<usize, Arc<PointCloud>>,
preview_models: BTreeMap<usize, Arc<Reconstruction>>,
refined_models: BTreeMap<usize, Arc<Reconstruction>>,
positions: HashMap<String, usize>,
}
#[derive(Clone, Copy, Default)]
struct Stamps {
arrived: Option<SystemTime>,
preview: Option<SystemTime>,
pose: Option<SystemTime>,
refined: Option<SystemTime>,
}
impl SnapshotSink {
pub fn new(out: PathBuf, log: RunLog) -> Self {
Self {
out,
log,
t0: None,
ranges: BTreeMap::new(),
times: BTreeMap::new(),
preview: BTreeMap::new(),
refined: BTreeMap::new(),
preview_models: BTreeMap::new(),
refined_models: BTreeMap::new(),
positions: HashMap::new(),
}
}
fn drop_zone(&mut self, k: usize) {
self.ranges.remove(&k);
self.times.remove(&k);
self.preview.remove(&k);
self.refined.remove(&k);
self.preview_models.remove(&k);
self.refined_models.remove(&k);
}
pub fn finish(&mut self) {
let t0 = epoch_secs(self.t0.unwrap_or(UNIX_EPOCH));
let rel = |t: Option<SystemTime>| t.map(|t| epoch_secs(t) - t0);
let times: BTreeMap<usize, ZoneTimes> = self
.times
.iter()
.map(|(k, s)| {
(*k, ZoneTimes { arrived: rel(s.arrived), preview_ready: rel(s.preview), refined_pose: rel(s.pose), refined_ready: rel(s.refined) })
})
.collect();
let windows = self
.ranges
.iter()
.map(|(k, r)| {
let hi = k.checked_sub(1).and_then(|j| self.ranges.get(&j)).map_or(r.hi, |p| p.hi);
(*k, (r.lo, hi))
})
.collect();
let zc = |kind: &str, m: &BTreeMap<usize, Arc<PointCloud>>| -> BTreeMap<usize, (String, Arc<PointCloud>)> {
m.iter()
.filter_map(|(k, c)| self.ranges.get(k).map(|r| (*k, (zone_file(kind, r), Arc::clone(c)))))
.collect()
};
let (pv, rf) = (zc("preview", &self.preview), zc("refined", &self.refined));
let positions = &self.positions;
let pos = move |n: &str| positions.get(n).copied();
let z = PostZones {
out: &self.out,
preview: pv.iter().map(|(k, (f, c))| (*k, ZoneCloud { file: f.clone(), cloud: c })).collect(),
refined: rf.iter().map(|(k, (f, c))| (*k, ZoneCloud { file: f.clone(), cloud: c })).collect(),
preview_models: self.preview_models.iter().map(|(k, m)| (*k, &**m)).collect(),
refined_models: self.refined_models.iter().map(|(k, m)| (*k, &**m)).collect(),
times,
windows,
position: &pos,
};
let t = Instant::now();
let r = post::run_zones(&z, self.log.logger());
self.log.add_time("post", t.elapsed());
if let Err(e) = r {
self.log.line(&format!("[post] 실패: {e}"));
}
let _ = std::fs::copy(self.out.join("timeline.txt"), self.out.join("snapshots").join("timeline.txt"));
}
}
impl Sink for SnapshotSink {
fn kinds(&self) -> Vec<EventKind> {
vec![
EventKind::Started,
EventKind::FrameIngested,
EventKind::FeaturesExtracted,
EventKind::ZoneArrived,
EventKind::ZonePreview,
EventKind::ZoneRefinedPose,
EventKind::ZoneRefined,
EventKind::Reset,
EventKind::ZoneInvalidated,
EventKind::AllRefinedDone,
]
}
fn handle(&mut self, e: &Event) {
let at = e.meta().at;
if matches!(e, Event::Started { .. }) || self.t0.is_none() {
self.t0 = Some(at);
}
match e {
Event::FrameIngested { position, images, .. } => {
for n in images {
self.positions.insert(n.clone(), *position);
}
}
Event::FeaturesExtracted { image, position, .. } => {
self.positions.insert(image.clone(), *position);
}
Event::ZoneArrived { range, .. } => {
self.ranges.insert(range.zone, *range);
self.times.entry(range.zone).or_default().arrived.get_or_insert(at);
}
Event::ZonePreview { range, cloud, model, dense, .. } => {
self.ranges.entry(range.zone).or_insert(*range);
self.times.entry(range.zone).or_default().preview.get_or_insert(at);
self.preview_models.insert(range.zone, Arc::clone(model));
if *dense {
self.preview.insert(range.zone, Arc::clone(cloud));
}
}
Event::ZoneRefinedPose { range, model, .. } => {
self.ranges.entry(range.zone).or_insert(*range);
self.times.entry(range.zone).or_default().pose.get_or_insert(at);
self.refined_models.insert(range.zone, Arc::clone(model));
}
Event::ZoneRefined { range, cloud, dense, .. } => {
self.ranges.entry(range.zone).or_insert(*range);
self.times.entry(range.zone).or_default().refined.get_or_insert(at);
if *dense {
self.refined.insert(range.zone, Arc::clone(cloud));
}
}
Event::Reset { position, .. } => {
let ks: Vec<usize> = self.ranges.values().filter(|r| r.hi > *position).map(|r| r.zone).collect();
for k in ks {
self.drop_zone(k);
}
}
Event::ZoneInvalidated { zone, .. } => self.drop_zone(*zone),
Event::AllRefinedDone { .. } => self.finish(),
_ => {}
}
}
}
#[derive(Clone, Debug, Default)]
pub struct SinkOptions {
pub echo: bool,
pub save_models: bool,
}
pub struct DefaultSinks {
sinks: Vec<Box<dyn Sink>>,
log: RunLog,
}
impl DefaultSinks {
pub fn new(out: &Path, opts: &SinkOptions) -> std::io::Result<Self> {
for d in ["full/preview", "full/refined", "work"] {
std::fs::create_dir_all(out.join(d))?;
}
let log = RunLog::open(Some(&out.join("run.log")), opts.echo)?;
let mut sinks: Vec<Box<dyn Sink>> = vec![
Box::new(TimelineSink::create(&out.join("timeline.txt"))?),
Box::new(RunLogSink::new(log.clone(), Some(out.join("DONE")))),
Box::new(ZonePlySink::new(out.join("full"), Some(log.clone()))),
];
if opts.save_models {
sinks.push(Box::new(ModelSink::new(out.join("work").join("models"), Some(log.clone()))));
}
sinks.push(Box::new(SnapshotSink::new(out.to_path_buf(), log.clone())));
Ok(Self { sinks, log })
}
pub fn run_log(&self) -> RunLog {
self.log.clone()
}
pub fn into_hook(self) -> Hook {
any_hook(self)
}
}
impl Sink for DefaultSinks {
fn kinds(&self) -> Vec<EventKind> {
let mut v: Vec<EventKind> = Vec::new();
for s in &self.sinks {
for k in s.kinds() {
if !v.contains(&k) {
v.push(k);
}
}
}
v
}
fn handle(&mut self, e: &Event) {
let k = e.kind();
for s in &mut self.sinks {
if s.kinds().contains(&k) {
s.handle(e);
}
}
}
}
pub fn default_sinks(out: &Path, opts: &SinkOptions) -> std::io::Result<Vec<(EventKind, Hook)>> {
Ok(hooks(DefaultSinks::new(out, opts)?))
}
pub fn attach<R: Reducer>(p: Pipeline<R>, out: &Path, opts: &SinkOptions) -> std::io::Result<Pipeline<R>> {
Ok(p.on_any(DefaultSinks::new(out, opts)?.into_hook()))
}
#[cfg(test)]
mod tests {
use super::*;
use crate::events::Meta;
fn meta(s: u64, n: u32) -> Meta {
Meta { version: 0, at: UNIX_EPOCH + Duration::new(s, n) }
}
fn r(zone: usize, lo: usize, hi: usize) -> ZoneRange {
ZoneRange { zone, lo, hi }
}
#[test]
fn timeline_table() {
let m = || meta(1_700_000_000, 5);
let model = Arc::new(Reconstruction::new());
let cloud = Arc::new(PointCloud::default());
let cases: Vec<(Event, &str)> = vec![
(Event::Started { meta: m(), positions: Some(26) }, "start NPOS=26"),
(Event::ModelInitialized { meta: m(), registered: 42, position: 13, model: model.clone() }, "init_model Registered images: 42"),
(Event::BaseAdopted { meta: m(), zone: 0 }, "adopt refined 0 as base"),
(Event::PositionDone { meta: m(), position: 14, registered: 44, expected: 45 }, "pos 14 registered 44/45"),
(Event::ZoneArrived { meta: m(), range: r(1, 10, 26), model: model.clone() }, "region 1 arrived pos[10,26)"),
(
Event::ZonePreview { meta: m(), range: r(1, 10, 26), cloud: cloud.clone(), model: model.clone(), dense: true, frame: "gps".into() },
"preview 1 ready",
),
(Event::RefineStarted { meta: m(), zone: 1 }, "refined 1 ba_start"),
(
Event::ZoneRefinedPose { meta: m(), range: r(1, 10, 26), registered: 48, mean_reproj_px: 0.25095, model: model.clone() },
"refined 1 pose_ready Registered images: 48 Mean reprojection error: 0.250950px ",
),
(Event::ZoneRefined { meta: m(), range: r(1, 10, 26), cloud, model, dense: true }, "refined 1 ready"),
(Event::AllPositionsDone { meta: m() }, "all positions done"),
(Event::AllRefinedDone { meta: m(), chain: None }, "all refined done"),
];
assert_eq!(cases.len(), TIMELINE_TABLE.len());
for ((e, want), (k, _)) in cases.iter().zip(TIMELINE_TABLE.iter()) {
assert_eq!(e.kind(), *k);
assert_eq!(TimelineSink::line(e).unwrap(), format!("1700000000.000000005 {want}"));
}
assert!(TimelineSink::line(&Event::Warning { meta: m(), message: "x".into() }).is_none());
}
}