use crate::common::kd_tree::KdTreeSearch;
use crate::geom3::align3::{AlignSurfMatch3, SurfaceTarget3};
use crate::{CloudIndex3, Point3, PointCloud3, Result, UnitVec3, VOXEL_COHERENCE_ATTR};
pub struct CloudTarget3<'a> {
index: CloudIndex3<'a>,
normals: &'a [UnitVec3],
stdev: Option<&'a [f64]>,
coherence: Option<&'a [f64]>,
max_extrapolation: f64,
}
impl<'a> CloudTarget3<'a> {
pub fn try_new(cloud: &'a PointCloud3, 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());
}
let normals = cloud.point_normals().ok_or(
"A cloud used as an alignment target must carry per-point normals, which supply both \
the tangent plane and the sign of the residual. See CloudIndex3::estimate_normals.",
)?;
Ok(Self {
index: cloud.compute_index()?,
normals,
stdev: cloud.point_stdev(),
coherence: cloud
.point_attr(VOXEL_COHERENCE_ATTR)
.and_then(|a| a.as_scalar()),
max_extrapolation,
})
}
pub fn cloud(&self) -> &'a PointCloud3 {
self.index.cloud()
}
}
impl SurfaceTarget3 for CloudTarget3<'_> {
fn find_align_match(&self, p: &Point3) -> AlignSurfMatch3 {
let (i, distance) = self.index.nearest_one(p);
let normal = self.normals[i];
let nearest = self.index.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 weight = self.coherence.map_or(1.0, |c| c[i]);
let matched =
AlignSurfMatch3::new(projected, normal, lateral <= self.max_extrapolation, weight);
match self.stdev {
Some(stdev) => matched.with_sigma(stdev[i]),
None => matched,
}
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::geom3::align3::{AlignOptions3, AlignParams3, points_to_surface3};
use crate::{Iso3, Mesh3, Vector3};
use approx::assert_relative_eq;
fn engine_blade() -> Mesh3 {
let path =
std::path::Path::new(env!("CARGO_MANIFEST_DIR")).join("tests/data/engine-blade.tcmesh");
let data = crate::io::read_tc_mesh_file(&path).expect("failed to load engine blade");
Mesh3::from_data(data, false).expect("failed to accelerate engine blade")
}
fn rms_displacement(points: &[Point3], t: &Iso3) -> f64 {
let sum: f64 = points.iter().map(|p| (t * p - p).norm_squared()).sum();
(sum / points.len() as f64).sqrt()
}
#[test]
fn cloud_target_requires_normals() {
let bare = PointCloud3::new(vec![Point3::origin(), Point3::new(1.0, 0.0, 0.0)]);
assert!(CloudTarget3::try_new(&bare, 1.0).is_err());
}
#[test]
fn cloud_target_rejects_a_nonsense_extrapolation() {
let cloud = engine_blade().sample_dense(1.0, None).unwrap();
assert!(CloudTarget3::try_new(&cloud, 0.0).is_err());
assert!(CloudTarget3::try_new(&cloud, f64::NAN).is_err());
}
#[test]
fn cloud_target_projects_onto_the_tangent_plane() {
let mut cloud = PointCloud3::new(vec![
Point3::new(0.0, 0.0, 0.0),
Point3::new(1.0, 0.0, 0.0),
Point3::new(0.0, 1.0, 0.0),
Point3::new(1.0, 1.0, 0.0),
]);
cloud
.set_point_normals(Some(vec![UnitVec3::new_normalize(Vector3::z()); 4]))
.unwrap();
let target = CloudTarget3::try_new(&cloud, 10.0).unwrap();
let query = Point3::new(0.5, 0.5, 2.0);
let m = target.find_align_match(&query);
assert_relative_eq!(m.point, Point3::new(0.5, 0.5, 0.0), epsilon = 1e-12);
assert_relative_eq!((query - m.point).norm(), 2.0, epsilon = 1e-12);
assert!(m.is_on);
}
#[test]
fn cloud_target_gates_on_lateral_distance_not_total_distance() {
let mut cloud = PointCloud3::new(vec![Point3::origin(), Point3::new(0.5, 0.0, 0.0)]);
cloud
.set_point_normals(Some(vec![UnitVec3::new_normalize(Vector3::z()); 2]))
.unwrap();
let target = CloudTarget3::try_new(&cloud, 1.0).unwrap();
let above = target.find_align_match(&Point3::new(0.0, 0.0, 50.0));
assert!(above.is_on, "a large normal offset must not be gated out");
let beside = target.find_align_match(&Point3::new(5.0, 0.0, 0.0));
assert!(!beside.is_on, "extrapolating past the cloud must be gated");
}
#[test]
fn cloud_target_takes_its_weight_from_voxel_coherence() {
let mesh = Mesh3::create_box(20.0, 20.0, 20.0, false);
let cloud = mesh
.sample_dense(0.5, None)
.unwrap()
.reduce_by_voxel(2.0)
.unwrap();
let target = CloudTarget3::try_new(&cloud, 4.0).unwrap();
let weights: Vec<f64> = cloud
.points()
.iter()
.map(|p| target.find_align_match(p).weight)
.collect();
let lowest = weights.iter().copied().fold(f64::INFINITY, f64::min);
let highest = weights.iter().copied().fold(f64::NEG_INFINITY, f64::max);
assert!(highest > 0.99, "a flat face should be weighted near one");
assert!(
lowest < 0.9,
"an edge voxel should be weighted down, got {lowest}"
);
}
struct NearestPointTarget<'a> {
index: CloudIndex3<'a>,
normals: &'a [UnitVec3],
}
impl SurfaceTarget3 for NearestPointTarget<'_> {
fn find_align_match(&self, p: &Point3) -> AlignSurfMatch3 {
let (i, _) = self.index.nearest_one(p);
AlignSurfMatch3::new(self.index.points()[i], self.normals[i], true, 1.0)
}
}
#[test]
fn the_tangent_plane_projection_is_what_removes_the_sampling_floor() {
let mesh = engine_blade();
let test_points = mesh.sample_poisson(1.0, None).unwrap().points().to_vec();
let center = mesh.aabb().center();
let delta = Iso3::new(Vector3::new(1.5, -0.8, 0.6), Vector3::new(0.0, 0.0, 0.04));
let moved: Vec<Point3> = test_points.iter().map(|p| delta * p).collect();
let spacing = 2.0;
let cloud = mesh.sample_dense(spacing, None).unwrap();
let normals = cloud.point_normals().unwrap();
let opts = AlignOptions3::default();
let recover = |out: &crate::geom3::AlignOutcome3| {
rms_displacement(&test_points, &(out.alignment().full_transform() * delta))
};
let plane = CloudTarget3::try_new(&cloud, spacing * 3.0).unwrap();
let plane_err = recover(
&points_to_surface3(
&moved,
&plane,
AlignParams3::from_center(center, None),
&opts,
)
.expect("tangent plane solve failed"),
);
let nearest = NearestPointTarget {
index: cloud.compute_index().unwrap(),
normals,
};
let nearest_err = recover(
&points_to_surface3(
&moved,
&nearest,
AlignParams3::from_center(center, None),
&opts,
)
.expect("nearest point solve failed"),
);
println!();
println!("at {spacing} mm sample spacing:");
println!(" tangent plane: {plane_err:.3e} mm");
println!(" nearest point: {nearest_err:.3e} mm");
println!(
" factor: {:.0}x",
nearest_err / plane_err.max(1e-15)
);
assert!(
plane_err < nearest_err / 100.0,
"the tangent plane projection should be far better than a nearest point match, \
got {plane_err} against {nearest_err}"
);
}
#[test]
fn cloud_target_recovers_a_pose_and_reports_its_cost_against_a_mesh() {
let mesh = engine_blade();
let test_points = mesh.sample_poisson(1.0, None).unwrap().points().to_vec();
assert!(test_points.len() > 200);
let center = mesh.aabb().center();
let delta = Iso3::new(Vector3::new(1.5, -0.8, 0.6), Vector3::new(0.0, 0.0, 0.04));
let moved: Vec<Point3> = test_points.iter().map(|p| delta * p).collect();
let start = rms_displacement(&test_points, &delta);
let opts = AlignOptions3::default();
let recover = |out: &crate::geom3::AlignOutcome3| {
rms_displacement(&test_points, &(out.alignment().full_transform() * delta))
};
let mesh_out = points_to_surface3(
&moved,
&mesh,
AlignParams3::from_center(center, None),
&opts,
)
.expect("mesh solve failed");
let mesh_err = recover(&mesh_out);
println!();
println!("start displacement: {start:.4} mm");
println!("mesh target: {mesh_err:.3e} mm");
println!();
println!(
"{:>10} {:>10} {:>12} {:>10}",
"spacing", "points", "RMS mm", "vs mesh"
);
let mut previous = 0.0;
for spacing in [0.25, 1.0, 2.0] {
let cloud = mesh.sample_dense(spacing, None).unwrap();
let target = CloudTarget3::try_new(&cloud, spacing * 3.0).unwrap();
let out = points_to_surface3(
&moved,
&target,
AlignParams3::from_center(center, None),
&opts,
)
.expect("cloud solve failed");
let err = recover(&out);
println!(
"{:>10.2} {:>10} {:>12.3e} {:>9.1}x",
spacing,
cloud.point_count(),
err,
err / mesh_err.max(1e-15)
);
assert!(
err < start / 10.0,
"spacing {spacing} did not recover the pose: {err} from {start}"
);
assert!(
err >= previous * 0.25,
"error fell sharply as samples got sparser, which is backwards"
);
previous = err;
}
}
}