use crate::common::kd_tree::KdTreeSearch;
use crate::geom2::align2::{AlignSurfMatch2, SurfaceTarget2};
use crate::geom2::{KdTree2, Point2};
use crate::{Result, UnitVec2};
pub struct CloudTarget2<'a> {
index: KdTree2,
points: &'a [Point2],
normals: &'a [UnitVec2],
stdev: Option<&'a [f64]>,
max_extrapolation: f64,
}
impl<'a> CloudTarget2<'a> {
pub fn try_new(
points: &'a [Point2],
normals: &'a [UnitVec2],
stdev: Option<&'a [f64]>,
max_extrapolation: f64,
) -> Result<Self> {
if !max_extrapolation.is_finite() || max_extrapolation <= 0.0 {
return Err(format!(
"max_extrapolation must be finite and positive, got {max_extrapolation}"
)
.into());
}
if points.is_empty() {
return Err("a point set alignment target must have at least one point".into());
}
if normals.len() != points.len() {
return Err(format!(
"there are {} points but {} normals",
points.len(),
normals.len()
)
.into());
}
if let Some(s) = stdev
&& s.len() != points.len()
{
return Err(format!(
"there are {} points but {} standard deviations",
points.len(),
s.len()
)
.into());
}
Ok(Self {
index: KdTree2::try_new(points)?,
points,
normals,
stdev,
max_extrapolation,
})
}
pub fn points(&self) -> &'a [Point2] {
self.points
}
}
impl SurfaceTarget2 for CloudTarget2<'_> {
fn find_align_match(&self, p: &Point2) -> AlignSurfMatch2 {
let (i, distance) = self.index.nearest_one(p);
let normal = self.normals[i];
let nearest = self.points[i];
let offset = normal.dot(&(p - nearest));
let lateral = (distance * distance - offset * offset).max(0.0).sqrt();
let projected = p - normal.into_inner() * offset;
let matched =
AlignSurfMatch2::new(projected, normal, lateral <= self.max_extrapolation, 1.0);
match self.stdev {
Some(stdev) => matched.with_sigma(stdev[i]),
None => matched,
}
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::common::points::dist;
use crate::geom2::align2::{AlignOptions2, AlignParams2, points_to_surface2};
use crate::geom2::{Curve2, Iso2, Vector2};
use approx::assert_relative_eq;
fn flat_run() -> (Vec<Point2>, Vec<UnitVec2>) {
let points: Vec<Point2> = (-5..=5).map(|i| Point2::new(i as f64, 0.0)).collect();
let normals = vec![UnitVec2::new_unchecked(Vector2::y()); points.len()];
(points, normals)
}
#[test]
fn a_nonsense_extrapolation_bound_is_rejected() {
let (points, normals) = flat_run();
assert!(CloudTarget2::try_new(&points, &normals, None, 0.0).is_err());
assert!(CloudTarget2::try_new(&points, &normals, None, -1.0).is_err());
assert!(CloudTarget2::try_new(&points, &normals, None, f64::NAN).is_err());
}
#[test]
fn mismatched_or_empty_inputs_are_rejected() {
let (points, normals) = flat_run();
assert!(CloudTarget2::try_new(&[], &[], None, 1.0).is_err());
assert!(CloudTarget2::try_new(&points, &normals[..3], None, 1.0).is_err());
let short = vec![0.1; 3];
assert!(CloudTarget2::try_new(&points, &normals, Some(&short), 1.0).is_err());
}
#[test]
fn the_match_is_a_projection_onto_the_tangent_line() {
let (points, normals) = flat_run();
let target = CloudTarget2::try_new(&points, &normals, None, 1.0).unwrap();
let query = Point2::new(0.5, 2.0);
let m = target.find_align_match(&query);
assert_relative_eq!(m.point, Point2::new(0.5, 0.0), epsilon = 1e-12);
assert_relative_eq!(dist(&query, &m.point), 2.0, epsilon = 1e-12);
assert!(m.is_on);
}
#[test]
fn the_gate_is_on_along_surface_distance_not_total_distance() {
let (points, normals) = flat_run();
let target = CloudTarget2::try_new(&points, &normals, None, 1.0).unwrap();
let above = target.find_align_match(&Point2::new(0.0, 50.0));
assert!(above.is_on, "a large normal offset must not be gated out");
let beside = target.find_align_match(&Point2::new(9.0, 0.0));
assert!(!beside.is_on, "extrapolating past the set must be gated");
}
#[test]
fn the_target_reports_its_own_uncertainty() {
let (points, normals) = flat_run();
let stdev: Vec<f64> = (0..points.len()).map(|i| 0.01 * (i + 1) as f64).collect();
let target = CloudTarget2::try_new(&points, &normals, Some(&stdev), 1.0).unwrap();
let m = target.find_align_match(&Point2::new(-5.0, 1.0));
assert_relative_eq!(m.sigma, 0.01, epsilon = 1e-12);
let m = target.find_align_match(&Point2::new(5.0, 1.0));
assert_relative_eq!(m.sigma, 0.01 * points.len() as f64, epsilon = 1e-12);
}
#[test]
fn the_tangent_line_projection_is_what_removes_the_sampling_floor() {
struct NearestSample<'a> {
points: &'a [Point2],
normals: &'a [UnitVec2],
index: KdTree2,
}
impl SurfaceTarget2 for NearestSample<'_> {
fn find_align_match(&self, p: &Point2) -> AlignSurfMatch2 {
let (i, _) = self.index.nearest_one(p);
AlignSurfMatch2::new(self.points[i], self.normals[i], true, 1.0)
}
}
let mut rg = crate::common::random_geometry::RandomGeometry2::from_seed(0x5a3_f100);
let on_ellipse = |a: f64| Point2::new(8.0 * a.cos(), 3.0 * a.sin());
let normal_at = |a: f64| {
UnitVec2::new_normalize(Vector2::new(3.0 * a.cos(), 8.0 * a.sin()))
};
let n = 60;
let angles: Vec<f64> = (0..n)
.map(|i| {
let base = std::f64::consts::TAU * (i as f64) / (n as f64);
base + rg.f64_sym(0.4) * std::f64::consts::TAU / (n as f64)
})
.collect();
let sampled: Vec<Point2> = angles.iter().map(|&a| on_ellipse(a)).collect();
let normals: Vec<UnitVec2> = angles.iter().map(|&a| normal_at(a)).collect();
let dense: Vec<Point2> = (0..500)
.map(|i| on_ellipse(std::f64::consts::TAU * (i as f64 + 0.37) / 500.0))
.collect();
let disturb = Iso2::new(Vector2::new(0.08, -0.06), 0.01);
let moved: Vec<Point2> = dense.iter().map(|p| disturb * p).collect();
let opts = AlignOptions2::default();
let params = || AlignParams2::from_origin(None);
let projected = CloudTarget2::try_new(&sampled, &normals, None, 2.0).unwrap();
let nearest = NearestSample {
points: &sampled,
normals: &normals,
index: KdTree2::try_new(&sampled).unwrap(),
};
let rms = |t: &Iso2| -> f64 {
let residual = t * disturb;
(dense
.iter()
.map(|p| dist(p, &(residual * p)).powi(2))
.sum::<f64>()
/ dense.len() as f64)
.sqrt()
};
let with_projection = points_to_surface2(&moved, &projected, params(), &opts).unwrap();
let with_nearest = points_to_surface2(&moved, &nearest, params(), &opts).unwrap();
let e_projection = rms(with_projection.alignment().full_transform());
let e_nearest = rms(with_nearest.alignment().full_transform());
assert!(
e_projection < 0.1 * e_nearest,
"the tangent line projection should resolve well below the sample spacing: \
projected {e_projection}, nearest sample {e_nearest}"
);
}
#[test]
fn a_point_set_target_recovers_a_disturbed_pose() {
let curve = Curve2::from_points(
&[
Point2::new(-5.0, -3.0),
Point2::new(5.0, -3.0),
Point2::new(5.0, 3.0),
Point2::new(-5.0, 3.0),
],
1e-8,
true,
)
.unwrap();
let spacing = 0.25;
let count = (curve.length() / spacing).floor() as usize;
let stations: Vec<_> = (0..count)
.filter_map(|k| curve.at_length((k as f64 + 0.5) * spacing))
.collect();
let sampled: Vec<Point2> = stations.iter().map(|s| s.point()).collect();
let normals: Vec<UnitVec2> = stations.iter().map(|s| s.normal()).collect();
let target = CloudTarget2::try_new(&sampled, &normals, None, 1.0).unwrap();
let disturb = Iso2::new(Vector2::new(0.15, -0.1), 0.02);
let moved: Vec<Point2> = sampled.iter().map(|p| disturb * p).collect();
let outcome = points_to_surface2(
&moved,
&target,
AlignParams2::from_origin(None),
&AlignOptions2::default(),
)
.unwrap();
let composed = outcome.alignment().full_transform() * disturb;
assert_relative_eq!(
composed.to_matrix(),
Iso2::identity().to_matrix(),
epsilon = 1e-4
);
}
}