1use std::path::{Path, PathBuf};
24use std::sync::{Arc, Mutex, OnceLock};
25use std::time::Instant;
26
27use nalgebra::Matrix4;
28
29use crate::coreg::matching::{match_stem_maps, MatchParams, StemMap, StemMatch};
30use crate::coreg::geometry::{planar_filter, voxel_centroids};
31use crate::coreg::ground::{fit_ground, GroundModel, GroundParams};
32use crate::coreg::icp::{evaluate_registration, icp_prepared, plane_information, IcpConfig, IcpResult, IcpTarget};
33use crate::coreg::reflectors::{match_reflectors, Reflector, ReflectorMatch};
34use crate::coreg::stemmap::StemRecord;
35use crate::coreg::transforms::{invert, transform_difference, transform_points, transform_vectors, Mat4};
36use crate::error::Error;
37use crate::util::numeric::median;
38use crate::util::pyformat::{fixed, percent, signed, thousands};
39use crate::trees::stems::{detect_stems_full, StemParams};
40use crate::Point;
41
42pub type Result<T> = std::result::Result<T, Error>;
44
45pub fn matmul(a: &Mat4, b: &Mat4) -> Mat4 {
50 Matrix4::from_fn(|i, j| {
51 let mut acc = a[(i, 0)] * b[(0, j)];
52 for k in 1..4 {
53 acc = a[(i, k)].mul_add(b[(k, j)], acc);
54 }
55 acc
56 })
57}
58
59fn norm3(v: [f64; 3]) -> f64 {
61 (v[0] * v[0] + v[1] * v[1] + v[2] * v[2]).sqrt()
62}
63
64fn degrees(x: f64) -> f64 {
66 x * (180.0 / std::f64::consts::PI)
67}
68
69fn pymax(a: f64, b: f64) -> f64 {
71 if b > a {
72 b
73 } else {
74 a
75 }
76}
77
78pub(crate) fn py_head<T: Clone>(values: &[T], end: i64) -> Vec<T> {
80 let n = values.len() as i64;
81 let stop = if end < 0 { (n + end).max(0) } else { end.min(n) };
82 values[..stop as usize].to_vec()
83}
84
85pub(crate) fn py_float(x: f64) -> String {
87 if x.is_nan() {
88 "nan".into()
89 } else if x.is_infinite() {
90 if x > 0.0 { "inf".into() } else { "-inf".into() }
91 } else {
92 crate::util::json::py_float_repr(x)
93 }
94}
95
96pub(crate) fn py_repr(s: &str) -> String {
98 let q = if s.contains('\'') && !s.contains('"') { '"' } else { '\'' };
99 let mut out = String::from(q);
100 for c in s.chars() {
101 match c {
102 '\\' => out.push_str("\\\\"),
103 '\n' => out.push_str("\\n"),
104 '\r' => out.push_str("\\r"),
105 '\t' => out.push_str("\\t"),
106 c if c == q => {
107 out.push('\\');
108 out.push(c);
109 }
110 c if (c as u32) < 0x20 || c as u32 == 0x7f => out.push_str(&format!("\\x{:02x}", c as u32)),
111 c => out.push(c),
112 }
113 }
114 out.push(q);
115 out
116}
117
118fn f32_point(p: &Point) -> Point {
120 [p[0] as f32 as f64, p[1] as f32 as f64, p[2] as f32 as f64]
121}
122
123#[derive(Debug, Clone)]
127pub struct ReadingOptions {
128 pub library: Option<PathBuf>,
131 pub drop_pseudo_echoes: bool,
133 pub echoes: String,
135 pub stride: usize,
137 pub shot_stride: usize,
139 pub bounds: [(Option<f64>, Option<f64>); 4],
142 pub any: bool,
144}
145
146impl Default for ReadingOptions {
147 fn default() -> Self {
148 ReadingOptions { library: None, drop_pseudo_echoes: true, echoes: "all".into(), stride: 1, shot_stride: 1, bounds: [(None, None); 4], any: false }
149 }
150}
151
152pub const GATES: [&str; 4] = ["range", "deviation", "reflectance", "amplitude"];
154
155#[derive(Debug, Clone)]
157pub struct CoregConfig {
158 pub ground_cell_size: f64,
160 pub ground_min_coverage: Option<f64>,
165 pub stems: StemParams,
167 pub matching: MatchParams,
169 pub icp: IcpConfig,
171 pub icp_voxel: f64,
173 pub icp_min_planarity: f64,
176 pub icp_max_height: f64,
179 pub use_reflectors: bool,
183 pub min_reflector_matches: i64,
186 pub reflector_tolerance: f64,
190 pub trusted_reflector_matches: i64,
194 pub trusted_reflector_rmse: f64,
196 pub min_match_inliers: i64,
199 pub max_match_ambiguity: f64,
203 pub ambiguity_margin: f64,
206 pub max_match_rmse: f64,
208 pub max_coarse_stem_rmse: f64,
211 pub height_from_ground: bool,
215 pub ground_radius: f64,
219 pub min_ground_cells: i64,
221 pub max_ground_disagreement: Option<f64>,
224 pub max_pair_distance: f64,
226 pub screen_pairs: bool,
228 pub max_pairs_per_scan: i64,
230 pub min_icp_fitness: f64,
233 pub max_icp_rmse: f64,
235 pub min_icp_fitness_above_ground: f64,
239 pub fitness_min_height: f64,
242 pub stem_agreement_tolerance: f64,
245 pub max_coarse_to_fine_shift: f64,
248 pub recover_unregistered: bool,
252 pub recovery_rounds: i64,
255 pub recovery_neighbours: i64,
258 pub refine_multiview: bool,
262 pub refinement_rounds: i64,
264 pub refinement_voxel_sizes: Vec<f64>,
266 pub refinement_max_distances: Vec<f64>,
268 pub refinement_points_per_scan: usize,
270 pub refinement_stem_weight: f64,
272 pub refinement_stem_radius: f64,
275 pub refinement_min_voxel_points: usize,
277 pub refinement_max_shift: f64,
279 pub optimise_globally: bool,
282 pub reference_scan: i64,
286 pub reject_outlier_edges: bool,
289 pub information_patch_points: f64,
293 pub information_min_sigma: f64,
295 pub max_prior_shift: f64,
298 pub max_prior_rotation: Option<f64>,
302 pub workers: i64,
305 pub memory_per_worker_gb: f64,
307 pub riscan_filter: String,
312 pub reading: ReadingOptions,
314 pub min_points_per_scan: i64,
317 pub max_points_per_scan: usize,
323}
324
325impl Default for CoregConfig {
326 fn default() -> Self {
334 CoregConfig {
335 ground_cell_size: 0.5,
336 ground_min_coverage: Some(0.8),
337 stems: StemParams::coreg(),
338 matching: MatchParams::default(),
339 icp: IcpConfig::default(),
340 icp_voxel: 0.05,
341 icp_min_planarity: 0.35,
342 icp_max_height: 12.0,
343 use_reflectors: true,
344 min_reflector_matches: 3,
345 reflector_tolerance: 0.05,
346 trusted_reflector_matches: 5,
347 trusted_reflector_rmse: 0.03,
348 min_match_inliers: 5,
349 max_match_ambiguity: 0.8,
350 ambiguity_margin: 1.25,
351 max_match_rmse: 0.30,
352 max_coarse_stem_rmse: f64::INFINITY,
353 height_from_ground: true,
354 ground_radius: 30.0,
355 min_ground_cells: 50,
356 max_ground_disagreement: Some(0.25),
357 max_pair_distance: 40.0,
358 screen_pairs: true,
359 max_pairs_per_scan: 0,
360 min_icp_fitness: 0.04,
361 max_icp_rmse: 0.15,
362 min_icp_fitness_above_ground: 0.03,
363 fitness_min_height: 1.0,
364 stem_agreement_tolerance: 0.25,
365 max_coarse_to_fine_shift: 2.0,
366 recover_unregistered: true,
367 recovery_rounds: 2,
368 recovery_neighbours: 6,
369 refine_multiview: false,
370 refinement_rounds: 3,
371 refinement_voxel_sizes: vec![0.10, 0.05, 0.03],
372 refinement_max_distances: vec![0.30, 0.15, 0.08],
373 refinement_points_per_scan: 400_000,
374 refinement_stem_weight: 0.05,
375 refinement_stem_radius: 0.15,
376 refinement_min_voxel_points: 1,
377 refinement_max_shift: 0.30,
378 optimise_globally: true,
379 reference_scan: 0,
380 reject_outlier_edges: true,
381 information_patch_points: 100.0,
382 information_min_sigma: 0.005,
383 max_prior_shift: 5.0,
384 max_prior_rotation: None,
385 workers: 0,
386 memory_per_worker_gb: 4.0,
387 riscan_filter: "none".into(),
388 reading: ReadingOptions::default(),
389 min_points_per_scan: 1000,
390 max_points_per_scan: 0,
391 }
392 }
393}
394
395#[derive(Debug, Clone)]
399pub struct ScanFeatures {
400 pub name: String,
402 pub n_points: usize,
404 pub ground: Option<GroundModel>,
407 pub stems: Vec<StemRecord>,
409 pub stem_map_name: String,
411 pub icp_points: Vec<Point>,
413 pub reflectors: Vec<Reflector>,
415 pub icp_heights: Vec<f32>,
417 pub levelling: Mat4,
419 pub origin: Point,
421 pub source: Option<String>,
423 pub seconds: f64,
425 pub error: String,
427}
428
429impl ScanFeatures {
430 pub fn usable(&self) -> bool {
432 self.error.is_empty() && !self.icp_points.is_empty()
433 }
434
435 pub fn location(&self, pose: &Mat4) -> Point {
437 transform_points(pose, &[self.origin])[0]
438 }
439
440 pub fn stem_positions(&self) -> Vec<Point> {
442 self.stems.iter().map(|s| [s.x, s.y, s.z]).collect()
443 }
444
445 pub fn stem_map(&self) -> StemMap {
447 stem_map(&self.stems)
448 }
449}
450
451pub fn stem_map(stems: &[StemRecord]) -> StemMap {
453 StemMap { positions: stems.iter().map(|s| [s.x, s.y, s.z]).collect(), diameters: stems.iter().map(|s| s.dbh).collect(), qualities: stems.iter().map(|s| s.quality()).collect() }
454}
455
456pub fn transform_stems(stems: &[StemRecord], t: &Mat4) -> Vec<StemRecord> {
458 let pos = transform_points(t, &stems.iter().map(|s| [s.x, s.y, s.z]).collect::<Vec<_>>());
459 let ax = transform_vectors(t, &stems.iter().map(|s| s.axis).collect::<Vec<_>>());
460 stems.iter().zip(pos).zip(ax).map(|((s, p), a)| StemRecord { x: p[0], y: p[1], z: p[2], axis: a, ..s.clone() }).collect()
461}
462
463#[derive(Debug, Clone)]
465pub enum ScanInput {
466 Path(PathBuf),
468 Points(Vec<Point>),
470 Failed {
472 reason: String,
474 source: Option<PathBuf>,
476 },
477}
478
479#[derive(Debug)]
481pub enum PrepareError {
482 MissingAttribute(String),
485 Core(Error),
488}
489
490impl From<Error> for PrepareError {
491 fn from(e: Error) -> Self {
492 PrepareError::Core(e)
493 }
494}
495
496impl std::fmt::Display for PrepareError {
497 fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
498 match self {
499 PrepareError::MissingAttribute(m) => f.write_str(m),
500 PrepareError::Core(e) => write!(f, "{e}"),
501 }
502 }
503}
504
505impl std::error::Error for PrepareError {}
506
507impl PrepareError {
508 pub fn describe(&self) -> String {
510 match self {
511 PrepareError::MissingAttribute(m) => format!("KeyError: {}", py_repr(m)),
512 PrepareError::Core(e) => format!("{}: {e}", python_exception(e)),
513 }
514 }
515}
516
517pub fn python_exception(e: &Error) -> &'static str {
519 match e {
520 Error::Io(_) | Error::File { .. } | Error::Las(_) => "OSError",
521 _ => "ValueError",
522 }
523}
524
525fn file_name(path: &Path) -> String {
526 path.file_name().map(|s| s.to_string_lossy().into_owned()).unwrap_or_default()
527}
528
529pub fn path_stem(path: &Path) -> String {
531 let name = file_name(path);
532 match name.rfind('.') {
533 Some(k) if k > 0 && k + 1 < name.len() => name[..k].to_string(),
534 _ => name,
535 }
536}
537
538pub fn read_scan(path: &Path, cfg: &CoregConfig) -> std::result::Result<Vec<Point>, PrepareError> {
544 let is_rxp = path.extension().map(|e| e.to_string_lossy().to_lowercase() == "rxp").unwrap_or(false);
545 let cloud = if is_rxp {
546 let o = &cfg.reading;
547 let opts = crate::io::riegl::RxpOptions { library: o.library.clone(), drop_pseudo_echoes: o.drop_pseudo_echoes, min_range: 0.0, max_range: f64::INFINITY, stride: o.stride.max(1), shot_stride: o.shot_stride.max(1), max_points: None, echoes: o.echoes.clone(),
548 mta: crate::io::rxp_mta::MtaOptions { mode: crate::io::rxp_mta::MtaMode::Off, ..Default::default() }, ..Default::default() };
550 crate::io::riegl::read_rxp(path, &opts)?
551 } else {
552 crate::io::read(path)?
553 };
554 let xyz = cloud.xyz;
555 let mut keep = vec![true; xyz.len()];
556 if cfg.riscan_filter != "none" {
557 let Some(amplitude) = cloud.attrs.get("amplitude") else {
558 return Err(PrepareError::MissingAttribute(format!("the RiSCAN filter needs amplitude, which {} does not carry", file_name(path))));
559 };
560 let p: Vec<[f32; 3]> = xyz.iter().map(|q| [q[0] as f32, q[1] as f32, q[2] as f32]).collect();
561 let a: Vec<f32> = amplitude.to_f64().into_iter().map(|v| v as f32).collect();
562 let mask = crate::riscan::riscan_like_mask(&p, &a, &cfg.riscan_filter, 0.5, &crate::riscan::LegacyFilter::default())?;
563 for (k, m) in keep.iter_mut().zip(mask) {
564 *k &= m;
565 }
566 }
567 for (g, name) in GATES.iter().enumerate() {
568 let (lo, hi) = cfg.reading.bounds[g];
569 if lo.is_none() && hi.is_none() {
570 continue;
571 }
572 let (values, lo, hi) = if *name == "range" {
573 (xyz.iter().map(|p| p[0] * p[0] + p[1] * p[1] + p[2] * p[2]).collect::<Vec<f64>>(), lo.map(|v| v * v), hi.map(|v| v * v))
574 } else if let Some(a) = cloud.attrs.get(*name) {
575 let mut v = a.to_f64();
576 if *name == "deviation" {
577 for x in v.iter_mut() {
578 if *x == 65535.0 {
579 *x = -1.0; }
581 }
582 }
583 (v, lo, hi)
584 } else {
585 return Err(PrepareError::MissingAttribute(format!("reading bounds {name}, which {} does not carry", file_name(path))));
586 };
587 for (k, v) in keep.iter_mut().zip(values) {
588 if let Some(lo) = lo {
589 *k &= v >= lo;
590 }
591 if let Some(hi) = hi {
592 *k &= v <= hi;
593 }
594 }
595 }
596 let mut out: Vec<Point> = xyz.into_iter().zip(keep).filter(|(_, k)| *k).map(|(p, _)| p).collect();
597 if cfg.max_points_per_scan > 0 && out.len() > cfg.max_points_per_scan {
598 out = stride_subsample(&out, cfg.max_points_per_scan);
599 }
600 Ok(out)
601}
602
603pub fn stride_subsample(points: &[Point], count: usize) -> Vec<Point> {
611 let n = points.len();
612 if n <= count {
613 return points.to_vec();
614 }
615 (0..count).map(|i| points[(i as u128 * n as u128 / count as u128) as usize]).collect()
616}
617
618fn ground_params(cfg: &CoregConfig) -> GroundParams {
619 GroundParams { cell_size: cfg.ground_cell_size, ..GroundParams::default() }
620}
621
622pub fn normalise_f32(ground: &GroundModel, points: &[Point]) -> Vec<f32> {
624 let xy: Vec<[f64; 2]> = points.iter().map(|p| [p[0], p[1]]).collect();
625 let h = ground.height_at(&xy);
626 points.iter().zip(h).map(|(p, g)| (p[2] - g) as f32).collect()
627}
628
629pub fn unusable_scan(name: &str, n_points: usize, source: Option<String>, start: Instant, reason: &str) -> ScanFeatures {
631 ScanFeatures {
632 name: name.into(),
633 n_points,
634 ground: None,
635 stems: Vec::new(),
636 stem_map_name: name.into(),
637 icp_points: Vec::new(),
638 reflectors: Vec::new(),
639 icp_heights: Vec::new(),
640 levelling: Matrix4::identity(),
641 origin: [0.0; 3],
642 source,
643 seconds: start.elapsed().as_secs_f64(),
644 error: reason.into(),
645 }
646}
647
648pub fn detect_scan_stems(points: &[Point], ground: &GroundModel, heights: &[f64], p: &StemParams) -> Vec<StemRecord> {
651 if points.len() < 100 {
652 return Vec::new();
653 }
654 let found = detect_stems_full(points, heights, p);
655 let xy: Vec<[f64; 2]> = found.iter().map(|s| [s.tree.x, s.tree.y]).collect();
656 let terrain = ground.height_at(&xy);
657 let mut stems: Vec<StemRecord> = found
658 .iter()
659 .zip(terrain)
660 .map(|(s, z0)| StemRecord { x: s.tree.x, y: s.tree.y, z: z0 + p.reference_height, dbh: s.tree.dbh, axis: s.axis, reference_height: p.reference_height, n_slices: s.tree.n_slices as i64, n_points: s.tree.n_points as i64, rmse: s.tree.rmse, coverage: s.coverage, lean_deg: s.tree.lean_deg })
661 .collect();
662 let q: Vec<f64> = stems.iter().map(|s| -s.quality() + 0.0).collect();
663 let mut order: Vec<usize> = (0..stems.len()).collect();
664 order.sort_by(|&a, &b| q[a].total_cmp(&q[b]));
665 let sorted = order.iter().map(|&k| stems[k].clone()).collect();
666 stems = sorted;
667 stems
668}
669
670pub fn prepare_scan(input: &ScanInput, cfg: &CoregConfig, name: &str, reflectors: &[Reflector], levelling: Option<&Mat4>, origin: Option<Point>) -> std::result::Result<ScanFeatures, PrepareError> {
678 let start = Instant::now();
679 let level = levelling.copied().unwrap_or_else(Matrix4::identity);
680 let mut name = name.to_string();
681 let (mut points, source) = match input {
682 ScanInput::Path(p) => {
683 let pts = read_scan(p, cfg)?;
684 if name.is_empty() {
685 name = path_stem(p);
686 }
687 (pts, Some(p.to_string_lossy().into_owned()))
688 }
689 ScanInput::Points(p) => (p.clone(), None),
690 ScanInput::Failed { reason, source } => return Ok(unusable_scan(&name, 0, source.as_ref().map(|s| s.to_string_lossy().into_owned()), start, reason)),
691 };
692 let mut reflectors = reflectors.to_vec();
693 if levelling.is_some() {
694 points = transform_points(&level, &points);
695 for r in reflectors.iter_mut() {
696 let p = transform_points(&level, &[r.position()])[0];
697 (r.x, r.y, r.z) = (p[0], p[1], p[2]);
698 }
699 }
700 let scanner = origin.unwrap_or([0.0; 3]);
701 if (points.len() as i64) < cfg.min_points_per_scan {
702 let reason = format!("only {} points{}", thousands(points.len() as i64), if cfg.reading.any { " after filtering" } else { "" });
703 return Ok(unusable_scan(&name, points.len(), source, start, &reason));
704 }
705 let mut ground = fit_ground(&points, &ground_params(cfg))?;
706 if cfg.ground_min_coverage.is_some() && (source.is_some() || origin.is_some()) {
707 if let Some(g) = refit_visible_ground(&points, scanner, cfg)? {
708 ground = g;
709 }
710 }
711 let heights = normalise_f32(&ground, &points);
712 let heights64: Vec<f64> = heights.iter().map(|&h| h as f64).collect();
713 let stems = detect_scan_stems(&points, &ground, &heights64, &cfg.stems);
714 let top = cfg.icp_max_height as f32;
715 let below: Vec<Point> = points.iter().zip(&heights).filter(|(_, &h)| h <= top).map(|(p, _)| *p).collect();
716 let icp_points: Vec<Point> = if cfg.icp_min_planarity > 0.0 {
717 if cfg.icp_voxel < 0.0 {
718 return Err(Error::invalid("voxel size must be positive").into());
719 }
720 planar_filter(&below, cfg.icp_min_planarity, Some(cfg.icp_voxel), 20, Some(0.15))
721 } else {
722 if cfg.icp_voxel.is_nan() || cfg.icp_voxel <= 0.0 {
723 return Err(Error::invalid("voxel size must be positive").into());
724 }
725 voxel_centroids(&below, cfg.icp_voxel, true).0
726 };
727 let icp_points: Vec<Point> = icp_points.iter().map(f32_point).collect();
728 let icp_heights = if icp_points.is_empty() { Vec::new() } else { normalise_f32(&ground, &icp_points) };
729 Ok(ScanFeatures { name: name.clone(), n_points: points.len(), ground: Some(ground), stems, stem_map_name: name, icp_points, reflectors, icp_heights, levelling: level, origin: scanner, source, seconds: start.elapsed().as_secs_f64(), error: String::new() })
730}
731
732const REFIT_MIN_RETURNS: usize = 100;
736
737pub fn refit_visible_ground(points: &[Point], scanner: Point, cfg: &CoregConfig) -> Result<Option<GroundModel>> {
747 let Some(min_coverage) = cfg.ground_min_coverage else { return Ok(None) };
748 let n = points.len();
749 let mut azimuth = vec![0usize; n];
750 let mut elevation = vec![0.0f64; n];
751 let mut sampled = vec![[false; 35]; 360];
752 for (k, p) in points.iter().enumerate() {
753 let rel = [p[0] - scanner[0], p[1] - scanner[1], p[2] - scanner[2]];
754 let horizontal = rel[0].hypot(rel[1]);
755 let a = (degrees(rel[1].atan2(rel[0])) + 180.0).floor() as i64;
756 azimuth[k] = a.rem_euclid(360) as usize;
757 elevation[k] = degrees(rel[2].atan2(horizontal));
758 if elevation[k] >= -30.0 && elevation[k] < 5.0 {
759 sampled[azimuth[k]][(elevation[k] + 30.0).floor() as usize] = true;
760 }
761 }
762 let blind: Vec<bool> = sampled.iter().map(|row| (row.iter().filter(|&&s| s).count() as f64 / 35.0) < min_coverage).collect();
763 if !blind.iter().any(|&b| b) || blind.iter().all(|&b| b) {
764 return Ok(None);
765 }
766 let widen = 3i64;
767 let widened: Vec<bool> = (0..360i64).map(|a| (-widen..=widen).any(|d| blind[(a + d).rem_euclid(360) as usize])).collect();
768 let kept: Vec<Point> = (0..n).filter(|&k| !widened[azimuth[k]] || elevation[k] < -45.0).map(|k| points[k]).collect();
769 if kept.len() < REFIT_MIN_RETURNS {
773 return Ok(None);
774 }
775 Ok(Some(fit_ground(&kept, &ground_params(cfg))?))
776}
777
778pub fn terrain_samples(scan: &ScanFeatures, radius: f64) -> Vec<Point> {
783 let Some(g) = &scan.ground else { return Vec::new() };
784 let mut xy = Vec::new();
785 for iy in 0..g.ny {
786 for ix in 0..g.nx {
787 if g.observed[iy * g.nx + ix] {
788 let p = [g.origin[0] + ix as f64 * g.cell_size, g.origin[1] + iy as f64 * g.cell_size];
789 if (p[0] - scan.origin[0]).hypot(p[1] - scan.origin[1]) <= radius {
790 xy.push(p);
791 }
792 }
793 }
794 }
795 let h = g.height_at(&xy);
796 xy.iter().zip(h).map(|(p, z)| [p[0], p[1], z]).collect()
797}
798
799pub fn height_offset(source: &ScanFeatures, world_from_source: &Mat4, targets: &[(&ScanFeatures, Mat4)], cfg: &CoregConfig) -> f64 {
807 let samples = terrain_samples(source, cfg.ground_radius);
808 if samples.is_empty() {
809 return f64::NAN;
810 }
811 let world = transform_points(world_from_source, &samples);
812 let mut offsets = Vec::new();
813 for (scan, pose) in targets {
814 let Some(g) = &scan.ground else { continue };
815 let local = transform_points(&invert(pose), &world);
816 let (rows, cols) = (g.ny as f64, g.nx as f64);
817 let inside: Vec<&Point> = local
818 .iter()
819 .filter(|p| {
820 let (cx, cy) = ((p[0] - g.origin[0]) / g.cell_size, (p[1] - g.origin[1]) / g.cell_size);
821 cx >= -0.5 && cx <= cols - 0.5 && cy >= -0.5 && cy <= rows - 0.5 && (p[0] - scan.origin[0]).hypot(p[1] - scan.origin[1]) <= cfg.ground_radius
822 })
823 .collect();
824 let support = g.support(&inside.iter().map(|p| [p[0], p[1]]).collect::<Vec<_>>());
825 let shared: Vec<&Point> = inside.into_iter().zip(support).filter(|(_, s)| *s).map(|(p, _)| p).collect();
826 let z: Vec<f64> = shared.iter().map(|p| p[2]).collect();
827 let shared: Vec<[f64; 2]> = shared.iter().map(|p| [p[0], p[1]]).collect();
828 let h = g.height_at(&shared);
829 offsets.extend(h.iter().zip(&z).map(|(h, z)| h - z));
830 }
831 if (offsets.len() as i64) < cfg.min_ground_cells.max(1) {
832 return f64::NAN;
833 }
834 median(&offsets)
835}
836
837pub fn on_ground(transform: &Mat4, source: &ScanFeatures, targets: &[(&ScanFeatures, Mat4)], cfg: &CoregConfig) -> Mat4 {
840 if !cfg.height_from_ground {
841 return *transform;
842 }
843 let dz = height_offset(source, transform, targets, cfg);
844 if !dz.is_finite() {
845 return *transform;
846 }
847 let mut out = *transform;
848 out[(2, 3)] += dz;
849 out
850}
851
852pub fn ground_disagrees(offset: f64, cfg: &CoregConfig) -> bool {
854 cfg.max_ground_disagreement.is_some_and(|m| offset.is_finite() && offset.abs() > m)
855}
856
857pub fn stem_median_residual(transform: &Mat4, matched_source: &[Point], matched_target: &[Point]) -> f64 {
860 if matched_source.is_empty() {
861 return f64::NAN;
862 }
863 let moved = transform_points(transform, matched_source);
864 let d: Vec<f64> = moved
865 .iter()
866 .zip(matched_target)
867 .map(|(p, q)| {
868 let (dx, dy) = (p[0] - q[0], p[1] - q[1]);
869 (dx * dx + dy * dy).sqrt()
870 })
871 .collect();
872 median(&d)
873}
874
875pub fn above_ground_fitness(source: &[Point], heights: &[f32], target: &[Point], transform: &Mat4, cfg: &CoregConfig) -> f64 {
879 if cfg.fitness_min_height <= 0.0 || heights.len() != source.len() {
880 return f64::NAN;
881 }
882 let bar = cfg.fitness_min_height as f32;
883 let above: Vec<Point> = source.iter().zip(heights).filter(|(_, &h)| h > bar).map(|(p, _)| *p).collect();
884 if above.len() < 100 {
885 return f64::NAN;
886 }
887 evaluate_registration(&above, target, transform, cfg.icp.fitness_threshold, 200_000, Some(0.05), 0).0
888}
889
890pub fn prior_ok(pose: &Mat4, prior: &Mat4, origin: Point, cfg: &CoregConfig) -> (bool, String) {
894 let (a, b) = (transform_points(pose, &[origin])[0], transform_points(prior, &[origin])[0]);
895 let shift = norm3([a[0] - b[0], a[1] - b[1], a[2] - b[2]]);
896 if shift > cfg.max_prior_shift {
897 return (false, format!("scanner {} m from its prior position", fixed(shift, 1)));
898 }
899 if let Some(limit) = cfg.max_prior_rotation {
900 let xi = crate::coreg::transforms::se3_log(&matmul(&invert(prior), pose));
901 let rot = degrees(norm3([xi[0], xi[1], xi[2]]));
902 if rot > limit {
903 return (false, format!("{} deg from the prior orientation", fixed(rot, 1)));
904 }
905 }
906 (true, String::new())
907}
908
909#[derive(Debug, Clone)]
914pub struct PairResult {
915 pub i: i64,
917 pub j: i64,
919 pub name_i: String,
921 pub name_j: String,
923 pub transform: Mat4,
926 pub coarse_transform: Mat4,
929 pub stem_match: Option<StemMatch>,
932 pub reflector_match: Option<ReflectorMatch>,
935 pub icp: Option<IcpResult>,
938 pub success: bool,
940 pub reason: String,
942 pub seconds: f64,
944 pub matched_source: Vec<Point>,
946 pub matched_target: Vec<Point>,
948 pub coarse_stem_rmse: f64,
951 pub fine_stem_rmse: f64,
954 pub fitness_above: f64,
956 pub rival: Option<StemMatch>,
959 pub used_icp: bool,
961 pub ground_offset: f64,
964 pub trusted: bool,
966}
967
968impl PairResult {
969 pub fn new(i: i64, j: i64, name_i: &str, name_j: &str) -> Self {
972 PairResult {
973 i,
974 j,
975 name_i: name_i.into(),
976 name_j: name_j.into(),
977 transform: Matrix4::identity(),
978 coarse_transform: Matrix4::identity(),
979 stem_match: None,
980 reflector_match: None,
981 icp: None,
982 success: false,
983 reason: String::new(),
984 seconds: 0.0,
985 matched_source: Vec::new(),
986 matched_target: Vec::new(),
987 coarse_stem_rmse: f64::NAN,
988 fine_stem_rmse: f64::NAN,
989 fitness_above: f64::NAN,
990 rival: None,
991 used_icp: true,
992 ground_offset: f64::NAN,
993 trusted: false,
994 }
995 }
996
997 pub fn fitness(&self) -> f64 {
999 self.icp.as_ref().map_or(0.0, |r| r.fitness)
1000 }
1001
1002 pub fn rmse(&self) -> f64 {
1004 self.icp.as_ref().map_or(f64::INFINITY, |r| r.inlier_rmse)
1005 }
1006
1007 pub fn n_stem_matches(&self) -> usize {
1009 self.stem_match.as_ref().map_or(0, |m| m.n_inliers)
1010 }
1011
1012 pub fn stem_rmse(&self) -> f64 {
1014 if self.used_icp {
1015 self.fine_stem_rmse
1016 } else {
1017 self.coarse_stem_rmse
1018 }
1019 }
1020
1021 pub fn summary(&self) -> String {
1023 let status = if self.success { "ok " } else { "FAIL" };
1024 let stem = match &self.reflector_match {
1025 Some(r) => format!(" targets={}", r.n_inliers),
1026 None if self.stem_rmse().is_finite() => format!(" stem={}cm", fixed(self.stem_rmse() * 100.0, 1)),
1027 None => String::new(),
1028 };
1029 let mut s = format!("[{status}] {} -> {}: stems={:2} ", self.name_i, self.name_j, self.n_stem_matches());
1030 if let Some(m) = self.stem_match.as_ref().filter(|m| m.ambiguity > 0.0) {
1031 s += &format!("amb={} ", fixed(m.ambiguity, 2));
1032 }
1033 s += &format!("fitness={} ", fixed(self.fitness(), 3));
1034 if self.fitness_above.is_finite() {
1035 s += &format!("above={} ", fixed(self.fitness_above, 3));
1036 }
1037 if self.ground_offset.is_finite() {
1038 s += &format!("dz={}cm ", signed(self.ground_offset * 100.0, 1));
1039 }
1040 s + &format!("rmse={} mm{stem} ({})", crate::util::pyformat::fixed_width(self.rmse() * 1000.0, 6, 1), self.reason)
1041 }
1042}
1043
1044fn attach_matched_stems(result: &mut PairResult, source: &ScanFeatures, target: &ScanFeatures, m: &StemMatch) {
1046 if !m.success || m.correspondences.is_empty() {
1047 return;
1048 }
1049 result.matched_source = m.correspondences.iter().map(|&(a, _)| { let s = &source.stems[a]; [s.x, s.y, s.z] }).collect();
1050 result.matched_target = m.correspondences.iter().map(|&(_, b)| { let s = &target.stems[b]; [s.x, s.y, s.z] }).collect();
1051}
1052
1053fn coarse_is_acceptable(result: &mut PairResult, m: &StemMatch, cfg: &CoregConfig) -> bool {
1056 if !m.success || (m.n_inliers as i64) < cfg.min_match_inliers {
1057 result.reason = format!("stem matching failed ({} inliers, need {})", m.n_inliers, cfg.min_match_inliers);
1058 return false;
1059 }
1060 if m.inlier_rmse > cfg.max_match_rmse {
1061 result.reason = format!("coarse match too loose ({} cm scatter, limit {} cm)", fixed(m.inlier_rmse * 100.0, 1), fixed(cfg.max_match_rmse * 100.0, 0));
1062 return false;
1063 }
1064 if m.ambiguity > cfg.max_match_ambiguity {
1065 match &m.rival {
1066 Some(r) if r.success => result.rival = Some((**r).clone()),
1067 _ => {
1068 result.reason = format!("ambiguous stem pattern (a rival alignment has {} of the inliers, limit {})", percent(m.ambiguity, 0), percent(cfg.max_match_ambiguity, 0));
1069 return false;
1070 }
1071 }
1072 }
1073 result.coarse_stem_rmse = stem_median_residual(&m.transform.0, &result.matched_source, &result.matched_target);
1074 if result.coarse_stem_rmse > cfg.max_coarse_stem_rmse {
1075 result.reason = format!("coarse stems disagree by {} m (limit {} m)", fixed(result.coarse_stem_rmse, 2), fixed(cfg.max_coarse_stem_rmse, 2));
1076 return false;
1077 }
1078 true
1079}
1080
1081pub fn screen_probe(probe: &mut PairResult, source: &ScanFeatures, target: &ScanFeatures, m: &StemMatch, cfg: &CoregConfig) -> bool {
1084 attach_matched_stems(probe, source, target, m);
1085 coarse_is_acceptable(probe, m, cfg)
1086}
1087
1088fn run_icp(source: &ScanFeatures, target: &ScanFeatures, pyramid: Option<&IcpTarget>, initial: &Mat4, cfg: &IcpConfig) -> Result<IcpResult> {
1090 match pyramid {
1091 Some(t) => icp_prepared(&source.icp_points, t, Some(*initial), cfg),
1092 None => crate::coreg::icp::icp(&source.icp_points, &target.icp_points, Some(*initial), cfg),
1093 }
1094}
1095
1096fn plane_config(cfg: &IcpConfig) -> IcpConfig {
1099 IcpConfig { voxel_sizes: cfg.voxel_sizes.clone(), max_distances: cfg.max_distances.clone(), robust: cfg.robust.clone(), robust_scale: cfg.robust_scale, trim_fraction: cfg.trim_fraction, trim_ramp: cfg.trim_ramp, min_planarity: cfg.min_planarity, normal_neighbours: cfg.normal_neighbours, max_points: cfg.max_points, seed: cfg.seed, ..IcpConfig::default() }
1100}
1101
1102fn score(source: &ScanFeatures, target: &ScanFeatures, pyramid: Option<&IcpTarget>, transform: &Mat4, cfg: &CoregConfig, refined: &IcpResult) -> Result<IcpResult> {
1104 let (fitness, rmse, n) = evaluate_registration(&source.icp_points, &target.icp_points, transform, cfg.icp.fitness_threshold, 200_000, Some(0.05), 0);
1105 let pc = plane_config(&cfg.icp);
1106 let information = match pyramid {
1107 Some(t) if t.matches(&pc) => plane_information(&source.icp_points, t, transform, &pc),
1110 _ => plane_information(&source.icp_points, &IcpTarget::new(&target.icp_points, &pc), transform, &pc),
1111 };
1112 Ok(IcpResult { transform: *transform, fitness, inlier_rmse: rmse, n_correspondences: n, iterations: refined.iterations, converged: false, history: refined.history.clone(), information })
1113}
1114
1115pub fn trust_reflectors(result: &mut PairResult, coarse: &Mat4, shift: f64, cfg: &CoregConfig) {
1118 let Some(found) = &result.reflector_match else { return };
1119 if cfg.trusted_reflector_matches <= 0 || (found.n_inliers as i64) < cfg.trusted_reflector_matches || found.rmse > cfg.trusted_reflector_rmse || result.reason.starts_with("ambiguous") {
1120 return;
1121 }
1122 let (n, rmse) = (found.n_inliers, found.rmse);
1123 let why = result.reason.clone();
1124 if shift > cfg.reflector_tolerance {
1125 result.transform = *coarse;
1126 result.used_icp = false;
1127 }
1128 result.success = true;
1129 result.trusted = true;
1130 result.reason = format!("{n} reflectors, {} mm, trusted ({} pose; ICP alone: {why})", fixed(rmse * 1000.0, 1), if result.used_icp { "ICP" } else { "targets" });
1131}
1132
1133fn yaw(t: &Mat4) -> f64 {
1134 t[(1, 0)].atan2(t[(0, 0)])
1135}
1136
1137pub fn refine_and_judge(result: &mut PairResult, source: &ScanFeatures, target: &ScanFeatures, cfg: &CoregConfig, coarse: &Mat4, prepared: Option<&IcpTarget>) -> Result<()> {
1144 let built;
1145 let pyramid = match prepared {
1146 Some(t) => Some(t),
1147 None if result.rival.is_some() => {
1148 built = IcpTarget::new(&target.icp_points, &cfg.icp); Some(&built)
1150 }
1151 None => None,
1152 };
1153 let mut coarse = *coarse;
1154 result.coarse_transform = coarse;
1155 let mut refined = run_icp(source, target, pyramid, &coarse, &cfg.icp)?;
1156 result.icp = Some(refined.clone());
1157 result.transform = refined.transform;
1158 if result.matched_source.len() >= 3 {
1160 result.coarse_stem_rmse = stem_median_residual(&coarse, &result.matched_source, &result.matched_target);
1161 result.fine_stem_rmse = stem_median_residual(&refined.transform, &result.matched_source, &result.matched_target);
1162 if result.fine_stem_rmse > result.coarse_stem_rmse + cfg.stem_agreement_tolerance {
1163 result.transform = coarse;
1164 result.used_icp = false;
1165 }
1166 }
1167 result.fitness_above = above_ground_fitness(&source.icp_points, &source.icp_heights, &target.icp_points, &result.transform, cfg);
1168 if let Some(rival) = result.rival.clone() {
1169 let rival_coarse = on_ground(&rival.transform.0, source, &[(target, Matrix4::identity())], cfg);
1172 let other = run_icp(source, target, pyramid, &rival_coarse, &cfg.icp)?;
1173 let other_above = above_ground_fitness(&source.icp_points, &source.icp_heights, &target.icp_points, &other.transform, cfg);
1174 let mut mine = if result.fitness_above.is_finite() { result.fitness_above } else { 0.0 };
1175 let mut theirs = if other_above.is_finite() { other_above } else { 0.0 };
1176 let (_, apart) = transform_difference(&result.transform, &other.transform);
1178 let mut yaw_apart = degrees((yaw(&result.transform) - yaw(&other.transform)).abs());
1179 yaw_apart = yaw_apart.min(360.0 - yaw_apart);
1180 let converged = apart < cfg.matching.distinct_translation && yaw_apart < cfg.matching.distinct_yaw_deg;
1181 if converged {
1182 result.reason = "rivals converged in ICP; ".into();
1183 } else if theirs > mine {
1184 result.stem_match = Some(rival.clone());
1185 attach_matched_stems(result, source, target, &rival);
1186 coarse = rival_coarse;
1187 result.coarse_transform = coarse;
1188 refined = other.clone();
1189 result.icp = Some(other.clone());
1190 result.transform = other.transform;
1191 result.fitness_above = other_above;
1192 std::mem::swap(&mut mine, &mut theirs);
1193 result.used_icp = true;
1194 if result.matched_source.len() >= 3 {
1195 result.coarse_stem_rmse = stem_median_residual(&coarse, &result.matched_source, &result.matched_target);
1196 result.fine_stem_rmse = stem_median_residual(&refined.transform, &result.matched_source, &result.matched_target);
1197 }
1198 }
1199 if !converged {
1200 if mine < cfg.ambiguity_margin * theirs {
1201 result.reason = format!("ambiguous stem pattern; ICP cannot separate the rivals (above-ground fitness {} vs {})", fixed(mine, 3), fixed(theirs, 3));
1202 return Ok(());
1203 }
1204 result.reason = format!("rival resolved by ICP ({} vs {} above ground); ", fixed(mine, 3), fixed(theirs, 3));
1205 }
1206 }
1207 if !result.used_icp {
1208 result.icp = Some(score(source, target, pyramid, &result.transform, cfg, &refined)?);
1209 }
1210 let scored = result.icp.clone().expect("scored");
1211 let (_, shift) = transform_difference(&coarse, &refined.transform);
1212 result.ground_offset = height_offset(source, &result.transform, &[(target, Matrix4::identity())], cfg);
1213 if result.used_icp && shift > cfg.max_coarse_to_fine_shift {
1214 result.reason = format!("ICP diverged from the coarse solution by {} m", fixed(shift, 2));
1215 } else if scored.fitness < cfg.min_icp_fitness {
1216 result.reason = format!("low ICP fitness ({} < {})", fixed(scored.fitness, 3), py_float(cfg.min_icp_fitness));
1217 } else if result.fitness_above < cfg.min_icp_fitness_above_ground {
1218 result.reason = format!("low above-ground fitness ({} < {}); ground alone matched", fixed(result.fitness_above, 3), py_float(cfg.min_icp_fitness_above_ground));
1219 } else if scored.inlier_rmse > cfg.max_icp_rmse {
1220 result.reason = format!("high ICP rmse ({} m > {})", fixed(scored.inlier_rmse, 3), py_float(cfg.max_icp_rmse));
1221 } else if ground_disagrees(result.ground_offset, cfg) {
1222 result.reason = format!("terrain heights disagree by {} m", signed(result.ground_offset, 2));
1223 } else {
1224 result.success = true;
1225 if result.reflector_match.is_none() {
1226 let kept = if result.reason.starts_with("rival resolved") || result.reason.starts_with("rivals converged") { result.reason.clone() } else { String::new() };
1227 result.reason = kept
1228 + &if result.used_icp {
1229 format!("shift from coarse {} cm", fixed(shift * 100.0, 1))
1230 } else {
1231 format!("kept coarse (ICP moved the stems {} cm)", fixed(shift * 100.0, 1))
1232 };
1233 } else if !result.used_icp {
1234 result.reason += " (kept coarse)";
1235 }
1236 }
1237 if !result.success {
1238 trust_reflectors(result, &coarse, shift, cfg);
1239 }
1240 Ok(())
1241}
1242
1243fn coarse_transform(result: &mut PairResult, source: &ScanFeatures, target: &ScanFeatures, cfg: &CoregConfig, given: Option<&StemMatch>, reflectors: bool) -> Result<Option<Mat4>> {
1245 if reflectors {
1246 let src: Vec<Point> = source.reflectors.iter().map(|r| r.position()).collect();
1247 let dst: Vec<Point> = target.reflectors.iter().map(|r| r.position()).collect();
1248 let found = match_reflectors(&src, &dst, cfg.reflector_tolerance, cfg.min_reflector_matches, 0.03)?;
1249 if !found.success {
1250 return Ok(None);
1251 }
1252 result.reason = format!("{} reflectors, {} mm", found.n_inliers, fixed(found.rmse * 1000.0, 1));
1253 let t = found.transform;
1254 result.reflector_match = Some(found);
1255 return Ok(Some(t));
1256 }
1257 let m = match given {
1258 Some(m) => m.clone(),
1259 None => match_stem_maps(&source.stem_map(), &target.stem_map(), &cfg.matching),
1260 };
1261 result.stem_match = Some(m.clone());
1262 attach_matched_stems(result, source, target, &m);
1263 if !coarse_is_acceptable(result, &m, cfg) {
1264 return Ok(None);
1265 }
1266 Ok(Some(on_ground(&m.transform.0, source, &[(target, Matrix4::identity())], cfg)))
1267}
1268
1269#[allow(clippy::too_many_arguments)]
1277pub fn register_pair(source: &ScanFeatures, target: &ScanFeatures, cfg: &CoregConfig, initial: Option<&Mat4>, stem_match: Option<&StemMatch>, i: i64, j: i64, prepared: Option<&IcpTarget>) -> Result<PairResult> {
1278 let start = Instant::now();
1279 let mut result = PairResult::new(i, j, &source.name, &target.name);
1280 if let Some(initial) = initial {
1281 refine_and_judge(&mut result, source, target, cfg, initial, prepared)?;
1282 result.seconds = start.elapsed().as_secs_f64();
1283 return Ok(result);
1284 }
1285 let mut attempts = Vec::new();
1289 if cfg.use_reflectors && !source.reflectors.is_empty() && !target.reflectors.is_empty() {
1290 attempts.push(true);
1291 }
1292 attempts.push(false);
1293 let mut last = result;
1294 for reflectors in attempts {
1295 let mut attempt = PairResult::new(i, j, &source.name, &target.name);
1296 let Some(coarse) = coarse_transform(&mut attempt, source, target, cfg, stem_match, reflectors)? else {
1297 if !attempt.reason.is_empty() {
1298 last = attempt;
1299 }
1300 continue;
1301 };
1302 refine_and_judge(&mut attempt, source, target, cfg, &coarse, prepared)?;
1303 attempt.seconds = start.elapsed().as_secs_f64();
1304 if attempt.success {
1305 return Ok(attempt);
1306 }
1307 last = attempt;
1308 }
1309 last.seconds = start.elapsed().as_secs_f64();
1310 Ok(last)
1311}
1312
1313pub fn edge_quality(pair: &PairResult) -> (f64, f64, i64) {
1317 if pair.trusted && !pair.used_icp {
1318 if let Some(found) = &pair.reflector_match {
1319 return (1.0, pymax(found.rmse, 1e-3), found.n_inliers as i64);
1320 }
1321 }
1322 (pair.fitness(), pair.rmse(), pair.icp.as_ref().map_or(0, |r| r.n_correspondences as i64))
1323}
1324
1325pub fn edge_information(pair: &PairResult, cfg: &CoregConfig) -> Option<nalgebra::Matrix6<f64>> {
1328 let info = pair.icp.as_ref()?.information.as_ref()?;
1329 if pair.trusted && !pair.used_icp {
1330 return None;
1331 }
1332 Some(crate::coreg::posegraph::plane_edge_information(&info.hessian, info.sigma, info.n as i64, &pair.transform, cfg.information_patch_points, cfg.information_min_sigma))
1333}
1334
1335pub struct TargetCache<'a> {
1343 scans: &'a [ScanFeatures],
1344 config: &'a IcpConfig,
1345 capacity: usize,
1346 built: Mutex<Vec<(usize, Arc<OnceLock<IcpTarget>>)>>,
1347}
1348
1349impl<'a> TargetCache<'a> {
1350 pub fn new(scans: &'a [ScanFeatures], config: &'a IcpConfig, capacity: usize) -> Self {
1353 TargetCache { scans, config, capacity: capacity.max(1), built: Mutex::new(Vec::new()) }
1354 }
1355
1356 pub fn get(&self, k: usize) -> Arc<OnceLock<IcpTarget>> {
1359 let cell = {
1360 let mut built = self.built.lock().expect("target cache");
1361 let cell = match built.iter().position(|(m, _)| *m == k) {
1362 Some(pos) => {
1363 let entry = built.remove(pos);
1364 let cell = Arc::clone(&entry.1);
1365 built.push(entry);
1366 cell
1367 }
1368 None => {
1369 let cell = Arc::new(OnceLock::new());
1370 built.push((k, Arc::clone(&cell)));
1371 while built.len() > self.capacity {
1372 built.remove(0);
1373 }
1374 cell
1375 }
1376 };
1377 cell
1378 };
1379 cell.get_or_init(|| IcpTarget::new(&self.scans[k].icp_points, self.config));
1380 cell
1381 }
1382}
1383
1384#[cfg(test)]
1385mod tests {
1386 use super::*;
1387
1388 fn stand(seed: u64) -> Vec<Point> {
1389 let mut rng = crate::util::nprandom::Generator::new(seed);
1390 let mut pts = Vec::new();
1391 for _ in 0..30000 {
1392 let (x, y) = (rng.random() * 30.0 - 15.0, rng.random() * 30.0 - 15.0);
1393 pts.push([x, y, 0.02 * x + 0.01 * rng.random()]);
1394 }
1395 for (cx, cy, r) in [(3.0, 2.0, 0.2), (-4.0, 5.0, 0.3), (6.0, -5.0, 0.25), (-6.0, -3.0, 0.15), (0.5, 8.0, 0.22), (9.0, 4.0, 0.35), (-9.0, 7.0, 0.18), (1.0, -8.0, 0.28)] {
1396 for _ in 0..3000 {
1397 let a = rng.random() * std::f64::consts::TAU;
1398 let h = rng.random() * 6.0;
1399 pts.push([cx + r * a.cos(), cy + r * a.sin(), 0.02 * cx + h]);
1400 }
1401 }
1402 pts
1403 }
1404
1405 #[test]
1406 fn products_are_numpys() {
1407 let a = crate::coreg::transforms::se3_exp(&nalgebra::Vector6::new(0.1, -0.2, 0.3, 1.0, 2.0, -3.0));
1408 let b = crate::coreg::transforms::se3_exp(&nalgebra::Vector6::new(-0.3, 0.1, 0.2, -1.0, 0.5, 4.0));
1409 let c = matmul(&a, &b);
1410 assert!((c - a * b).abs().max() < 1e-12);
1411 assert_eq!(matmul(&Matrix4::identity(), &b), b);
1412 }
1413
1414 #[test]
1415 fn python_helpers() {
1416 assert_eq!(py_head(&[1, 2, 3], 2), vec![1, 2]);
1417 assert_eq!(py_head(&[1, 2, 3], -1), vec![1, 2]);
1418 assert_eq!(py_head(&[1, 2, 3], -5), Vec::<i32>::new());
1419 assert_eq!(py_head(&[1, 2, 3], 9), vec![1, 2, 3]);
1420 assert_eq!(py_float(0.04), "0.04");
1421 assert_eq!(py_float(1.0), "1.0");
1422 assert_eq!(py_repr("it's"), "\"it's\"");
1423 assert_eq!(py_repr("a"), "'a'");
1424 assert_eq!(path_stem(Path::new("/x/a.b.laz")), "a.b");
1425 assert_eq!(path_stem(Path::new("/x/.hidden")), ".hidden");
1426 assert_eq!(PrepareError::MissingAttribute("x".into()).describe(), "KeyError: 'x'");
1427 }
1428
1429 #[test]
1430 fn a_scan_is_prepared_and_registers_to_itself() {
1431 let cfg = CoregConfig::default();
1432 let pts = stand(3);
1433 let scan = prepare_scan(&ScanInput::Points(pts.clone()), &cfg, "a", &[], None, None).unwrap();
1434 assert!(scan.usable());
1435 assert_eq!(scan.stems.len(), 8);
1436 assert!(scan.stems.windows(2).all(|w| w[0].quality() >= w[1].quality()));
1437 assert!(scan.icp_points.iter().all(|p| p.iter().all(|&v| v as f32 as f64 == v)));
1438 let moved = crate::coreg::transforms::yaw_transform(0.4, 2.0, -1.0, 0.3);
1439 let other = prepare_scan(&ScanInput::Points(transform_points(&invert(&moved), &pts)), &cfg, "b", &[], None, None).unwrap();
1440 let pair = register_pair(&scan, &other, &cfg, None, None, 0, 1, None).unwrap();
1441 assert!(pair.success, "{}", pair.reason);
1442 let truth = invert(&moved);
1443 let (_, t) = transform_difference(&pair.transform, &truth);
1444 assert!(t < 0.02, "{t}");
1445 assert!(pair.summary().starts_with("[ok ] a -> b: stems= 8"));
1446 let few = prepare_scan(&ScanInput::Points(pts[..500].to_vec()), &cfg, "few", &[], None, None).unwrap();
1447 assert_eq!(few.error, "only 500 points");
1448 assert!(!few.usable());
1449 let offset = height_offset(&other, &moved, &[(&scan, Matrix4::identity())], &cfg);
1450 assert!(offset.abs() < 0.02, "{offset}");
1451 }
1452
1453 #[test]
1454 fn a_kept_coarse_transform_is_scored_against_a_point_to_point_pyramid() {
1455 let mut cfg = CoregConfig { stem_agreement_tolerance: -1.0, ..CoregConfig::default() };
1459 cfg.icp.method = "point_to_point".into();
1460 let pts = stand(3);
1461 let moved = crate::coreg::transforms::yaw_transform(0.4, 2.0, -1.0, 0.3);
1462 let a = prepare_scan(&ScanInput::Points(pts.clone()), &cfg, "a", &[], None, None).unwrap();
1463 let b = prepare_scan(&ScanInput::Points(transform_points(&invert(&moved), &pts)), &cfg, "b", &[], None, None).unwrap();
1464 let prepared = IcpTarget::new(&b.icp_points, &cfg.icp);
1465 let pair = register_pair(&a, &b, &cfg, None, None, 0, 1, Some(&prepared)).unwrap();
1466 assert!(pair.success && !pair.used_icp, "{}", pair.reason);
1467 assert!(pair.icp.unwrap().information.is_some());
1468 }
1469
1470 #[test]
1471 fn the_point_cap_strides_through_the_whole_scan() {
1472 let pts: Vec<Point> = (0..10).map(|k| [k as f64, 0.0, 0.0]).collect();
1473 let x = |v: Vec<Point>| v.iter().map(|p| p[0] as usize).collect::<Vec<_>>();
1474 assert_eq!(x(stride_subsample(&pts, 4)), vec![0, 2, 5, 7]);
1475 assert_eq!(x(stride_subsample(&pts, 10)), (0..10).collect::<Vec<_>>());
1476 assert_eq!(stride_subsample(&pts, 99).len(), 10);
1477 assert!(stride_subsample(&pts, 0).is_empty());
1478 let dir = std::env::temp_dir().join(format!("sylva-cap-{}", std::process::id()));
1480 std::fs::create_dir_all(&dir).unwrap();
1481 let path = dir.join("scan.xyz");
1482 let text: String = (0..1000).map(|k| format!("{k} 0 0\n")).collect();
1483 std::fs::write(&path, text).unwrap();
1484 let cfg = CoregConfig { max_points_per_scan: 10, ..CoregConfig::default() };
1485 let read = read_scan(&path, &cfg);
1486 std::fs::remove_dir_all(&dir).unwrap();
1487 let read = read.unwrap();
1488 assert_eq!(read.len(), 10);
1489 assert_eq!(read[9][0], 900.0);
1490 }
1491
1492 #[test]
1493 fn a_refit_that_would_leave_no_ground_keeps_the_first_fit() {
1494 let mut rng = crate::util::nprandom::Generator::new(5);
1499 let mut pts: Vec<Point> = Vec::new();
1500 for _ in 0..60000 {
1501 let (a, r) = (rng.random() * std::f64::consts::TAU, 3.0 + 27.0 * rng.random());
1502 pts.push([r * a.cos(), r * a.sin(), 0.0]);
1503 }
1504 for k in 0..60 {
1505 let a = (k as f64 * 6.0 + 0.5).to_radians();
1506 pts.extend((0..500).map(|i| [8.0 * a.cos(), 8.0 * a.sin(), 6.0 * i as f64 / 500.0]));
1507 }
1508 let cfg = CoregConfig::default();
1509 assert!(refit_visible_ground(&pts, [0.0, 0.0, 1.6], &cfg).unwrap().is_none());
1510 let scan = prepare_scan(&ScanInput::Points(pts), &cfg, "open", &[], None, Some([0.0, 0.0, 1.6])).unwrap();
1511 assert!(scan.usable(), "{}", scan.error);
1512 }
1513}