Skip to main content

sylva_rs/coreg/
pipeline.rs

1// Sylva: LiDAR processing for forest ecology and remote sensing research.
2// Copyright (C) 2026 Tim Devereux, The University of Queensland.
3// Free software under the GNU General Public License v3.0 or later;
4// see the LICENSE file. There is no warranty, to the extent permitted by law.
5//! The coregistration pipeline: scans and pairs.
6//!
7//! **Per scan** ([`prepare_scan`]): a terrain model (refitted without the
8//! directions a tilted scanner could not see, [`refit_visible_ground`]), the
9//! stem map and a planarity-filtered subsample for ICP. **Per pair**
10//! ([`register_pair`]): reflective targets where both scans saw them,
11//! otherwise a global stem-map match with its height taken from the two
12//! terrain models ([`on_ground`]), refined by ICP; the pair is accepted only
13//! if it fits over all points and above the ground and its terrain agrees,
14//! and a strong target match can be trusted where ICP fails. The whole
15//! survey is in [`crate::coreg::survey`].
16//!
17//! Every decision, message and number follows the NumPy implementation this
18//! replaced (`sylva.coreg.pipeline`), including NumPy's arithmetic where it
19//! matters: 4x4 products are sequential fused multiply-adds (NumPy hands
20//! them to BLAS), heights are compared in float32 as NumPy compares a
21//! float32 array with a Python float, and the ICP points are float32.
22
23use 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
42/// Result of a pipeline function, failing with the crate's [`Error`].
43pub type Result<T> = std::result::Result<T, Error>;
44
45// ----------------------------------------------------------------- arithmetic
46
47/// `a @ b` as NumPy computes a 4x4 product (BLAS: one fused multiply-add
48/// after another along the inner dimension).
49pub 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
59/// `np.linalg.norm` of a 3-vector.
60fn norm3(v: [f64; 3]) -> f64 {
61    (v[0] * v[0] + v[1] * v[1] + v[2] * v[2]).sqrt()
62}
63
64/// `np.degrees`.
65fn degrees(x: f64) -> f64 {
66    x * (180.0 / std::f64::consts::PI)
67}
68
69/// Python's `max(a, b)`.
70fn pymax(a: f64, b: f64) -> f64 {
71    if b > a {
72        b
73    } else {
74        a
75    }
76}
77
78/// `values[:end]` with Python's slice bounds.
79pub(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
85/// `str(x)` of a Python float.
86pub(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
96/// Python's `repr()` of a string.
97pub(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
118/// `f32` rounding of a point, as NumPy's `astype(np.float32)`.
119fn 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// --------------------------------------------------------------------- config
124
125/// Reading bounds and RiVLib options (`CoregConfig.riegl_options`).
126#[derive(Debug, Clone)]
127pub struct ReadingOptions {
128    /// RiVLib directory or library file to load for `.rxp`; `None` finds it
129    /// the usual way.
130    pub library: Option<PathBuf>,
131    /// Drop RiVLib's pseudo echoes (flag bit 4) when reading `.rxp`.
132    pub drop_pseudo_echoes: bool,
133    /// Echo selection for `.rxp`: `"all"`, `"first"`, `"last"` or `"single"`.
134    pub echoes: String,
135    /// Keep every `stride`-th point (1 = all); splits multi-echo pulses.
136    pub stride: usize,
137    /// Keep every `shot_stride`-th laser pulse with all its echoes (1 = all).
138    pub shot_stride: usize,
139    /// `(min, max)` of range (m, from the scanner), deviation, reflectance
140    /// and amplitude, in that order; `None` leaves a side open.
141    pub bounds: [(Option<f64>, Option<f64>); 4],
142    /// Whether any option was given (a reason then says "after filtering").
143    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
152/// The attributes the reading bounds apply to, in [`ReadingOptions::bounds`] order.
153pub const GATES: [&str; 4] = ["range", "deviation", "reflectance", "amplitude"];
154
155/// Settings of the whole pipeline (`sylva.coreg.CoregConfig`).
156#[derive(Debug, Clone)]
157pub struct CoregConfig {
158    /// Cell size (m) of each scan's raster terrain model.
159    pub ground_cell_size: f64,
160    /// Least fraction (0-1) of the elevations from -30 to +5 degrees an azimuth
161    /// of the scan must sample for the terrain in that direction to be fitted
162    /// from it ([`refit_visible_ground`]); `None` fits the terrain from every
163    /// return. Tilted scans sample almost none along their tilt axis.
164    pub ground_min_coverage: Option<f64>,
165    /// Stem detection, in the detector's coregistration mode.
166    pub stems: StemParams,
167    /// Settings of the global stem-map match that gives a pair its coarse transform.
168    pub matching: MatchParams,
169    /// Settings of the ICP that refines each pair.
170    pub icp: IcpConfig,
171    /// Voxel size (m) the ICP subsample of each scan is thinned to.
172    pub icp_voxel: f64,
173    /// Planarity gate (0-1) on the ICP points of every scan; 0 keeps foliage,
174    /// which costs accuracy and can trap ICP in a local minimum.
175    pub icp_min_planarity: f64,
176    /// Returns higher above ground than this (m) are left out of ICP: the upper
177    /// canopy is the least repeatable part of the scene and the most moved by wind.
178    pub icp_max_height: f64,
179    /// Try reflective targets before stems where both scans saw at least
180    /// `min_reflector_matches`; far more precise, but they usually cover only
181    /// part of a survey.
182    pub use_reflectors: bool,
183    /// Targets both scans must hold, and a reflector match must bring into
184    /// agreement, for it to be attempted and to succeed; never fewer than 3.
185    pub min_reflector_matches: i64,
186    /// Distance (m) within which a transformed target counts as matched; also
187    /// how far ICP may move from a trusted reflector pose before the targets'
188    /// own pose is kept instead.
189    pub reflector_tolerance: f64,
190    /// A reflector match with at least this many targets, agreeing within
191    /// `trusted_reflector_rmse`, is accepted even when ICP then fails its
192    /// tests; 0 always requires ICP to agree.
193    pub trusted_reflector_matches: i64,
194    /// RMS target disagreement (m) a reflector match must stay under to be trusted.
195    pub trusted_reflector_rmse: f64,
196    /// Stem correspondences a coarse match needs to reach ICP; permissive,
197    /// since ICP fitness decides.
198    pub min_match_inliers: i64,
199    /// A coarse match whose best distinctly different rival has more than this
200    /// share (0-1) of its inliers is ambiguous: both are refined by ICP and the
201    /// pair is accepted only if one fits the above-ground points clearly better.
202    pub max_match_ambiguity: f64,
203    /// Factor by which the winner's above-ground fitness must exceed its
204    /// rival's for an ambiguous match to be accepted.
205    pub ambiguity_margin: f64,
206    /// Largest RMS scatter (m) of a coarse match's own stem correspondences.
207    pub max_match_rmse: f64,
208    /// Cap (m) on the median horizontal disagreement of the matched stems under
209    /// the coarse transform; infinite (the default) disables it.
210    pub max_coarse_stem_rmse: f64,
211    /// Take a stem match's vertical offset from the two terrain models where
212    /// both saw ground rather than from the matched stems, whose heights rest
213    /// on the terrain exactly where it is least certain.
214    pub height_from_ground: bool,
215    /// Terrain cells further than this (m) from a scanner are left out of
216    /// height comparisons: there ground returns are sparse and grazing and the
217    /// model is mostly understorey.
218    pub ground_radius: f64,
219    /// Shared observed terrain cells a height comparison needs.
220    pub min_ground_cells: i64,
221    /// A pair whose terrain models still differ by more than this (m, median
222    /// over the shared ground) after ICP is refused; `None` disables the test.
223    pub max_ground_disagreement: Option<f64>,
224    /// Pairs whose approximate scanner positions are further apart (m) are not tried.
225    pub max_pair_distance: f64,
226    /// Match every pair's stems first and run ICP only on the survivors.
227    pub screen_pairs: bool,
228    /// 0 keeps every pair (Python's None and 0 alike).
229    pub max_pairs_per_scan: i64,
230    /// Least ICP fitness (0-1, over all source points) of an accepted pair; it
231    /// falls with the distance between scans as less of each is shared.
232    pub min_icp_fitness: f64,
233    /// Largest ICP inlier RMSE (m) of an accepted pair.
234    pub max_icp_rmse: f64,
235    /// Least fitness (0-1) over source points higher than `fitness_min_height`:
236    /// ground is planar and fits under any horizontal shift, so stems, branches
237    /// and logs must agree too.
238    pub min_icp_fitness_above_ground: f64,
239    /// Height above ground (m) from which points count in the above-ground
240    /// fitness; <= 0 skips that test.
241    pub fitness_min_height: f64,
242    /// If ICP leaves the matched stems this much (m) further apart than the
243    /// coarse match did, the coarse transform is kept: ICP slid somewhere unrelated.
244    pub stem_agreement_tolerance: f64,
245    /// ICP moving further than this (m) from the coarse solution has diverged,
246    /// and the pair is refused.
247    pub max_coarse_to_fine_shift: f64,
248    /// Retry each scan left unregistered against the combined registered
249    /// survey: it can overlap several registered scans partly without
250    /// overlapping any one of them enough.
251    pub recover_unregistered: bool,
252    /// Passes of that recovery, each able to tie in scans the previous one
253    /// registered; treated as at least 1.
254    pub recovery_rounds: i64,
255    /// Nearest registered scans whose points are merged into the target a
256    /// recovery attempt is registered against.
257    pub recovery_neighbours: i64,
258    /// Refine every registered pose in one joint Gauss-Newton over the
259    /// point-to-plane correspondences of every accepted pair plus the matched
260    /// stems ([`crate::coreg::refine::refine_joint`]).
261    pub refine_multiview: bool,
262    /// Associations per level of that refinement; treated as at least 1.
263    pub refinement_rounds: i64,
264    /// Voxel sizes (m) of its levels, coarse to fine.
265    pub refinement_voxel_sizes: Vec<f64>,
266    /// Correspondence cut-off (m) at each of those levels.
267    pub refinement_max_distances: Vec<f64>,
268    /// Cap on the points of each scan at each level, a seeded random subset beyond it.
269    pub refinement_points_per_scan: usize,
270    /// Share (0-1) of each pair's weight carried by its matched stems.
271    pub refinement_stem_weight: f64,
272    /// Horizontal radius (m) within which mutually nearest stems of a pair are
273    /// tied together.
274    pub refinement_stem_radius: f64,
275    /// A voxel needs at least this many points for its centroid to take part.
276    pub refinement_min_voxel_points: usize,
277    /// A refinement moving any scan further than this (m) is discarded.
278    pub refinement_max_shift: f64,
279    /// Solve all poses together over the pose graph of accepted pairs; off, the
280    /// poses stay as the spanning tree over the edges placed them.
281    pub optimise_globally: bool,
282    /// Index of the scan whose levelled frame is the world frame, held fixed;
283    /// clamped into range, and replaced if that scan was set aside or no
284    /// accepted pair touches it.
285    pub reference_scan: i64,
286    /// Drop accepted pairs whose measured transform the rest of the graph
287    /// contradicts (solve-and-reject passes).
288    pub reject_outlier_edges: bool,
289    /// ICP correspondences counted as one independent observation when
290    /// weighting pose-graph edges: neighbouring residuals share the error of
291    /// the surface they sample.
292    pub information_patch_points: f64,
293    /// Floor (m) on the point-to-plane residual that weights an edge.
294    pub information_min_sigma: f64,
295    /// With priors: results putting a scanner further than this (m) from its
296    /// prior position are refused.
297    pub max_prior_shift: f64,
298    /// With priors: if set, results rotated more than this (degrees) from the
299    /// prior are refused too; `None` by default, since recorded headings can
300    /// be wrong by any amount.
301    pub max_prior_rotation: Option<f64>,
302    /// Scans prepared and pairs registered at once; 0 picks from the cores
303    /// and free memory.
304    pub workers: i64,
305    /// 0 leaves memory out of the choice of workers.
306    pub memory_per_worker_gb: f64,
307    /// RiSCAN PRO's RXP import filter applied when reading
308    /// ([`crate::riscan::riscan_like_mask`]): `"none"`, `"current"` (drops
309    /// echoes within 0.5 m of the scanner) or `"legacy"` (also the weak,
310    /// isolated echoes the older conversion discarded).
311    pub riscan_filter: String,
312    /// Bounds and RiVLib options applied while reading each scan file.
313    pub reading: ReadingOptions,
314    /// Scans with fewer points (after the reading filters) are set aside
315    /// rather than processed.
316    pub min_points_per_scan: i64,
317    /// Most points a scan is prepared from, after the reading filters; a
318    /// larger scan is thinned by an even stride through its recording order
319    /// ([`stride_subsample`]), so the whole rotation stays covered. 0 reads
320    /// every point (Python's None and 0 alike). Only preparation is capped:
321    /// [`crate::coreg::survey::merge_clouds`] always merges every point.
322    pub max_points_per_scan: usize,
323}
324
325impl Default for CoregConfig {
326    /// The settings a survey runs with unless the caller changes them.
327    ///
328    /// These are the values the published interface documents, chosen on TLS
329    /// plots of the kind the package was written for; a caller overrides the
330    /// few it cares about and leaves the rest - `..Default::default()`. They
331    /// live here rather than in the Python wrapper so the R bindings and the
332    /// command line get the same run from the same request.
333    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// ---------------------------------------------------------------------- scans
396
397/// Everything later stages need from one scan (`ScanFeatures`).
398#[derive(Debug, Clone)]
399pub struct ScanFeatures {
400    /// Scan position name (the file's stem when none was given for a path).
401    pub name: String,
402    /// Points the scan was prepared from, after the reading filters.
403    pub n_points: usize,
404    /// Raster terrain model of the scan in its levelled frame; `None` for an
405    /// unusable scan.
406    pub ground: Option<GroundModel>,
407    /// Stems, best first.
408    pub stems: Vec<StemRecord>,
409    /// Name the stem map is saved under (the scan's name).
410    pub stem_map_name: String,
411    /// Planar subsample for ICP; float32 values held as f64.
412    pub icp_points: Vec<Point>,
413    /// Retro-reflective targets seen from this scan, in its levelled frame.
414    pub reflectors: Vec<Reflector>,
415    /// Height above ground of each ICP point.
416    pub icp_heights: Vec<f32>,
417    /// `level_from_scan` applied to the raw points first.
418    pub levelling: Mat4,
419    /// Scanner position in the levelled frame.
420    pub origin: Point,
421    /// Path of the file the scan was read from; `None` for points given directly.
422    pub source: Option<String>,
423    /// Wall-clock time (s) spent preparing the scan.
424    pub seconds: f64,
425    /// Why the scan is unusable; empty if it is fine.
426    pub error: String,
427}
428
429impl ScanFeatures {
430    /// Can the scan take part in registration at all?
431    pub fn usable(&self) -> bool {
432        self.error.is_empty() && !self.icp_points.is_empty()
433    }
434
435    /// Where the scanner stood, given this scan's `world_from_scan`.
436    pub fn location(&self, pose: &Mat4) -> Point {
437        transform_points(pose, &[self.origin])[0]
438    }
439
440    /// Stem positions, `(x, y, z)` at the reference height.
441    pub fn stem_positions(&self) -> Vec<Point> {
442        self.stems.iter().map(|s| [s.x, s.y, s.z]).collect()
443    }
444
445    /// The stem map as the matcher takes it.
446    pub fn stem_map(&self) -> StemMap {
447        stem_map(&self.stems)
448    }
449}
450
451/// Positions, diameters and qualities of stems, for [`match_stem_maps`].
452pub 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
456/// Stems moved by a rigid transform (`StemMap.transformed`).
457pub 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/// What a scan is prepared from.
464#[derive(Debug, Clone)]
465pub enum ScanInput {
466    /// A file: `.rxp` through RiVLib, anything else through [`crate::io::read`].
467    Path(PathBuf),
468    /// Points in the scan's own frame.
469    Points(Vec<Point>),
470    /// An input that could not even be converted: set aside with this reason.
471    Failed {
472        /// Why the input could not be converted.
473        reason: String,
474        /// Path of the file it came from, if any.
475        source: Option<PathBuf>,
476    },
477}
478
479/// Why a scan could not be read or prepared.
480#[derive(Debug)]
481pub enum PrepareError {
482    /// A reading bound or filter needs an attribute the file does not carry
483    /// (the Python package's `KeyError`).
484    MissingAttribute(String),
485    /// Any other error of the core library (reading the file, fitting the
486    /// ground, ...).
487    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    /// `f"{type(exc).__name__}: {exc}"` of the exception the Python package raises.
509    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
517/// The Python exception a core error is raised as (the bindings' mapping).
518pub 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
529/// Python's `Path.stem`.
530pub 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
538/// Points of a scan file in its own frame, filtered for coregistration
539/// (`_read_scan`): RiSCAN's import filter on the whole stream first, then
540/// the closed intervals on range (from the scanner), deviation, reflectance
541/// and amplitude, then the cap on the number of points
542/// (`max_points_per_scan`, an even stride through the file: [`stride_subsample`]).
543pub 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            // coregistration reads many scans thinned by stride; plain zone-1 reading keeps it fast
549            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; // RIEGL's "not measured"
580                    }
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
603/// `count` of `points` spread evenly through their order (point
604/// `floor(i * n / count)` for `i` in `0..count`), or all of them if there are
605/// no more than `count`.
606///
607/// A scan file is in recording order, so its first `count` points are one
608/// wedge of the rotation; an even stride keeps the whole field of view at a
609/// lower density, and is the same every run.
610pub 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
622/// Heights above ground of points, as float32 (`GroundModel.normalise(..., dtype=float32)`).
623pub 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
629/// A placeholder for a scan that cannot be registered (`_unusable_scan`).
630pub 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
648/// Stems of one scan, best first, with `z` on the scan's own terrain
649/// (`sylva.coreg.detect_stems` with heights given).
650pub 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
670/// Fit the ground, detect stems and build the ICP subsample of one scan.
671///
672/// `name` empty takes the file's stem for a path. `levelling` rotates the
673/// points and targets first; `origin` is the scanner's position in the
674/// levelled cloud (the origin if `None`), and with it, or for a scan read
675/// from file, the terrain is refitted without the directions the scanner
676/// could not see.
677pub 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
732/// Fewest returns the terrain refit of [`refit_visible_ground`] is made from;
733/// with fewer left (the returns steeper than 45 degrees below the scanner and
734/// those at sighted azimuths), the first fit stands.
735const REFIT_MIN_RETURNS: usize = 100;
736
737/// The terrain refitted without the directions in which the scanner could
738/// not see it (`_refit_visible_ground`), or `None` where every azimuth, or
739/// none, is blind, or fewer than 100 returns would be left
740/// to fit, and the fit stands.
741///
742/// An azimuth (1 degree) is blind when the scan sampled less than
743/// `ground_min_coverage` of the elevations from -30 to +5 degrees in it;
744/// blind azimuths, widened by 3 degrees either side, keep only their returns
745/// more than 45 degrees below the horizon.
746pub 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    // Scattered blind azimuths, once widened, can cover the whole circle and
770    // leave nothing (an upright scanner sees nothing 45 degrees down): a
771    // terrain cannot be fitted to that, so the fit from every return stands.
772    if kept.len() < REFIT_MIN_RETURNS {
773        return Ok(None);
774    }
775    Ok(Some(fit_ground(&kept, &ground_params(cfg))?))
776}
777
778// ------------------------------------------------------------------- terrain
779
780/// Observed terrain cells within `radius` of the scanner, in the scan's
781/// frame, as `(x, y, height)` (`_terrain_samples`).
782pub 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
799/// Median height of the targets' terrain over the source's where both saw
800/// ground (`_height_offset`): metres to add to the source's height, NaN
801/// with fewer than `min_ground_cells` shared cells.
802///
803/// `targets` are `(scan, world_from_scan)` of the scans compared against,
804/// `world_from_source` the source's pose (or the pair transform, the
805/// target's frame being the world).
806pub 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
837/// `transform` with its height set from the terrain (`_on_ground`), when
838/// `height_from_ground` and the scans share enough ground.
839pub 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
852/// Does the terrain disagree beyond `max_ground_disagreement`?
853pub 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
857/// Median horizontal distance between matched stems under `transform`; NaN
858/// without matches (`_stem_median_residual`).
859pub 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
875/// ICP fitness over source points more than `fitness_min_height` up; NaN
876/// ("no evidence") if heights are missing or fewer than 100 points qualify
877/// (`_above_ground_fitness`).
878pub 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
890/// Does `pose` put the scanner where its prior says it stood (`_prior_ok`)?
891/// `(false, why)` if it is more than `max_prior_shift` away, or rotated more
892/// than `max_prior_rotation` degrees from it.
893pub 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// ---------------------------------------------------------------------- pairs
910
911/// Registration of one pair (`PairResult`); `transform` maps scan `i` into
912/// scan `j`.
913#[derive(Debug, Clone)]
914pub struct PairResult {
915    /// Index of the source scan, whose points `transform` moves.
916    pub i: i64,
917    /// Index of the target scan.
918    pub j: i64,
919    /// Name of scan `i`.
920    pub name_i: String,
921    /// Name of scan `j`.
922    pub name_j: String,
923    /// `j_from_i`, the transform the pair reports: ICP's, or the coarse one
924    /// when `used_icp` is false; identity until something was tried.
925    pub transform: Mat4,
926    /// The transform ICP started from: the reflector match's, or the stem
927    /// match's with its height set from the two terrain models.
928    pub coarse_transform: Mat4,
929    /// Global stem-map match behind the coarse transform; `None` when targets
930    /// were used or stems were never matched.
931    pub stem_match: Option<StemMatch>,
932    /// Alignment of the shared retro-reflective targets, where both scans had
933    /// enough and it succeeded; else `None`.
934    pub reflector_match: Option<ReflectorMatch>,
935    /// ICP outcome; when `used_icp` is false it scores the coarse transform,
936    /// not the refinement that was discarded.
937    pub icp: Option<IcpResult>,
938    /// Whether the pair passed every test and becomes a pose-graph edge.
939    pub success: bool,
940    /// Why the pair failed, or how it was accepted; empty while undecided.
941    pub reason: String,
942    /// Wall-clock time (s) spent registering the pair.
943    pub seconds: f64,
944    /// Positions of the matched stems in each scan's own frame.
945    pub matched_source: Vec<Point>,
946    /// The same stems in the target scan's frame, row for row with `matched_source`.
947    pub matched_target: Vec<Point>,
948    /// Median horizontal distance (m) between matched stems under the coarse
949    /// transform; NaN until known.
950    pub coarse_stem_rmse: f64,
951    /// The same under the ICP transform; NaN if ICP did not run or fewer than
952    /// 3 stems matched.
953    pub fine_stem_rmse: f64,
954    /// ICP fitness over source points above ground; NaN if unknown.
955    pub fitness_above: f64,
956    /// A distinctly different coarse hypothesis nearly as well supported
957    /// (`ambiguity` above `max_match_ambiguity`), resolved by ICP; else `None`.
958    pub rival: Option<StemMatch>,
959    /// False when the coarse transform was kept over ICP's.
960    pub used_icp: bool,
961    /// Median height (m) of the target's terrain over the source's where both
962    /// saw ground, under `transform`; NaN if they share too little.
963    pub ground_offset: f64,
964    /// Accepted on its reflector match although ICP failed.
965    pub trusted: bool,
966}
967
968impl PairResult {
969    /// An undecided pair of scans `i` and `j`: identity transforms, no match,
970    /// NaN statistics.
971    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    /// ICP fitness (0-1) of the reported transform; 0 when ICP did not run.
998    pub fn fitness(&self) -> f64 {
999        self.icp.as_ref().map_or(0.0, |r| r.fitness)
1000    }
1001
1002    /// ICP inlier RMSE (m); infinite when ICP did not run.
1003    pub fn rmse(&self) -> f64 {
1004        self.icp.as_ref().map_or(f64::INFINITY, |r| r.inlier_rmse)
1005    }
1006
1007    /// Stems the coarse stem match paired; 0 without one.
1008    pub fn n_stem_matches(&self) -> usize {
1009        self.stem_match.as_ref().map_or(0, |m| m.n_inliers)
1010    }
1011
1012    /// Stem agreement of the transform this pair reports.
1013    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    /// One line for the report (`PairResult.summary`).
1022    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
1044/// Store the positions of the matched stems (`_attach_matched_stems`).
1045fn 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
1053/// Screen a coarse match before paying for ICP (`_coarse_is_acceptable`);
1054/// sets `reason` on failure.
1055fn 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
1081/// A screened pair's probe (`_screen`): the matched stems attached and the
1082/// coarse match judged; whether it is worth ICP on its stems.
1083pub 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
1088/// ICP from `initial` against `target`'s points or its prepared pyramid.
1089fn 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
1096/// The settings `plane_information` is computed with: the ICP's, but always
1097/// point-to-plane, as the Python binding builds them.
1098fn 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
1102/// ICP-style quality of a transform ICP did not produce (`_score`).
1103fn 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        // A pyramid prepared for point-to-point ICP has no normals: the
1108        // information is always point-to-plane, so that pyramid is built here.
1109        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
1115/// Accept a pair ICP refused if its reflector match is strong enough alone
1116/// (`_trust_reflectors`).
1117pub 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
1137/// Refine a coarse transform with ICP and decide whether to accept it
1138/// (`_refine_and_judge`).
1139///
1140/// Every test, and later the pose-graph edge, judges the transform the pair
1141/// reports: when the stem cross-check keeps the coarse transform, it is
1142/// scored afresh rather than by the ICP it replaced.
1143pub 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); // two ICPs against it
1149            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    // Cross-check ICP against the stems it was meant to refine.
1159    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        // An ambiguous stem pattern: refine the rival too and keep whichever
1170        // fits the above-ground points better, if the margin is clear.
1171        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        // Two hypotheses ICP pulls to the same pose were never rivals.
1177        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
1243/// A coarse transform by one method, or `None` (`_coarse_transform`).
1244fn 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/// Coarse-match then ICP-refine one pair of prepared scans (`register_pair`).
1270///
1271/// With `initial`, ICP starts there. Otherwise reflective targets are tried
1272/// first where both scans have them, then stems (with `stem_match` if the
1273/// pair was screened already); the first that ICP accepts wins, and a
1274/// target match ICP rejects does not cost the pair its stem match.
1275/// `prepared` is `target`'s ICP pyramid, if built already.
1276#[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    // Targets first: a target is located to millimetres and three fix all
1286    // six degrees of freedom. Stems stay as a fallback, because three
1287    // targets can form a congruent triangle by coincidence.
1288    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
1313/// `(fitness, rmse, correspondences)` weighting a pair's pose-graph edge
1314/// (`_edge_quality`): a trusted reflector pair that kept the targets' pose
1315/// is weighted by the targets' own residual and count.
1316pub 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
1325/// Information of a pair's edge from its point-to-plane correspondences, or
1326/// `None` (the default information) without them (`_edge_information`).
1327pub 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
1335// ------------------------------------------------------------ shared targets
1336
1337/// The ICP pyramids of the most recently used targets (`_TargetCache`).
1338///
1339/// Each pyramid is built once, outside the lock, by whichever thread asks
1340/// first; the others wait for it. The pyramid depends only on the target
1341/// and the settings, so the cache changes no result.
1342pub 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    /// A cache over `scans` for pyramids built with `config`, keeping the
1351    /// `capacity` (at least 1) most recently used.
1352    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    /// The pyramid of scan `k`, built on first use and marked most recently
1357    /// used; `get()` on the returned cell is always `Some`.
1358    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        // A negative tolerance makes the stem cross-check keep the coarse
1456        // transform, which is then scored: with a pyramid prepared for
1457        // point-to-point ICP that used to fail the pair, and the survey with it.
1458        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        // Through a file: the cap reaches the far end of the recording.
1479        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        // Flat ground seen from 1.6 m at 3-30 m (elevations -28 to -3 degrees,
1495        // under 80 % of the band) and a thin pole every 6 degrees filling the
1496        // band there: the blind azimuths, widened, cover the circle, and an
1497        // upright scanner has nothing 45 degrees down to keep.
1498        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}