pub mod prune;
pub mod stems;
pub mod touch;
use std::collections::HashMap;
use rayon::prelude::*;
use crate::cluster::{dijkstra_gravity, dijkstra_scaled, directed_knn_graph, directed_knn_graph_bridged, Bend};
use crate::error::{Error, Result};
use crate::filters::Rng;
use crate::util::spatial::first_in_voxel;
use crate::util::optim::levenberg_marquardt;
use crate::util::spatial::KdTree;
use crate::Point;
#[derive(Debug, Clone, PartialEq)]
pub struct Tree {
pub tree_id: i64,
pub x: f64,
pub y: f64,
pub dbh: f64,
pub height: f64,
pub n_points: usize,
pub inlier_fraction: f64,
pub n_slices: usize,
pub rmse: f64,
pub lean_deg: f64,
pub quality: f64,
}
fn centroid_xy(xy: &[[f64; 2]]) -> [f64; 2] {
let n = xy.len().max(1) as f64;
[xy.iter().map(|p| p[0]).sum::<f64>() / n, xy.iter().map(|p| p[1]).sum::<f64>() / n]
}
pub fn fit_circle_algebraic(xy: &[[f64; 2]]) -> Result<(f64, f64, f64)> {
if xy.len() < 3 {
return Err(Error::invalid("need >= 3 points"));
}
let m = centroid_xy(xy);
let local: Vec<[f64; 2]> = xy.iter().map(|p| [p[0] - m[0], p[1] - m[1]]).collect();
let (cx, cy, r) = fit_circle_algebraic_local(&local)?;
Ok((cx + m[0], cy + m[1], r))
}
fn fit_circle_algebraic_local(xy: &[[f64; 2]]) -> Result<(f64, f64, f64)> {
let mut ata = nalgebra::Matrix3::<f64>::zeros();
let mut atb = nalgebra::Vector3::<f64>::zeros();
for p in xy {
let row = nalgebra::Vector3::new(p[0], p[1], 1.0);
ata += row * row.transpose();
atb += row * (p[0] * p[0] + p[1] * p[1]);
}
let s = ata.lu().solve(&atb).ok_or_else(|| Error::invalid("degenerate circle fit"))?;
let cx = s[0] / 2.0;
let cy = s[1] / 2.0;
let r = (s[2] + cx * cx + cy * cy).max(0.0).sqrt();
Ok((cx, cy, r))
}
pub fn fit_circle(xy: &[[f64; 2]]) -> Result<(f64, f64, f64, f64)> {
if xy.len() < 3 {
return Err(Error::invalid("need >= 3 points"));
}
let m = centroid_xy(xy);
let local: Vec<[f64; 2]> = xy.iter().map(|p| [p[0] - m[0], p[1] - m[1]]).collect();
let (cx, cy, r, rmse) = fit_circle_local(&local)?;
Ok((cx + m[0], cy + m[1], r, rmse))
}
fn fit_circle_local(xy: &[[f64; 2]]) -> Result<(f64, f64, f64, f64)> {
let (cx, cy, r) = fit_circle_algebraic_local(xy)?;
let res = levenberg_marquardt(
|p, out| {
out.clear();
out.extend(xy.iter().map(|q| ((q[0] - p[0]).hypot(q[1] - p[1])) - p[2]));
},
&[cx, cy, r],
50,
1e-10,
);
Ok((res.x[0], res.x[1], res.x[2].abs(), res.rmse))
}
fn circle_through_3(a: [f64; 2], b: [f64; 2], c: [f64; 2]) -> Option<(f64, f64, f64)> {
let d = 2.0 * (a[0] * (b[1] - c[1]) + b[0] * (c[1] - a[1]) + c[0] * (a[1] - b[1]));
if d.abs() < 1e-12 {
return None;
}
let (a2, b2, c2) = (a[0] * a[0] + a[1] * a[1], b[0] * b[0] + b[1] * b[1], c[0] * c[0] + c[1] * c[1]);
let ux = (a2 * (b[1] - c[1]) + b2 * (c[1] - a[1]) + c2 * (a[1] - b[1])) / d;
let uy = (a2 * (c[0] - b[0]) + b2 * (a[0] - c[0]) + c2 * (b[0] - a[0])) / d;
Some((ux, uy, (a[0] - ux).hypot(a[1] - uy)))
}
#[derive(Debug, Clone)]
pub struct RansacCircleParams {
pub threshold: f64,
pub iterations: usize,
pub min_radius: f64,
pub max_radius: f64,
pub seed: u64,
}
impl Default for RansacCircleParams {
fn default() -> Self {
RansacCircleParams { threshold: 0.01, iterations: 200, min_radius: 0.02, max_radius: 1.5, seed: 0 }
}
}
pub fn fit_circle_ransac(xy: &[[f64; 2]], p: &RansacCircleParams) -> Result<(f64, f64, f64, Vec<bool>)> {
let n = xy.len();
if n < 3 {
return Err(Error::invalid("need >= 3 points"));
}
let m = centroid_xy(xy);
let local: Vec<[f64; 2]> = xy.iter().map(|q| [q[0] - m[0], q[1] - m[1]]).collect();
let xy = &local[..];
let mut rng = Rng::new(p.seed);
let mut best: Vec<bool> = Vec::new();
let mut best_n = 0usize;
for _ in 0..p.iterations {
let (i, j, k) = (rng.below(n), rng.below(n), rng.below(n));
if i == j || j == k || i == k {
continue;
}
let Some((cx, cy, r)) = circle_through_3(xy[i], xy[j], xy[k]) else { continue };
if r < p.min_radius || r > p.max_radius {
continue;
}
let cnt = xy.iter().filter(|q| (((q[0] - cx).hypot(q[1] - cy)) - r).abs() < p.threshold).count();
if cnt > best_n {
best_n = cnt;
best = xy.iter().map(|q| (((q[0] - cx).hypot(q[1] - cy)) - r).abs() < p.threshold).collect();
}
}
if best_n < 3 {
return Err(Error::invalid("RANSAC found no valid circle"));
}
let inl: Vec<[f64; 2]> = xy.iter().zip(&best).filter(|(_, &b)| b).map(|(q, _)| *q).collect();
let (cx, cy, r, _) = fit_circle(&inl)?;
Ok((cx + m[0], cy + m[1], r, best))
}
pub use crate::trees::stems::{detect_stems, StemParams};
pub fn dbh_profile(points: &[Point], heights: &[f64], cx: f64, cy: f64, at_heights: &[f64], slice_thickness: f64, search_radius: f64) -> Vec<f64> {
let ransac = RansacCircleParams::default();
at_heights
.iter()
.map(|&hh| {
let xy: Vec<[f64; 2]> = (0..points.len())
.filter(|&i| (heights[i] - hh).abs() <= slice_thickness / 2.0 && (points[i][0] - cx).hypot(points[i][1] - cy) <= search_radius)
.map(|i| [points[i][0], points[i][1]])
.collect();
if xy.len() < 10 {
return f64::NAN;
}
fit_circle_ransac(&xy, &ransac).map(|(_, _, r, _)| 2.0 * r).unwrap_or(f64::NAN)
})
.collect()
}
#[derive(Debug, Clone)]
pub struct SegmentParams {
pub k: usize,
pub max_edge: f64,
pub voxel_size: f64,
pub seed_height: f64,
pub seed_radius: f64,
pub power: f64,
pub bend_cost: bool,
pub bend_power: f64,
pub bend_floor: f64,
pub path_weight: f64,
pub gravity: f64,
pub ground_clearance: f64,
pub height_prior: bool,
pub height_prior_radius: f64,
pub seed_ring: bool,
pub height_prior_power: f64,
pub low_height: f64,
pub low_radius: f64,
pub understorey_height: f64,
pub understorey_band: f64,
pub voxel_origin: Option<Point>,
pub bridge: bool,
}
impl SegmentParams {
fn bend(&self) -> Option<Bend> {
self.bend_cost.then_some(Bend { power: self.bend_power, floor: self.bend_floor, path_weight: self.path_weight })
}
fn graph<'a>(&self, work: &'a [Point]) -> crate::cluster::Graph<'a> {
if self.bridge {
directed_knn_graph_bridged(work, self.k, self.max_edge, self.power, self.bend())
} else {
directed_knn_graph(work, self.k, self.max_edge, self.power, self.bend())
}
}
}
impl Default for SegmentParams {
fn default() -> Self {
SegmentParams { k: 6, max_edge: 1.0, voxel_size: 0.03, seed_height: 1.5, seed_radius: 0.25, power: 6.0, bend_cost: true, bend_power: 4.0, bend_floor: 0.4, path_weight: 0.3, gravity: 0.0, ground_clearance: 0.1, seed_ring: true, height_prior: true, height_prior_radius: 1.5, height_prior_power: 1.0, low_height: 0.5, low_radius: 1.0, understorey_height: 0.0, understorey_band: 0.5, voxel_origin: None, bridge: true }
}
}
pub fn graph_nodes(points: &[Point], heights: &[f64], p: &SegmentParams) -> Vec<usize> {
graph_nodes_masked(points, heights, p, None)
}
pub fn graph_nodes_masked(points: &[Point], heights: &[f64], p: &SegmentParams, exclude: Option<&[bool]>) -> Vec<usize> {
let above: Vec<usize> = (0..points.len()).filter(|&i| heights[i] >= p.ground_clearance && exclude.is_none_or(|e| !e[i])).collect();
if p.voxel_size.is_nan() || p.voxel_size <= 0.0 {
return above;
}
let origin = p.voxel_origin.unwrap_or_else(|| {
let lo = crate::util::spatial::min_corner(points);
[lo[0].floor(), lo[1].floor(), lo[2].floor()]
});
first_in_voxel(points, above, &origin, p.voxel_size)
}
pub fn segment_trees(points: &[Point], heights: &[f64], trees: &[Tree], p: &SegmentParams) -> Vec<i64> {
segment_trees_masked(points, heights, trees, p, None)
}
pub fn segment_trees_masked(points: &[Point], heights: &[f64], trees: &[Tree], p: &SegmentParams, exclude: Option<&[bool]>) -> Vec<i64> {
let task = crate::util::progress::start("segmenting trees", 3);
let work_idx = graph_nodes_masked(points, heights, p, exclude);
let work: Vec<Point> = work_idx.iter().map(|&i| points[i]).collect();
let hw: Vec<f64> = work_idx.iter().map(|&i| heights[i]).collect();
task.inc(1); let labels_work = segment_nodes(&work, &hw, trees, p);
task.inc(1); let labels = label_points_masked(points, heights, &work, &labels_work, trees, p, exclude);
task.inc(1); labels
}
pub struct Seeds {
pub seeds: Vec<usize>,
pub seed_tree: Vec<i64>,
pub seed_xy: Vec<[f64; 2]>,
pub seed_scale: Vec<f64>,
}
struct FlatGrid {
cell: f64,
cells: Vec<[i64; 2]>,
starts: Vec<usize>,
order: Vec<u32>,
}
impl FlatGrid {
fn key(x: f64, y: f64, cell: f64) -> [i64; 2] {
[(x / cell).floor() as i64, (y / cell).floor() as i64]
}
fn new(points: &[Point], cell: f64) -> FlatGrid {
let mut keyed: Vec<([i64; 2], u32)> = points.iter().enumerate().map(|(i, q)| (Self::key(q[0], q[1], cell), i as u32)).collect();
keyed.par_sort_unstable();
let mut cells = Vec::new();
let mut starts = Vec::new();
let mut order = Vec::with_capacity(keyed.len());
for (k, i) in keyed {
if cells.last() != Some(&k) {
cells.push(k);
starts.push(order.len());
}
order.push(i);
}
starts.push(order.len());
FlatGrid { cell, cells, starts, order }
}
fn candidates(&self, x: f64, y: f64, r: f64) -> impl Iterator<Item = usize> + '_ {
let pad = r * 1e-9 + 1e-9;
let lo = Self::key(x - r - pad, y - r - pad, self.cell);
let hi = Self::key(x + r + pad, y + r + pad, self.cell);
(lo[0]..=hi[0])
.flat_map(move |cx| (lo[1]..=hi[1]).map(move |cy| [cx, cy]))
.filter_map(move |k| self.cells.binary_search(&k).ok())
.flat_map(move |slot| self.order[self.starts[slot]..self.starts[slot + 1]].iter().map(|&i| i as usize))
}
}
pub fn collect_seeds(work: &[Point], hw: &[f64], trees: &[Tree], p: &SegmentParams) -> Seeds {
let mut out = Seeds { seeds: Vec::new(), seed_tree: Vec::new(), seed_xy: Vec::new(), seed_scale: Vec::new() };
if trees.is_empty() || work.is_empty() {
return out;
}
let ring = trees.iter().map(|t| if p.seed_ring { 0.5 * t.dbh } else { 0.0 } + p.seed_radius).fold(0.0, f64::max);
let widest = if p.height_prior { ring.max(p.height_prior_radius) } else { ring };
let grid = FlatGrid::new(work, widest.max(1e-3));
let mut found: Vec<usize> = Vec::new();
for t in trees {
let scale = if p.height_prior {
let mut hs: Vec<f64> = grid.candidates(t.x, t.y, p.height_prior_radius).filter(|&i| (work[i][0] - t.x).hypot(work[i][1] - t.y) <= p.height_prior_radius).map(|i| hw[i]).collect();
hs.sort_by(|a, b| a.partial_cmp(b).unwrap());
let h95 = hs.get(((hs.len() as f64 * 0.95) as usize).min(hs.len().saturating_sub(1))).copied().unwrap_or(2.0);
1.0 / h95.max(2.0).powf(p.height_prior_power)
} else {
1.0
};
let inner = if p.seed_ring { 0.5 * t.dbh } else { 0.0 };
found.clear();
found.extend(grid.candidates(t.x, t.y, inner + p.seed_radius).filter(|&i| {
let d = (work[i][0] - t.x).hypot(work[i][1] - t.y);
(d - inner).abs() <= p.seed_radius && hw[i] < p.seed_height + 0.5
}));
found.sort_unstable();
for &i in &found {
out.seeds.push(i);
out.seed_tree.push(t.tree_id);
out.seed_xy.push([t.x, t.y]);
out.seed_scale.push(scale);
}
}
out
}
pub fn segment_nodes(work: &[Point], hw: &[f64], trees: &[Tree], p: &SegmentParams) -> Vec<i64> {
let graph = p.graph(work);
let Seeds { mut seeds, mut seed_tree, mut seed_xy, mut seed_scale } = collect_seeds(work, hw, trees, p);
if seeds.is_empty() {
return vec![-1; work.len()];
}
if p.understorey_height > 0.0 {
let stems = KdTree::new(&trees.iter().map(|t| [t.x, t.y, 0.0]).collect::<Vec<_>>());
let clear: Vec<f64> = trees.iter().map(|t| p.low_radius.max(1.5 * t.dbh)).collect();
let reach = clear.iter().cloned().fold(0.0, f64::max);
let scale = if p.height_prior { 1.0 / p.understorey_height } else { 1.0 };
for (i, q) in work.iter().enumerate() {
if hw[i] > p.ground_clearance + p.understorey_band {
continue;
}
let flat = [q[0], q[1], 0.0];
let near = stems.within(&flat, reach).into_iter().any(|(j, d)| d <= clear[j]);
if !near {
seeds.push(i);
seed_tree.push(-1);
seed_xy.push([q[0], q[1]]);
seed_scale.push(scale);
}
}
}
let (dist, src, _) = dijkstra_scaled(&graph, &seeds, Some(&seed_xy), p.gravity, if p.height_prior { Some(&seed_scale) } else { None });
(0..work.len()).map(|i| if dist[i].is_finite() { seed_tree[src[i]] } else { -1 }).collect()
}
pub fn label_points(points: &[Point], heights: &[f64], work: &[Point], labels_work: &[i64], trees: &[Tree], p: &SegmentParams) -> Vec<i64> {
label_points_masked(points, heights, work, labels_work, trees, p, None)
}
pub fn label_points_masked(points: &[Point], heights: &[f64], work: &[Point], labels_work: &[i64], trees: &[Tree], p: &SegmentParams, exclude: Option<&[bool]>) -> Vec<i64> {
let tree = KdTree::new(work);
let base: std::collections::HashMap<i64, (f64, f64, f64)> = trees.iter().map(|t| (t.tree_id, (t.x, t.y, p.low_radius.max(1.5 * t.dbh)))).collect();
(0..points.len())
.into_par_iter()
.map(|i| {
let (q, h) = (&points[i], heights[i]);
if !(h >= p.ground_clearance) || exclude.is_some_and(|e| e[i]) {
return -1;
}
let l = tree.nearest(q).map(|(j, _)| labels_work[j]).unwrap_or(-1);
if l >= 0 && h < p.low_height {
if let Some(&(x, y, r)) = base.get(&l) {
if (q[0] - x).hypot(q[1] - y) > r {
return -1;
}
}
}
l
})
.collect()
}
pub fn merge_branches(points: &[Point], heights: &[f64], trees: &[Tree], p: &SegmentParams, ground_height: f64, trunk_scale: f64, trunk_min: f64, search_radius: f64) -> (Vec<Tree>, Vec<i64>) {
let parent = branch_parents(points, heights, trees, p, ground_height, trunk_scale, trunk_min, search_radius, None);
resolve_branches(trees, &parent)
}
#[allow(clippy::too_many_arguments)]
pub fn branch_parents(points: &[Point], heights: &[f64], trees: &[Tree], p: &SegmentParams, ground_height: f64, trunk_scale: f64, trunk_min: f64, search_radius: f64, only: Option<&[bool]>) -> Vec<Option<usize>> {
let mut parent: Vec<Option<usize>> = vec![None; trees.len()];
let work_idx = graph_nodes(points, heights, p);
let work: Vec<Point> = work_idx.iter().map(|&i| points[i]).collect();
let hw: Vec<f64> = work_idx.iter().map(|&i| heights[i]).collect();
let graph = p.graph(&work);
let sources: Vec<usize> = (0..work.len()).filter(|&i| hw[i] < ground_height).collect();
if sources.is_empty() {
return parent;
}
let (dist, _, pred) = dijkstra_gravity(&graph, &sources, None, 0.0);
let traced = |ci: usize| only.is_none_or(|o| o[ci]);
let grid = FlatGrid::new(&work, p.seed_radius.max(1e-3));
let seed_node: Vec<Option<usize>> = trees
.iter()
.enumerate()
.map(|(ci, t)| {
if !traced(ci) {
return None;
}
let mut best: Option<(usize, f64)> = None;
for i in grid.candidates(t.x, t.y, p.seed_radius) {
if (work[i][0] - t.x).hypot(work[i][1] - t.y) > p.seed_radius || (hw[i] - p.seed_height).abs() > 0.5 || !dist[i].is_finite() {
continue;
}
if best.map(|b| dist[i] < b.1 || (dist[i] == b.1 && i < b.0)).unwrap_or(true) {
best = Some((i, dist[i]));
}
}
best.map(|b| b.0)
})
.collect();
for (ci, t) in trees.iter().enumerate() {
let Some(mut node) = seed_node[ci] else { continue };
let near: Vec<usize> = (0..trees.len()).filter(|&oi| oi != ci && (trees[oi].x - t.x).hypot(trees[oi].y - t.y) <= search_radius).collect();
let mut guard = 0;
while pred[node] != usize::MAX && guard < 1_000_000 {
node = pred[node];
guard += 1;
let q = work[node];
let h = hw[node];
for &oi in &near {
let o = &trees[oi];
let r = (trunk_scale * o.dbh / 2.0).max(trunk_min);
if h >= p.ground_clearance && h <= p.seed_height + 0.5 && (q[0] - o.x).hypot(q[1] - o.y) <= r {
parent[ci] = Some(oi);
}
}
if parent[ci].is_some() {
break;
}
}
}
parent
}
pub fn resolve_branches(trees: &[Tree], parent: &[Option<usize>]) -> (Vec<Tree>, Vec<i64>) {
let mut target: Vec<usize> = (0..trees.len()).collect();
let mut step_of: Vec<usize> = vec![usize::MAX; trees.len()];
let mut walk: Vec<usize> = Vec::new();
for ci in 0..trees.len() {
walk.clear();
let mut cur = ci;
let end = loop {
if step_of[cur] != usize::MAX {
let ring = &walk[step_of[cur]..];
break ring.iter().copied().fold(ring[0], |b, q| {
let (qb, qq) = (trees[b].quality, trees[q].quality);
if qq > qb || (qq == qb && q < b) { q } else { b }
});
}
step_of[cur] = walk.len();
walk.push(cur);
match parent[cur] {
Some(pi) => cur = pi,
None => break cur,
}
};
for &q in &walk {
step_of[q] = usize::MAX;
}
target[ci] = end;
}
let survivors: Vec<Tree> = trees.iter().enumerate().filter(|(i, _)| target[*i] == *i).map(|(_, t)| t.clone()).collect();
let merged_into: Vec<i64> = target.iter().map(|&ti| trees[ti].tree_id).collect();
(survivors, merged_into)
}
pub fn tree_heights(heights: &[f64], labels: &[i64], trees: &mut [Tree], percentile: f64) {
let mut slot: HashMap<i64, usize> = HashMap::new();
for (k, t) in trees.iter().enumerate() {
slot.entry(t.tree_id).or_insert(k);
}
let mut buckets: Vec<Vec<f64>> = vec![Vec::new(); trees.len()];
for (&l, &h) in labels.iter().zip(heights) {
if let Some(&k) = slot.get(&l) {
buckets[k].push(h);
}
}
for (t, k) in trees.iter_mut().zip(0..) {
let mut hs = std::mem::take(&mut buckets[slot[&t.tree_id]]);
if slot[&t.tree_id] != k {
hs = labels.iter().zip(heights).filter(|(&l, _)| l == t.tree_id).map(|(_, &h)| h).collect();
}
t.n_points = hs.len();
hs.retain(|h| !h.is_nan());
if hs.is_empty() {
t.height = f64::NAN;
continue;
}
hs.sort_by(|a, b| a.total_cmp(b));
let pos = ((percentile / 100.0) * (hs.len() - 1) as f64).round() as usize;
t.height = hs[pos.min(hs.len() - 1)];
}
}
pub fn convex_hull(xy: &[[f64; 2]]) -> Vec<[f64; 2]> {
let mut pts: Vec<[f64; 2]> = xy.iter().copied().filter(|p| p[0].is_finite() && p[1].is_finite()).collect();
pts.sort_by(|a, b| a[0].partial_cmp(&b[0]).unwrap().then(a[1].partial_cmp(&b[1]).unwrap()));
pts.dedup();
if pts.len() < 3 {
return pts;
}
let cross = |o: &[f64; 2], a: &[f64; 2], b: &[f64; 2]| (a[0] - o[0]) * (b[1] - o[1]) - (a[1] - o[1]) * (b[0] - o[0]);
let mut lower: Vec<[f64; 2]> = Vec::new();
for p in &pts {
while lower.len() >= 2 && cross(&lower[lower.len() - 2], &lower[lower.len() - 1], p) <= 0.0 {
lower.pop();
}
lower.push(*p);
}
let mut upper: Vec<[f64; 2]> = Vec::new();
for p in pts.iter().rev() {
while upper.len() >= 2 && cross(&upper[upper.len() - 2], &upper[upper.len() - 1], p) <= 0.0 {
upper.pop();
}
upper.push(*p);
}
lower.pop();
upper.pop();
lower.into_iter().chain(upper).collect()
}
pub fn polygon_area(poly: &[[f64; 2]]) -> f64 {
let mut area = 0.0;
for i in 0..poly.len() {
let j = (i + 1) % poly.len();
area += poly[i][0] * poly[j][1] - poly[j][0] * poly[i][1];
}
area.abs() / 2.0
}
pub fn in_convex_polygon(poly: &[[f64; 2]], p: [f64; 2]) -> bool {
if poly.len() < 3 {
return false;
}
(0..poly.len()).all(|i| {
let (a, b) = (poly[i], poly[(i + 1) % poly.len()]);
(b[0] - a[0]) * (p[1] - a[1]) - (b[1] - a[1]) * (p[0] - a[0]) >= -1e-12
})
}
pub fn convex_hull_area(xy: &[[f64; 2]]) -> f64 {
let hull = convex_hull(xy);
if hull.len() < 3 {
return f64::NAN;
}
polygon_area(&hull)
}
#[derive(Debug, Clone, Default, PartialEq)]
pub struct CrownShape {
pub projected_area: f64,
pub diameter: f64,
pub max_width: f64,
pub volume: f64,
pub surface: f64,
pub base_height: f64,
pub top_height: f64,
pub offset: f64,
pub offset_direction: f64,
pub asymmetry: f64,
}
pub fn crown_shape(points: &[Point], base_xy: Option<[f64; 2]>, z_min: f64, slice: f64) -> CrownShape {
let crown: Vec<&Point> = points.iter().filter(|p| p[2] >= z_min && p.iter().all(|v| v.is_finite())).collect();
if crown.len() < 3 || !(slice > 0.0) {
return CrownShape { projected_area: f64::NAN, diameter: f64::NAN, max_width: f64::NAN, volume: f64::NAN, surface: f64::NAN, base_height: z_min, top_height: f64::NAN, offset: f64::NAN, offset_direction: f64::NAN, asymmetry: f64::NAN };
}
let top = crown.iter().map(|p| p[2]).fold(f64::NEG_INFINITY, f64::max);
let base = crown.iter().map(|p| p[2]).fold(f64::INFINITY, f64::min);
let xy: Vec<[f64; 2]> = crown.iter().map(|p| [p[0], p[1]]).collect();
let hull = convex_hull(&xy);
let area = if hull.len() >= 3 { polygon_area(&hull) } else { 0.0 };
let mut max_width = 0.0f64;
for i in 0..hull.len() {
for j in i + 1..hull.len() {
max_width = max_width.max((hull[i][0] - hull[j][0]).hypot(hull[i][1] - hull[j][1]));
}
}
let h0 = hull.first().copied().unwrap_or([0.0, 0.0]);
let (mut cx, mut cy, mut a2) = (0.0, 0.0, 0.0);
for i in 0..hull.len() {
let (p, q) = (hull[i], hull[(i + 1) % hull.len()]);
let (p, q) = ([p[0] - h0[0], p[1] - h0[1]], [q[0] - h0[0], q[1] - h0[1]]);
let c = p[0] * q[1] - q[0] * p[1];
cx += (p[0] + q[0]) * c;
cy += (p[1] + q[1]) * c;
a2 += c;
}
let centroid = if a2.abs() > 1e-12 { [h0[0] + cx / (3.0 * a2), h0[1] + cy / (3.0 * a2)] } else { [xy.iter().map(|p| p[0]).sum::<f64>() / xy.len() as f64, xy.iter().map(|p| p[1]).sum::<f64>() / xy.len() as f64] };
let lowest = crown.iter().min_by(|a, b| a[2].partial_cmp(&b[2]).unwrap()).unwrap();
let origin = base_xy.unwrap_or([lowest[0], lowest[1]]);
let (dx, dy) = (centroid[0] - origin[0], centroid[1] - origin[1]);
let n_slices = (((top - base) / slice).ceil() as usize).max(1);
let mut slices: Vec<Vec<[f64; 2]>> = vec![Vec::new(); n_slices];
for p in &crown {
let k = (((p[2] - base) / slice) as usize).min(n_slices - 1);
slices[k].push([p[0], p[1]]);
}
let (mut volume, mut surface) = (0.0, 0.0);
let mut top_area = 0.0;
for s in &slices {
let h = convex_hull(s);
if h.len() >= 3 {
let a = polygon_area(&h);
let perimeter: f64 = (0..h.len()).map(|i| (h[i][0] - h[(i + 1) % h.len()][0]).hypot(h[i][1] - h[(i + 1) % h.len()][1])).sum();
volume += a * slice;
surface += perimeter * slice;
top_area = a;
}
}
surface += top_area;
let radius = (area / std::f64::consts::PI).sqrt();
CrownShape {
projected_area: area,
diameter: 2.0 * radius,
max_width,
volume,
surface,
base_height: base,
top_height: top,
offset: dx.hypot(dy),
offset_direction: dy.atan2(dx).to_degrees(),
asymmetry: if radius > 0.0 { dx.hypot(dy) / radius } else { f64::NAN },
}
}
pub fn crown_metrics_all(points: &[Point], heights: &[f64], labels: &[i64], crown_base_fraction: f64) -> Vec<(i64, [f64; 4])> {
let mut groups: HashMap<i64, Vec<usize>> = HashMap::new();
for (i, &l) in labels.iter().enumerate() {
if l >= 0 {
groups.entry(l).or_default().push(i);
}
}
let mut ids: Vec<i64> = groups.keys().cloned().collect();
ids.sort_unstable();
ids.par_iter()
.filter_map(|&id| crown_metrics_indices(points, heights, &groups[&id], crown_base_fraction).map(|m| (id, m)))
.collect()
}
pub fn crown_metrics(points: &[Point], heights: &[f64], labels: &[i64], tree_id: i64, crown_base_fraction: f64) -> Option<[f64; 4]> {
let idx: Vec<usize> = (0..points.len()).filter(|&i| labels[i] == tree_id).collect();
crown_metrics_indices(points, heights, &idx, crown_base_fraction)
}
fn crown_metrics_indices(points: &[Point], heights: &[f64], idx: &[usize], crown_base_fraction: f64) -> Option<[f64; 4]> {
if idx.len() < 4 {
return None;
}
let top = idx.iter().map(|&i| heights[i]).fold(f64::NEG_INFINITY, f64::max);
let nb = ((top / 0.5).ceil() as usize).max(1);
let mut counts = vec![0usize; nb];
for &i in idx {
let b = ((heights[i] / 0.5) as usize).min(nb - 1);
counts[b] += 1;
}
let thresh = crown_base_fraction * *counts.iter().max().unwrap() as f64;
let start = nb / 4;
let base_idx = (start..nb).find(|&b| counts[b] as f64 >= thresh).unwrap_or(start);
let crown_base = base_idx as f64 * 0.5;
let xy: Vec<[f64; 2]> = idx.iter().filter(|&&i| heights[i] >= crown_base).map(|&i| [points[i][0], points[i][1]]).collect();
let area = convex_hull_area(&xy);
let diam = if area.is_finite() { 2.0 * (area / std::f64::consts::PI).sqrt() } else { f64::NAN };
Some([area, crown_base, top - crown_base, diam])
}
#[cfg(test)]
mod graph_node_tests {
use super::*;
fn reference(points: &[Point], heights: &[f64], p: &SegmentParams) -> Vec<usize> {
let above: Vec<usize> = (0..points.len()).filter(|&i| heights[i] >= p.ground_clearance).collect();
if p.voxel_size.is_nan() || p.voxel_size <= 0.0 {
return above;
}
let above_pts: Vec<Point> = above.iter().map(|&i| points[i]).collect();
let keep = match &p.voxel_origin {
Some(o) => crate::filters::voxel_downsample_indices_at(&above_pts, o, p.voxel_size),
None => crate::filters::voxel_downsample_indices(&above_pts, p.voxel_size),
};
keep.into_iter().map(|i| above[i]).collect()
}
#[test]
fn nodes_are_those_of_the_copying_thinning() {
let mut rng = Rng::new(11);
let mut unit = || (rng.next_u64() >> 11) as f64 / (1u64 << 53) as f64;
let mut pts: Vec<Point> = (0..6000).map(|_| [5.0 * unit() - 2.0, 5.0 * unit(), 8.0 * unit() - 0.5]).collect();
for i in 0..300 {
pts[5000 + i] = pts[i];
}
let heights: Vec<f64> = pts.iter().enumerate().map(|(i, q)| if i % 97 == 0 { f64::NAN } else { q[2] }).collect();
for (voxel, origin) in [(0.03, None), (0.1, Some([0.0, 0.0, 0.0])), (0.0, None), (0.25, Some([1.0, -1.0, 0.0]))] {
let p = SegmentParams { voxel_size: voxel, voxel_origin: origin, ..Default::default() };
assert_eq!(graph_nodes(&pts, &heights, &p), reference(&pts, &heights, &p));
}
let none = SegmentParams { ground_clearance: 100.0, ..Default::default() };
assert!(graph_nodes(&pts, &heights, &none).is_empty());
}
}
#[cfg(test)]
mod seed_and_height_tests {
use super::*;
fn forest(n: usize, seed: u64) -> (Vec<Point>, Vec<f64>, Vec<Tree>) {
let mut rng = Rng::new(seed);
let mut unit = || (rng.next_u64() >> 11) as f64 / (1u64 << 53) as f64;
let mut pts: Vec<Point> = (0..n).map(|_| [20.0 * unit() - 10.0, 20.0 * unit() - 10.0, 12.0 * unit()]).collect();
let trees: Vec<Tree> = (0..9)
.map(|i| Tree { tree_id: i as i64 + 1, x: 6.0 * (i % 3) as f64 - 6.0, y: 6.0 * (i / 3) as f64 - 6.0, dbh: 0.1 + 0.15 * (i % 4) as f64, height: f64::NAN, n_points: 0, inlier_fraction: 1.0, n_slices: 5, rmse: 0.01, lean_deg: 0.0, quality: 0.9 })
.collect();
for t in &trees {
for k in 0..60 {
let a = k as f64 * 0.1047;
let r = 0.5 * t.dbh + 0.02 * (k % 3) as f64;
pts.push([t.x + r * a.cos(), t.y + r * a.sin(), 1.5 + 0.3 * ((k % 5) as f64 - 2.0)]);
}
pts.push([t.x + 1.5, t.y, 3.0]); pts.push([t.x + 0.5 * t.dbh + 0.25, t.y, 1.5]); }
let hw = pts.iter().map(|q| q[2]).collect();
(pts, hw, trees)
}
fn reference(work: &[Point], hw: &[f64], trees: &[Tree], p: &SegmentParams) -> Seeds {
let mut out = Seeds { seeds: Vec::new(), seed_tree: Vec::new(), seed_xy: Vec::new(), seed_scale: Vec::new() };
for t in trees {
let scale = if p.height_prior {
let mut hs: Vec<f64> = work.iter().zip(hw).filter(|(q, _)| (q[0] - t.x).hypot(q[1] - t.y) <= p.height_prior_radius).map(|(_, &h)| h).collect();
hs.sort_by(|a, b| a.partial_cmp(b).unwrap());
let h95 = hs.get(((hs.len() as f64 * 0.95) as usize).min(hs.len().saturating_sub(1))).copied().unwrap_or(2.0);
1.0 / h95.max(2.0).powf(p.height_prior_power)
} else {
1.0
};
let inner = if p.seed_ring { 0.5 * t.dbh } else { 0.0 };
for (i, q) in work.iter().enumerate() {
let d = (q[0] - t.x).hypot(q[1] - t.y);
if (d - inner).abs() <= p.seed_radius && hw[i] < p.seed_height + 0.5 {
out.seeds.push(i);
out.seed_tree.push(t.tree_id);
out.seed_xy.push([t.x, t.y]);
out.seed_scale.push(scale);
}
}
}
out
}
#[test]
fn gridded_seeds_are_the_scanned_seeds() {
for (seed, prior, ring) in [(1, true, true), (2, false, true), (3, true, false), (4, false, false)] {
let (pts, hw, trees) = forest(20_000, seed);
let p = SegmentParams { height_prior: prior, seed_ring: ring, ..Default::default() };
let a = collect_seeds(&pts, &hw, &trees, &p);
let b = reference(&pts, &hw, &trees, &p);
assert!(!a.seeds.is_empty());
assert_eq!(a.seeds, b.seeds);
assert_eq!(a.seed_tree, b.seed_tree);
assert_eq!(a.seed_xy, b.seed_xy);
assert!(a.seed_scale.iter().zip(&b.seed_scale).all(|(x, y)| x.to_bits() == y.to_bits()));
}
let (pts, hw, _) = forest(100, 5);
assert!(collect_seeds(&pts, &hw, &[], &SegmentParams::default()).seeds.is_empty());
assert!(collect_seeds(&[], &[], &forest(10, 6).2, &SegmentParams::default()).seeds.is_empty());
}
#[test]
fn an_exclusion_mask_keeps_points_out_of_graph_and_labels() {
let (pts, hw, trees) = forest(8000, 21);
let p = SegmentParams { voxel_size: 0.05, ..Default::default() };
assert_eq!(segment_trees_masked(&pts, &hw, &trees, &p, None), segment_trees(&pts, &hw, &trees, &p));
assert_eq!(graph_nodes_masked(&pts, &hw, &p, None), graph_nodes(&pts, &hw, &p));
let mask: Vec<bool> = pts.iter().map(|q| q[2] < 0.6 && q[0] > 0.0).collect();
let nodes = graph_nodes_masked(&pts, &hw, &p, Some(&mask));
assert!(!nodes.is_empty() && nodes.iter().all(|&i| !mask[i]));
let labels = segment_trees_masked(&pts, &hw, &trees, &p, Some(&mask));
assert!(labels.iter().zip(&mask).all(|(&l, &m)| !m || l == -1));
assert!(labels.iter().any(|&l| l >= 0));
let low = SegmentParams { ground_clearance: 0.0, voxel_size: 0.05, ..Default::default() };
let ground: Vec<bool> = pts.iter().map(|q| q[2] < 0.05).collect();
let labels_low = segment_trees_masked(&pts, &hw, &trees, &low, Some(&ground));
let labelled_below_cut = labels_low.iter().zip(&pts).filter(|(&l, q)| l >= 0 && q[2] < 0.25).count();
assert!(labelled_below_cut > 0);
assert!(labels_low.iter().zip(&ground).all(|(&l, &g)| !g || l == -1));
}
#[test]
fn one_pass_heights_are_the_per_tree_heights() {
let (pts, hw, mut trees) = forest(5000, 7);
let labels: Vec<i64> = (0..pts.len()).map(|i| (i % 12) as i64 - 2).collect(); let mut expect = trees.clone();
for t in expect.iter_mut() {
let mut hs: Vec<f64> = labels.iter().zip(&hw).filter(|(&l, _)| l == t.tree_id).map(|(_, &h)| h).collect();
t.n_points = hs.len();
if hs.is_empty() {
t.height = f64::NAN;
continue;
}
hs.sort_by(|a, b| a.partial_cmp(b).unwrap());
let pos = ((99.0 / 100.0) * (hs.len() - 1) as f64).round() as usize;
t.height = hs[pos.min(hs.len() - 1)];
}
tree_heights(&hw, &labels, &mut trees, 99.0);
for (a, b) in trees.iter().zip(&expect) {
assert_eq!(a.n_points, b.n_points);
assert_eq!(a.height.to_bits(), b.height.to_bits());
}
let mut odd = vec![trees[0].clone(), Tree { tree_id: 77, ..trees[1].clone() }, trees[0].clone()];
tree_heights(&hw, &labels, &mut odd, 100.0);
assert_eq!(odd[0].n_points, odd[2].n_points);
assert_eq!(odd[0].height.to_bits(), odd[2].height.to_bits());
assert_eq!(odd[1].n_points, 0);
assert!(odd[1].height.is_nan());
}
}
#[cfg(test)]
mod review_regression_tests {
use super::*;
const UTM: [f64; 2] = [500_000.0, 8_500_000.0];
fn unit(rng: &mut Rng) -> f64 {
(rng.next_u64() >> 11) as f64 / (1u64 << 53) as f64
}
#[test]
fn circle_fits_hold_in_projected_coordinates() {
let mut rng = Rng::new(1);
let local: Vec<[f64; 2]> = (0..400)
.map(|_| {
let a = unit(&mut rng) * std::f64::consts::TAU;
let r = 0.25 + 0.002 * (unit(&mut rng) - 0.5);
[r * a.cos(), r * a.sin()]
})
.collect();
let far: Vec<[f64; 2]> = local.iter().map(|q| [q[0] + UTM[0], q[1] + UTM[1]]).collect();
let (ax, ay, ar) = fit_circle_algebraic(&far).unwrap();
assert!((ax - UTM[0]).abs() < 1e-3 && (ay - UTM[1]).abs() < 1e-3 && (ar - 0.25).abs() < 1e-3, "{ax} {ay} {ar}");
let (cx, cy, r, rmse) = fit_circle(&far).unwrap();
assert!((cx - UTM[0]).abs() < 1e-3 && (cy - UTM[1]).abs() < 1e-3 && (r - 0.25).abs() < 1e-3 && rmse < 2e-3, "{cx} {cy} {r} {rmse}");
let (rx, ry, rr, inl) = fit_circle_ransac(&far, &RansacCircleParams::default()).unwrap();
assert!((rx - UTM[0]).abs() < 1e-3 && (ry - UTM[1]).abs() < 1e-3 && (rr - 0.25).abs() < 1e-3);
assert_eq!(inl.iter().filter(|&&b| b).count(), 400);
let (lx, ly, lr, _) = fit_circle(&local).unwrap();
assert!((cx - UTM[0] - lx).abs() < 1e-6 && (cy - UTM[1] - ly).abs() < 1e-6 && (r - lr).abs() < 1e-6);
let pts: Vec<Point> = far.iter().map(|q| [q[0], q[1], 1.3]).collect();
let d = dbh_profile(&pts, &vec![1.3; pts.len()], UTM[0], UTM[1], &[1.3], 0.2, 1.0);
assert!((d[0] - 0.5).abs() < 2e-3, "{}", d[0]);
}
#[test]
fn crown_offset_holds_in_projected_coordinates() {
let mut rng = Rng::new(3);
let local: Vec<Point> = (0..5000)
.map(|_| {
let a = unit(&mut rng) * std::f64::consts::TAU;
let r = 3.0 * unit(&mut rng).sqrt();
[1.0 + r * a.cos(), r * a.sin(), 5.0 + 5.0 * unit(&mut rng)]
})
.collect();
let far: Vec<Point> = local.iter().map(|q| [q[0] + UTM[0], q[1] + UTM[1], q[2]]).collect();
let a = crown_shape(&local, Some([0.0, 0.0]), 0.0, 0.5);
let b = crown_shape(&far, Some(UTM), 0.0, 0.5);
assert!((a.offset - 1.0).abs() < 0.05, "{}", a.offset);
assert!((b.offset - a.offset).abs() < 1e-4, "{} vs {}", b.offset, a.offset);
assert!((b.offset_direction - a.offset_direction).abs() < 0.01 && (b.asymmetry - a.asymmetry).abs() < 1e-4);
assert!((b.projected_area - a.projected_area).abs() < 0.01);
}
#[test]
fn a_nan_height_is_unlabelled_and_does_not_stop_the_heights() {
let mut pts: Vec<Point> = Vec::new();
for k in 0..4000 {
let a = k as f64 * 0.37;
pts.push([0.1 * a.cos(), 0.1 * a.sin(), 6.0 * (k as f64 / 4000.0)]);
}
let mut h: Vec<f64> = pts.iter().map(|q| q[2]).collect();
h[2000] = f64::NAN;
let mut trees = vec![Tree { tree_id: 1, x: 0.0, y: 0.0, dbh: 0.2, height: f64::NAN, n_points: 0, inlier_fraction: 1.0, n_slices: 5, rmse: 0.0, lean_deg: 0.0, quality: 1.0 }];
let labels = segment_trees(&pts, &h, &trees, &SegmentParams { voxel_size: 0.05, ..Default::default() });
assert_eq!(labels[2000], -1, "a point without a height is not labelled");
assert_eq!(labels[2001], 1);
tree_heights(&h, &labels, &mut trees, 100.0);
assert!((trees[0].height - 6.0).abs() < 0.01);
let all = vec![1i64; pts.len()];
tree_heights(&h, &all, &mut trees, 100.0);
assert_eq!(trees[0].n_points, pts.len());
assert!((trees[0].height - 6.0).abs() < 0.01);
tree_heights(&[f64::NAN, f64::NAN], &[1, 1], &mut trees, 100.0);
assert!(trees[0].height.is_nan() && trees[0].n_points == 2);
}
fn candidate(id: i64, x: f64, quality: f64) -> Tree {
Tree { tree_id: id, x, y: 0.0, dbh: 0.2, height: f64::NAN, n_points: 1, inlier_fraction: 1.0, n_slices: 5, rmse: 0.0, lean_deg: 0.0, quality }
}
#[test]
fn a_branch_of_a_mutual_pair_goes_to_the_pairs_survivor() {
let trees = vec![candidate(1, 0.0, 0.1), candidate(2, 1.0, 0.5), candidate(3, 2.0, 0.6), candidate(4, 9.0, 0.9)];
let (kept, merged) = resolve_branches(&trees, &[Some(1), Some(2), Some(1), None]);
assert_eq!(kept.iter().map(|t| t.tree_id).collect::<Vec<_>>(), vec![3, 4]);
assert_eq!(merged, vec![3, 3, 3, 4]);
let trees = vec![candidate(1, 0.0, 0.9), candidate(2, 1.0, 0.5), candidate(3, 2.0, 0.6)];
let (kept, merged) = resolve_branches(&trees, &[Some(1), Some(2), Some(1)]);
assert_eq!(kept.iter().map(|t| t.tree_id).collect::<Vec<_>>(), vec![3]);
assert_eq!(merged, vec![3, 3, 3]);
let trees = vec![candidate(1, 0.0, 0.5), candidate(2, 1.0, 0.5), candidate(3, 2.0, 0.5)];
let (kept, merged) = resolve_branches(&trees, &[Some(1), Some(2), Some(0)]);
assert_eq!((kept.len(), merged), (1, vec![1, 1, 1]));
}
fn branch_parents_scanning(points: &[Point], heights: &[f64], trees: &[Tree], p: &SegmentParams, ground_height: f64, trunk_scale: f64, trunk_min: f64, search_radius: f64) -> Vec<Option<usize>> {
let mut parent: Vec<Option<usize>> = vec![None; trees.len()];
let work_idx = graph_nodes(points, heights, p);
let work: Vec<Point> = work_idx.iter().map(|&i| points[i]).collect();
let hw: Vec<f64> = work_idx.iter().map(|&i| heights[i]).collect();
let graph = p.graph(&work);
let sources: Vec<usize> = (0..work.len()).filter(|&i| hw[i] < ground_height).collect();
if sources.is_empty() {
return parent;
}
let (dist, _, pred) = dijkstra_gravity(&graph, &sources, None, 0.0);
for (ci, t) in trees.iter().enumerate() {
let mut best: Option<(usize, f64)> = None;
for i in 0..work.len() {
if (work[i][0] - t.x).hypot(work[i][1] - t.y) > p.seed_radius || (hw[i] - p.seed_height).abs() > 0.5 || !dist[i].is_finite() {
continue;
}
if best.map(|b| dist[i] < b.1).unwrap_or(true) {
best = Some((i, dist[i]));
}
}
let Some((mut node, _)) = best else { continue };
while pred[node] != usize::MAX {
node = pred[node];
for (oi, o) in trees.iter().enumerate() {
if oi == ci || (o.x - t.x).hypot(o.y - t.y) > search_radius {
continue;
}
let r = (trunk_scale * o.dbh / 2.0).max(trunk_min);
if hw[node] >= p.ground_clearance && hw[node] <= p.seed_height + 0.5 && (work[node][0] - o.x).hypot(work[node][1] - o.y) <= r {
parent[ci] = Some(oi);
}
}
if parent[ci].is_some() {
break;
}
}
}
parent
}
#[test]
fn gridded_branch_parents_are_the_scanned_ones() {
let mut pts: Vec<Point> = Vec::new();
for (x, y) in [(UTM[0], UTM[1]), (UTM[0] + 4.0, UTM[1])] {
for k in 0..3000 {
let a = k as f64 * 0.37;
pts.push([x + 0.1 * a.cos(), y + 0.1 * a.sin(), 4.0 * (k as f64 / 3000.0)]);
}
}
for k in 0..1500 {
let s = k as f64 / 1500.0;
let a = k as f64 * 0.61;
pts.push([UTM[0] + 0.1 + 1.0 * s + 0.03 * a.cos(), UTM[1] + 0.03 * a.sin(), 1.0 + 1.0 * s]);
}
let h: Vec<f64> = pts.iter().map(|q| q[2]).collect();
let trees = vec![candidate(1, UTM[0], 0.9), candidate(2, UTM[0] + 4.0, 0.8), candidate(3, UTM[0] + 0.6, 0.3)];
let trees: Vec<Tree> = trees.into_iter().map(|t| Tree { y: UTM[1], ..t }).collect();
let p = SegmentParams { voxel_size: 0.03, ..Default::default() };
let got = branch_parents(&pts, &h, &trees, &p, 0.5, 1.5, 0.15, 6.0, None);
assert_eq!(got, branch_parents_scanning(&pts, &h, &trees, &p, 0.5, 1.5, 0.15, 6.0));
assert_eq!(got, vec![None, None, Some(0)]);
}
}