use crate::common::SPCoords;
use crate::common::align::{RefinementHalt, SolveQuality, TerminationReason};
use crate::common::consensus::weights::{MagsacWeight, estimate_sigma_max};
use crate::common::dist;
use crate::geom3::align3::jacobian::{copy_jacobian, point_surf_jacobian};
use crate::geom3::align3::{
AlignOptions3, AlignParams3, AlignSurfMatch3, AlignValues3, SurfaceTarget3,
};
use crate::geom3::{Align3, AlignOutcome3};
use crate::na::{Dyn, Matrix, Owned, U1, U6, Vector};
use crate::{Point3, Result};
use levenberg_marquardt::{LeastSquaresProblem, LevenbergMarquardt};
const RESIDUAL_DOF: usize = 3;
const N_PARAMS: usize = 6;
pub fn points_to_surface3(
points: &[Point3],
target: &impl SurfaceTarget3,
params: AlignParams3,
opts: &AlignOptions3,
) -> Result<AlignOutcome3> {
validate(points, opts)?;
let lm = LevenbergMarquardt::<f64>::new().with_patience(opts.patience);
let (mut problem, termination) =
solve(&lm, PointsToSurface3::new(points, target, params, opts));
if !SolveQuality::from_termination(&termination).is_usable() {
return Err(format!("Failed to align points to surface: {termination:?}").into());
}
let mut solves = vec![termination];
let mut halt = None;
if opts.refinement_steps > 0 {
match resolve_sigma_max(opts, &problem) {
None => halt = Some(RefinementHalt::NoNoiseEstimate),
Some(sigma_max) => {
let weighting = MagsacWeight::new(sigma_max, RESIDUAL_DOF);
for _ in 0..opts.refinement_steps {
problem.refresh_inv_sigma();
let weighted = problem.count_if_reweighted(&weighting);
if weighted < N_PARAMS {
halt = Some(RefinementHalt::Underdetermined {
weighted,
params: N_PARAMS,
});
break;
}
let last_good = problem.params.clone();
problem.apply_magsac_weights(&weighting);
let (next, termination) = solve(&lm, problem);
problem = next;
if !SolveQuality::from_termination(&termination).is_usable() {
problem.restore(last_good);
halt = Some(RefinementHalt::SolveFailed(termination));
break;
}
solves.push(termination);
}
}
}
}
let c = problem.params.compute_values();
let alignment = Align3::new(
c.transform,
c.align,
problem.params.local,
problem.params.offset,
problem.residuals.clone(),
);
Ok(AlignOutcome3::new(alignment, solves, halt))
}
fn validate(points: &[Point3], opts: &AlignOptions3) -> Result<()> {
if opts.patience == 0 {
return Err("patience must be greater than zero".into());
}
if let Some(s) = opts.sigma_max
&& (!s.is_finite() || s <= 0.0)
{
return Err(format!("sigma_max is {s}, but must be finite and strictly positive").into());
}
if let Some(sigma) = opts.point_sigma {
if sigma.len() != points.len() {
return Err(format!(
"point_sigma has {} entries but there are {} points",
sigma.len(),
points.len()
)
.into());
}
if let Some(i) = sigma.iter().position(|s| !s.is_finite() || *s <= 0.0) {
return Err(format!(
"point_sigma[{}] is {}, but every entry must be finite and strictly positive",
i, sigma[i]
)
.into());
}
}
Ok(())
}
fn solve<'a, T: SurfaceTarget3>(
lm: &LevenbergMarquardt<f64>,
problem: PointsToSurface3<'a, T>,
) -> (PointsToSurface3<'a, T>, TerminationReason) {
let (result, report) = lm.minimize(problem);
(result, report.termination)
}
fn resolve_sigma_max<T: SurfaceTarget3>(
opts: &AlignOptions3,
problem: &PointsToSurface3<'_, T>,
) -> Option<f64> {
match opts.sigma_max {
Some(s) => Some(s),
None => estimate_sigma_max(&problem.normalized_residuals()),
}
}
struct PointsToSurface3<'a, T: SurfaceTarget3> {
points: &'a [Point3],
target: &'a T,
params: AlignParams3,
current: AlignValues3,
moved: Vec<Point3>,
closest: Vec<AlignSurfMatch3>,
residuals: Vec<f64>,
test_sigma: Vec<f64>,
inv_sigma: Vec<f64>,
target_weights: Vec<f64>,
magsac_weights: Vec<f64>,
ignore_off_target: bool,
}
impl<'a, T: SurfaceTarget3> PointsToSurface3<'a, T> {
fn new(
points: &'a [Point3],
target: &'a T,
params: AlignParams3,
opts: &AlignOptions3,
) -> Self {
let current = params.compute_values();
let test_sigma = match opts.point_sigma {
Some(s) => s.to_vec(),
None => vec![0.0; points.len()],
};
let mut x = Self {
points,
target,
params,
current,
moved: vec![Point3::origin(); points.len()],
closest: vec![AlignSurfMatch3::default(); points.len()],
residuals: vec![0.0; points.len()],
test_sigma,
inv_sigma: vec![1.0; points.len()],
target_weights: vec![1.0; points.len()],
magsac_weights: vec![1.0; points.len()],
ignore_off_target: opts.ignore_off_target,
};
x.move_points();
x.refresh_inv_sigma();
x
}
fn move_points(&mut self) {
self.current = self.params.compute_values();
let transform = self.current.transform;
for i in 0..self.points.len() {
let m = transform * self.points[i];
let c = self.target.find_align_match(&m);
self.residuals[i] = dist(&m, &c.point) * c.scalar_projection(&m).signum();
self.target_weights[i] = if self.ignore_off_target {
c.weight * f64::from(c.is_on)
} else {
c.weight
};
self.moved[i] = m;
self.closest[i] = c;
}
}
fn restore(&mut self, params: AlignParams3) {
self.params = params;
self.move_points();
}
fn refresh_inv_sigma(&mut self) {
for i in 0..self.points.len() {
let test = self.test_sigma[i];
let target = self.closest[i].sigma;
let combined = (test * test + target * target).sqrt();
self.inv_sigma[i] = if combined > 0.0 { 1.0 / combined } else { 1.0 };
}
}
fn normalized_residuals(&self) -> Vec<f64> {
self.residuals
.iter()
.zip(self.inv_sigma.iter())
.map(|(r, inv)| r * inv)
.collect()
}
fn scale(&self, i: usize) -> f64 {
(self.target_weights[i] * self.magsac_weights[i]).sqrt() * self.inv_sigma[i]
}
fn apply_magsac_weights(&mut self, weighting: &MagsacWeight) {
for i in 0..self.points.len() {
let r = (self.residuals[i] * self.inv_sigma[i]).abs();
self.magsac_weights[i] = weighting.weight(r);
}
}
fn count_if_reweighted(&self, weighting: &MagsacWeight) -> usize {
(0..self.points.len())
.filter(|&i| {
let r = (self.residuals[i] * self.inv_sigma[i]).abs();
self.target_weights[i] * weighting.weight(r) > 0.0
})
.count()
}
}
impl<T: SurfaceTarget3> LeastSquaresProblem<f64, Dyn, U6> for PointsToSurface3<'_, T> {
type ResidualStorage = Owned<f64, Dyn, U1>;
type JacobianStorage = Owned<f64, Dyn, U6>;
type ParameterStorage = Owned<f64, U6>;
fn set_params(&mut self, x: &Vector<f64, U6, Self::ParameterStorage>) {
self.params.set_storage(*x);
self.move_points();
}
fn params(&self) -> Vector<f64, U6, Self::ParameterStorage> {
self.params.storage()
}
fn residuals(&self) -> Option<Vector<f64, Dyn, Self::ResidualStorage>> {
let mut res = Matrix::<f64, Dyn, U1, Self::ResidualStorage>::zeros(self.points.len());
for i in 0..self.points.len() {
res[i] = self.residuals[i] * self.scale(i);
}
Some(res)
}
fn jacobian(&self) -> Option<Matrix<f64, Dyn, U6, Self::JacobianStorage>> {
let mut jac = Matrix::<f64, Dyn, U6, Self::JacobianStorage>::zeros(self.points.len());
for (i, (p, c)) in self.moved.iter().zip(self.closest.iter()).enumerate() {
let values = point_surf_jacobian(p, c, &self.current) * self.scale(i);
copy_jacobian(&values, &mut jac, i);
}
Some(jac)
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::common::points::{mean_point, transform_points};
use crate::na::{Translation3, UnitQuaternion};
use crate::tests::engine_blade;
use crate::{Iso3, Mesh3, SelectOp, Selection, Vector3};
use approx::assert_relative_eq;
use std::f64::consts::PI;
fn box_mesh() -> Mesh3 {
Mesh3::create_box(10.0, 5.0, 2.0, false)
}
fn box_points(mesh: &Mesh3) -> Vec<Point3> {
mesh.sample_poisson(0.5, None).unwrap().points().to_vec()
}
fn small_disturbance() -> Iso3 {
Iso3::from_parts(
Translation3::new(0.3, -0.2, 0.1),
UnitQuaternion::from_euler_angles(PI / 60.0, -PI / 90.0, PI / 45.0),
)
}
#[test]
fn simple_box_disturbed() -> Result<()> {
let mesh = box_mesh();
let points = mesh.sample_poisson(0.1, None).unwrap().points().to_vec();
let disturb = Iso3::from_parts(
Translation3::new(3.0, 2.0, 1.0),
UnitQuaternion::from_euler_angles(PI / 8.0, PI / 12.0, PI / 16.0),
);
let params = AlignParams3::from_origin(None);
let to_align = transform_points(&points, &disturb);
let outcome = points_to_surface3(&to_align, &mesh, params, &AlignOptions3::default())?;
assert_relative_eq!(
disturb.inverse(),
outcome.alignment().full_transform(),
epsilon = 1e-8
);
Ok(())
}
#[test]
fn blade_example() -> Result<()> {
let mesh = engine_blade();
let mask = mesh
.face_select(Selection::None)
.facing(&Vector3::y(), PI / 4.0, SelectOp::Add)
.take_mask();
let expected_points = mesh
.sample_poisson(2.0, Some(&mask))
.unwrap()
.points()
.to_vec();
let disturb = Iso3::from_parts(
Translation3::new(-100.0, 150.0, 0.0),
UnitQuaternion::new(Vector3::new(1.0, 1.0, 1.0).normalize() * PI / 6.0),
);
let to_align = transform_points(&expected_points, &disturb);
let params = AlignParams3::from_center(mean_point(&to_align), None);
let outcome = points_to_surface3(&to_align, &mesh, params, &AlignOptions3::default())?;
let aligned = transform_points(&to_align, outcome.alignment().full_transform());
let max_deviation = aligned
.iter()
.zip(expected_points.iter())
.map(|(a, e)| (a - e).norm())
.max_by(|a, b| a.partial_cmp(b).unwrap())
.unwrap();
assert!(
max_deviation < 1e-6,
"Max deviation is too high: {}",
max_deviation
);
Ok(())
}
#[test]
fn locked_tx_is_not_recovered() -> Result<()> {
use crate::geom3::align3::Dof6;
let mesh = box_mesh();
let points = box_points(&mesh);
let disturb = Iso3::translation(0.4, 0.0, 0.0);
let to_align = transform_points(&points, &disturb);
let dof = Dof6::new(false, true, true, true, true, true);
let params = AlignParams3::from_origin(Some(dof));
let outcome = points_to_surface3(&to_align, &mesh, params, &AlignOptions3::default())?;
assert_eq!(
outcome.alignment().full_transform().translation.vector.x,
0.0
);
Ok(())
}
#[test]
fn gross_outliers_are_rejected() -> Result<()> {
let mesh = box_mesh();
let expected = box_points(&mesh);
let disturb = small_disturbance();
let mut to_align = transform_points(&expected, &disturb);
let n = to_align.len();
for k in 0..n / 10 {
to_align[k * 10] += Vector3::new(0.0, 0.0, 3.0);
}
let params = AlignParams3::from_center(mean_point(&to_align), None);
let naive = points_to_surface3(
&to_align,
&mesh,
params.clone(),
&AlignOptions3::no_refine(),
)?;
let robust = points_to_surface3(&to_align, &mesh, params, &AlignOptions3::default())?;
let clean: Vec<usize> = (0..n).filter(|i| !i.is_multiple_of(10)).collect();
let dev_of = |a: &AlignOutcome3| {
let t = transform_points(&to_align, a.alignment().full_transform());
clean
.iter()
.map(|&i| (t[i] - expected[i]).norm())
.fold(0.0_f64, f64::max)
};
let naive_dev = dev_of(&naive);
let robust_dev = dev_of(&robust);
assert!(
robust_dev < naive_dev * 0.25,
"the robust fit should have rejected the outliers: \
naive deviation {naive_dev}, robust deviation {robust_dev}"
);
Ok(())
}
#[test]
fn high_sigma_point_has_less_influence() -> Result<()> {
let mesh = box_mesh();
let expected = box_points(&mesh);
let mut to_align = expected.clone();
let bad = to_align.len() / 2;
to_align[bad] += Vector3::new(0.0, 0.0, 1.0);
let params = AlignParams3::from_center(mean_point(&to_align), None);
let uniform = points_to_surface3(
&to_align,
&mesh,
params.clone(),
&AlignOptions3::no_refine(),
)?;
let mut sigma = vec![0.01; to_align.len()];
sigma[bad] = 100.0;
let weighted = points_to_surface3(
&to_align,
&mesh,
params,
&AlignOptions3 {
refinement_steps: 0,
point_sigma: Some(&sigma),
..Default::default()
},
)?;
let clean: Vec<usize> = (0..expected.len()).filter(|i| *i != bad).collect();
let dev_of = |a: &AlignOutcome3| {
let t = transform_points(&to_align, a.alignment().full_transform());
clean
.iter()
.map(|&i| (t[i] - expected[i]).norm())
.fold(0.0_f64, f64::max)
};
let uniform_dev = dev_of(&uniform);
let weighted_dev = dev_of(&weighted);
assert!(
weighted_dev < uniform_dev * 0.5,
"declaring the point noisy should have cut its influence: \
uniform deviation {uniform_dev}, weighted deviation {weighted_dev}"
);
Ok(())
}
struct WithSigma<'a>(&'a Mesh3, f64);
impl SurfaceTarget3 for WithSigma<'_> {
fn find_align_match(&self, p: &Point3) -> AlignSurfMatch3 {
self.0.find_align_match(p).with_sigma(self.1)
}
}
#[test]
fn target_sigma_combines_in_quadrature() -> Result<()> {
let mesh = box_mesh();
let mut to_align = box_points(&mesh);
let bad = to_align.len() / 2;
to_align[bad] += Vector3::new(0.0, 0.0, 1.0);
to_align[1] += Vector3::new(0.0, 0.5, 0.0);
let target_sigma = 0.04;
let split: Vec<f64> = (0..to_align.len())
.map(|i| if i == bad { 0.5 } else { 0.01 })
.collect();
let combined: Vec<f64> = split
.iter()
.map(|s| (s * s + target_sigma * target_sigma).sqrt())
.collect();
let params = AlignParams3::from_center(mean_point(&to_align), None);
fn opts(sigma: &[f64]) -> AlignOptions3<'_> {
AlignOptions3 {
refinement_steps: 0,
point_sigma: Some(sigma),
..Default::default()
}
}
let a = points_to_surface3(
&to_align,
&WithSigma(&mesh, target_sigma),
params.clone(),
&opts(&split),
)?;
let b = points_to_surface3(
&to_align,
&WithSigma(&mesh, 0.0),
params.clone(),
&opts(&combined),
)?;
assert_relative_eq!(
a.alignment().full_transform().to_matrix(),
b.alignment().full_transform().to_matrix(),
epsilon = 1e-9
);
let c = points_to_surface3(&to_align, &WithSigma(&mesh, 0.0), params, &opts(&split))?;
let delta = (c.alignment().full_transform().to_matrix()
- a.alignment().full_transform().to_matrix())
.norm();
assert!(
delta > 1e-6,
"target sigma had no effect on the alignment (delta {delta})"
);
Ok(())
}
#[test]
fn mesh_reports_its_own_point_stdev() -> Result<()> {
let mut mesh = box_mesh();
mesh.set_point_stdev(Some(vec![0.25; mesh.point_count()]))?;
let m = mesh.find_align_match(&Point3::new(1.0, 1.0, 5.0));
assert_relative_eq!(m.sigma, 0.25, epsilon = 1e-12);
let plain = box_mesh();
assert_eq!(
plain.find_align_match(&Point3::new(1.0, 1.0, 5.0)).sigma,
0.0
);
Ok(())
}
#[test]
fn reported_residuals_are_geometric() -> Result<()> {
let mesh = box_mesh();
let mut to_align = box_points(&mesh);
let bad = to_align.len() / 2;
to_align[bad] += Vector3::new(0.0, 0.0, 0.4);
let sigma = vec![0.01; to_align.len()];
let opts = AlignOptions3 {
refinement_steps: 0,
point_sigma: Some(&sigma),
..Default::default()
};
let params = AlignParams3::from_center(mean_point(&to_align), None);
let outcome = points_to_surface3(&to_align, &mesh, params, &opts)?;
let largest = outcome
.alignment()
.residuals()
.iter()
.map(|r| r.abs())
.fold(0.0_f64, f64::max);
assert!(
largest < 1.0,
"residuals look normalized rather than geometric, largest was {largest}"
);
Ok(())
}
#[test]
fn exhausted_budget_is_reported_not_raised() -> Result<()> {
let mesh = box_mesh();
let disturb = Iso3::from_parts(
Translation3::new(3.0, 2.0, 1.0),
UnitQuaternion::from_euler_angles(PI / 8.0, PI / 12.0, PI / 16.0),
);
let to_align = transform_points(&box_points(&mesh), &disturb);
let opts = AlignOptions3 {
patience: 1,
..Default::default()
};
let outcome = points_to_surface3(&to_align, &mesh, AlignParams3::from_origin(None), &opts)?;
assert_eq!(outcome.quality(), SolveQuality::Unconverged);
assert!(!outcome.converged());
assert!(
outcome.solves().contains(&TerminationReason::LostPatience),
"expected a LostPatience among {:?}",
outcome.solves()
);
let t = outcome.alignment().full_transform().to_matrix();
assert!(t.iter().all(|v| v.is_finite()));
Ok(())
}
#[test]
fn converged_alignment_reports_converged() -> Result<()> {
let mesh = box_mesh();
let to_align = transform_points(&box_points(&mesh), &small_disturbance());
let opts = AlignOptions3 {
sigma_max: Some(0.5),
refinement_steps: 4,
..Default::default()
};
let outcome = points_to_surface3(&to_align, &mesh, AlignParams3::from_origin(None), &opts)?;
assert!(outcome.converged());
assert_eq!(outcome.quality(), SolveQuality::Converged);
assert_eq!(outcome.refinement_rounds(), 4);
assert_eq!(outcome.solves().len(), 5);
assert_eq!(outcome.halt(), None);
Ok(())
}
#[test]
fn degenerate_noise_estimate_halts_refinement() -> Result<()> {
struct ExactTarget;
impl SurfaceTarget3 for ExactTarget {
fn find_align_match(&self, p: &Point3) -> AlignSurfMatch3 {
AlignSurfMatch3::new(*p, Vector3::z_axis(), true, 1.0)
}
}
let mesh = box_mesh();
let to_align = box_points(&mesh);
let outcome = points_to_surface3(
&to_align,
&ExactTarget,
AlignParams3::from_origin(None),
&AlignOptions3::default(),
)?;
assert_eq!(outcome.halt(), Some(&RefinementHalt::NoNoiseEstimate));
assert_eq!(outcome.refinement_rounds(), 0);
Ok(())
}
#[test]
fn underdetermined_reweighting_halts_refinement() -> Result<()> {
let mesh = box_mesh();
let expected = box_points(&mesh);
let center = mean_point(&expected);
let to_align: Vec<Point3> = expected
.iter()
.map(|p| center + (p - center) * 1.5)
.collect();
let opts = AlignOptions3 {
sigma_max: Some(1e-6),
..Default::default()
};
let outcome = points_to_surface3(
&to_align,
&mesh,
AlignParams3::from_center(center, None),
&opts,
)?;
assert_eq!(
outcome.halt(),
Some(&RefinementHalt::Underdetermined {
weighted: 0,
params: N_PARAMS
})
);
assert_eq!(outcome.refinement_rounds(), 0);
assert!(
outcome
.alignment()
.residuals()
.iter()
.all(|r| r.is_finite())
);
Ok(())
}
#[test]
fn failed_initial_solve_is_an_error() {
let mesh = box_mesh();
let result = points_to_surface3(
&[],
&mesh,
AlignParams3::from_origin(None),
&AlignOptions3::default(),
);
let err = result.unwrap_err().to_string();
assert!(err.contains("Failed to align"), "unexpected message: {err}");
}
#[test]
fn patience_is_validated() {
let mesh = box_mesh();
let points = box_points(&mesh);
let opts = AlignOptions3 {
patience: 0,
..Default::default()
};
let err = points_to_surface3(&points, &mesh, AlignParams3::from_origin(None), &opts)
.unwrap_err()
.to_string();
assert!(err.contains("patience"), "unexpected message: {err}");
}
#[test]
fn sigma_max_is_validated() {
let mesh = box_mesh();
let points = box_points(&mesh);
for bad in [0.0, -1.0, f64::NAN, f64::INFINITY] {
let opts = AlignOptions3 {
sigma_max: Some(bad),
..Default::default()
};
let result = points_to_surface3(&points, &mesh, AlignParams3::from_origin(None), &opts);
assert!(result.is_err(), "sigma_max {bad} should have been rejected");
}
}
#[test]
fn point_sigma_is_validated() {
let mesh = box_mesh();
let points = box_points(&mesh);
let short = vec![0.1; points.len() - 1];
let err = points_to_surface3(
&points,
&mesh,
AlignParams3::from_origin(None),
&AlignOptions3 {
point_sigma: Some(&short),
..Default::default()
},
)
.unwrap_err()
.to_string();
assert!(err.contains("entries"), "unexpected message: {err}");
for bad in [0.0, -1.0, f64::NAN, f64::INFINITY] {
let mut sigma = vec![0.1; points.len()];
sigma[3] = bad;
let result = points_to_surface3(
&points,
&mesh,
AlignParams3::from_origin(None),
&AlignOptions3 {
point_sigma: Some(&sigma),
..Default::default()
},
);
assert!(
result.is_err(),
"sigma value {bad} should have been rejected"
);
}
}
}