use crate::Result;
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::geom2::Point2;
use crate::geom2::align2::jacobian::{copy_jacobian, point_surf_jacobian};
use crate::geom2::align2::{
AlignOptions2, AlignParams2, AlignSurfMatch2, AlignValues2, SurfaceTarget2,
};
use crate::geom2::{Align2, AlignOutcome2};
use crate::na::{Dyn, Matrix, Owned, U1, U3, Vector};
use levenberg_marquardt::{LeastSquaresProblem, LevenbergMarquardt};
const RESIDUAL_DOF: usize = 2;
pub fn points_to_surface2(
points: &[Point2],
target: &impl SurfaceTarget2,
params: AlignParams2,
opts: &AlignOptions2,
) -> Result<AlignOutcome2> {
validate(points, opts)?;
let lm = LevenbergMarquardt::<f64>::new().with_patience(opts.patience);
let (mut problem, termination) =
solve(&lm, PointsToSurface2::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 = Align2::new(
c.transform,
c.align,
problem.params.local,
problem.params.offset,
problem.residuals.clone(),
);
Ok(AlignOutcome2::new(alignment, solves, halt))
}
const N_PARAMS: usize = 3;
fn validate(points: &[Point2], opts: &AlignOptions2) -> 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: SurfaceTarget2>(
lm: &LevenbergMarquardt<f64>,
problem: PointsToSurface2<'a, T>,
) -> (PointsToSurface2<'a, T>, TerminationReason) {
let (result, report) = lm.minimize(problem);
(result, report.termination)
}
fn resolve_sigma_max<T: SurfaceTarget2>(
opts: &AlignOptions2,
problem: &PointsToSurface2<'_, T>,
) -> Option<f64> {
match opts.sigma_max {
Some(s) => Some(s),
None => estimate_sigma_max(&problem.normalized_residuals()),
}
}
struct PointsToSurface2<'a, T: SurfaceTarget2> {
points: &'a [Point2],
target: &'a T,
params: AlignParams2,
current: AlignValues2,
moved: Vec<Point2>,
closest: Vec<AlignSurfMatch2>,
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: SurfaceTarget2> PointsToSurface2<'a, T> {
fn new(
points: &'a [Point2],
target: &'a T,
params: AlignParams2,
opts: &AlignOptions2,
) -> 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![Point2::origin(); points.len()],
closest: vec![AlignSurfMatch2::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: AlignParams2) {
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: SurfaceTarget2> LeastSquaresProblem<f64, Dyn, U3> for PointsToSurface2<'_, T> {
type ResidualStorage = Owned<f64, Dyn, U1>;
type JacobianStorage = Owned<f64, Dyn, U3>;
type ParameterStorage = Owned<f64, U3>;
fn set_params(&mut self, x: &Vector<f64, U3, Self::ParameterStorage>) {
self.params.set_storage(*x);
self.move_points();
}
fn params(&self) -> Vector<f64, U3, 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, U3, Self::JacobianStorage>> {
let mut jac = Matrix::<f64, Dyn, U3, 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::geom2::align2::{AlignStorage2, Dof3};
use crate::geom2::{Boundary2, BoundaryData2, BoundaryEditor};
use crate::{Curve2, Iso2};
use approx::assert_relative_eq;
use std::f64::consts::PI;
fn to_pts(v: &[(f64, f64)]) -> Vec<Point2> {
v.iter().map(|(x, y)| Point2::new(*x, *y)).collect()
}
fn closed_curve(p: &[(f64, f64)]) -> Curve2 {
Curve2::from_points(&to_pts(p), 1e-8, true).unwrap()
}
fn rect_curve() -> Curve2 {
closed_curve(&[(0.0, 0.0), (5.0, 0.0), (5.0, 1.0), (0.0, 1.0)])
}
fn rect_points() -> Vec<Point2> {
to_pts(&[
(1.0, 0.0),
(2.0, 0.0),
(3.0, 0.0),
(5.0, 0.25),
(5.0, 0.75),
(1.0, 1.0),
(2.0, 1.0),
(3.0, 1.0),
(0.0, 0.25),
(0.0, 0.75),
])
}
fn stadium_boundary() -> Boundary2 {
let mut data = BoundaryData2::new_closed();
let mut cursor = data.get_cursor(None);
cursor.add_seg_xy(2.0, 0.0);
cursor.add_arc_xy(2.0, 0.5, 2.0, 1.0, false);
cursor.add_seg_xy(0.0, 1.0);
cursor.add_arc_xy(0.0, 0.5, 0.0, 0.0, false);
data.try_to_boundary().unwrap()
}
fn small_disturbance() -> Iso2 {
Iso2::new(crate::Vector2::new(0.05, -0.05), 10.0 * PI / 180.0)
}
fn max_deviation(moved: &[Point2], expected: &[Point2], result: &AlignOutcome2) -> f64 {
transform_points(moved, result.alignment().full_transform())
.iter()
.zip(expected.iter())
.map(|(a, e)| (a - e).norm())
.fold(0.0_f64, f64::max)
}
#[test]
fn curve_recovers_disturbance() -> Result<()> {
let curve = rect_curve();
let disturb = small_disturbance();
let moved = transform_points(&rect_points(), &disturb);
let params = AlignParams2::from_origin(None);
let result = points_to_surface2(&moved, &curve, params, &AlignOptions2::default())?;
assert_relative_eq!(
result.alignment().full_transform().to_matrix(),
disturb.inverse().to_matrix(),
epsilon = 1e-10
);
Ok(())
}
#[test]
fn curve_recovers_disturbance_at_center() -> Result<()> {
let curve = rect_curve();
let disturb = small_disturbance();
let moved = transform_points(&rect_points(), &disturb);
let params = AlignParams2::from_center(mean_point(&moved), None);
let result = points_to_surface2(&moved, &curve, params, &AlignOptions2::default())?;
assert_relative_eq!(
result.alignment().full_transform().to_matrix(),
disturb.inverse().to_matrix(),
epsilon = 1e-10
);
Ok(())
}
#[test]
fn boundary_with_arcs_recovers_disturbance() -> Result<()> {
let boundary = stadium_boundary();
let points = boundary.to_points(1e-4)?;
let disturb = Iso2::new(crate::Vector2::new(0.03, -0.02), 5.0 * PI / 180.0);
let moved = transform_points(&points, &disturb);
let params = AlignParams2::from_center(mean_point(&moved), None);
let result = points_to_surface2(&moved, &boundary, params, &AlignOptions2::default())?;
assert_relative_eq!(
result.alignment().full_transform().to_matrix(),
disturb.inverse().to_matrix(),
epsilon = 1e-6
);
Ok(())
}
#[test]
fn locked_tx_is_not_recovered() -> Result<()> {
let curve = rect_curve();
let points = rect_points();
let disturb = Iso2::translation(0.3, 0.0);
let moved = transform_points(&points, &disturb);
let dof = Dof3::new(false, true, true);
let params = AlignParams2::from_origin(Some(dof));
let result = points_to_surface2(&moved, &curve, params, &AlignOptions2::default())?;
assert_eq!(
result.alignment().full_transform().translation.vector.x,
0.0
);
let max_dev = max_deviation(&moved, &points, &result);
assert!(
max_dev > 0.1,
"locked tx should have prevented recovery, max deviation was {max_dev}"
);
Ok(())
}
#[test]
fn clean_data_recovery_survives_irls() -> Result<()> {
let curve = rect_curve();
let disturb = small_disturbance();
let moved = transform_points(&rect_points(), &disturb);
let opts = AlignOptions2 {
sigma_max: Some(0.5),
refinement_steps: 4,
..Default::default()
};
let params = AlignParams2::from_origin(None);
let result = points_to_surface2(&moved, &curve, params, &opts)?;
assert_relative_eq!(
result.alignment().full_transform().to_matrix(),
disturb.inverse().to_matrix(),
epsilon = 1e-10
);
Ok(())
}
#[test]
fn gross_outliers_are_rejected() -> Result<()> {
let curve = rect_curve();
let expected = rect_points();
let disturb = small_disturbance();
let mut moved = transform_points(&expected, &disturb);
moved[2] += crate::Vector2::new(0.0, -1.5);
moved[7] += crate::Vector2::new(0.0, 2.0);
let params = AlignParams2::from_center(mean_point(&moved), None);
let naive = points_to_surface2(
&moved,
&curve,
params.clone(),
&AlignOptions2 {
refinement_steps: 0,
..Default::default()
},
)?;
let robust = points_to_surface2(&moved, &curve, params, &AlignOptions2::default())?;
let clean: Vec<usize> = (0..expected.len()).filter(|i| *i != 2 && *i != 7).collect();
let dev_of = |a: &AlignOutcome2| {
let t = transform_points(&moved, 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!(
naive_dev > 0.05,
"the outliers should have pulled the unweighted fit off, but deviation was {naive_dev}"
);
assert!(
robust_dev < 1e-6,
"the robust fit should have rejected the outliers, but deviation was {robust_dev}"
);
Ok(())
}
#[test]
fn high_sigma_point_has_less_influence() -> Result<()> {
let curve = rect_curve();
let expected = rect_points();
let mut moved = expected.clone();
let bad = 4;
moved[bad] += crate::Vector2::new(0.4, 0.0);
let params = AlignParams2::from_center(mean_point(&moved), None);
let uniform = points_to_surface2(
&moved,
&curve,
params.clone(),
&AlignOptions2 {
refinement_steps: 0,
..Default::default()
},
)?;
let mut sigma = vec![0.01; moved.len()];
sigma[bad] = 10.0;
let weighted = points_to_surface2(
&moved,
&curve,
params,
&AlignOptions2 {
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: &AlignOutcome2| {
let t = transform_points(&moved, 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.1,
"declaring the point noisy should have cut its influence sharply: \
uniform deviation {uniform_dev}, weighted deviation {weighted_dev}"
);
Ok(())
}
struct WithSigma<'a>(&'a Curve2, f64);
impl SurfaceTarget2 for WithSigma<'_> {
fn find_align_match(&self, p: &Point2) -> AlignSurfMatch2 {
self.0.find_align_match(p).with_sigma(self.1)
}
}
#[test]
fn target_sigma_combines_in_quadrature() -> Result<()> {
let curve = rect_curve();
let expected = rect_points();
let mut moved = expected.clone();
moved[4] += crate::Vector2::new(0.4, 0.0);
moved[1] += crate::Vector2::new(0.0, -0.2);
let target_sigma = 0.04;
let split: Vec<f64> = (0..moved.len())
.map(|i| if i == 4 { 0.5 } else { 0.01 })
.collect();
let combined: Vec<f64> = split
.iter()
.map(|s| (s * s + target_sigma * target_sigma).sqrt())
.collect();
let params = AlignParams2::from_center(mean_point(&moved), None);
fn opts(sigma: &[f64]) -> AlignOptions2<'_> {
AlignOptions2 {
refinement_steps: 0,
point_sigma: Some(sigma),
..Default::default()
}
}
let a = points_to_surface2(
&moved,
&WithSigma(&curve, target_sigma),
params.clone(),
&opts(&split),
)?;
let b = points_to_surface2(
&moved,
&WithSigma(&curve, 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_surface2(&moved, &WithSigma(&curve, 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 target_sigma_alone_is_enough() -> Result<()> {
let curve = rect_curve();
let moved = rect_points();
let params = AlignParams2::from_center(mean_point(&moved), None);
let with = points_to_surface2(
&moved,
&WithSigma(&curve, 0.05),
params.clone(),
&AlignOptions2::default(),
)?;
let without = points_to_surface2(
&moved,
&WithSigma(&curve, 0.0),
params,
&AlignOptions2::default(),
)?;
for a in [&with, &without] {
for r in a.alignment().residuals() {
assert!(r.is_finite(), "residual was not finite: {r}");
}
}
assert_relative_eq!(
with.alignment().full_transform().to_matrix(),
without.alignment().full_transform().to_matrix(),
epsilon = 1e-9
);
Ok(())
}
#[test]
fn reported_residuals_are_geometric() -> Result<()> {
let curve = rect_curve();
let expected = rect_points();
let mut moved = expected.clone();
moved[4] += crate::Vector2::new(0.4, 0.0);
let sigma = vec![0.01; moved.len()];
let opts = AlignOptions2 {
refinement_steps: 0,
point_sigma: Some(&sigma),
..Default::default()
};
let params = AlignParams2::from_center(mean_point(&moved), None);
let result = points_to_surface2(&moved, &curve, params, &opts)?;
let largest = result
.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 point_sigma_length_is_validated() {
let curve = rect_curve();
let points = rect_points();
let sigma = vec![0.1; points.len() - 1];
let opts = AlignOptions2 {
point_sigma: Some(&sigma),
..Default::default()
};
let err = points_to_surface2(&points, &curve, AlignParams2::from_origin(None), &opts)
.unwrap_err()
.to_string();
assert!(err.contains("entries"), "unexpected message: {err}");
}
#[test]
fn point_sigma_values_are_validated() {
let curve = rect_curve();
let points = rect_points();
for bad in [0.0, -1.0, f64::NAN, f64::INFINITY] {
let mut sigma = vec![0.1; points.len()];
sigma[3] = bad;
let opts = AlignOptions2 {
point_sigma: Some(&sigma),
..Default::default()
};
let result =
points_to_surface2(&points, &curve, AlignParams2::from_origin(None), &opts);
assert!(
result.is_err(),
"sigma value {bad} should have been rejected"
);
}
}
#[test]
fn exhausted_budget_is_reported_not_raised() -> Result<()> {
let curve = rect_curve();
let moved = transform_points(&rect_points(), &small_disturbance());
let opts = AlignOptions2 {
patience: 1,
..Default::default()
};
let outcome = points_to_surface2(&moved, &curve, AlignParams2::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 curve = rect_curve();
let moved = transform_points(&rect_points(), &small_disturbance());
let opts = AlignOptions2 {
sigma_max: Some(0.5),
refinement_steps: 4,
..Default::default()
};
let outcome = points_to_surface2(&moved, &curve, AlignParams2::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<()> {
let curve = rect_curve();
let moved = rect_points();
let outcome = points_to_surface2(
&moved,
&curve,
AlignParams2::from_origin(None),
&AlignOptions2::default(),
)?;
assert_eq!(outcome.halt(), Some(&RefinementHalt::NoNoiseEstimate));
assert_eq!(outcome.refinement_rounds(), 0);
assert!(outcome.converged());
Ok(())
}
#[test]
fn underdetermined_reweighting_halts_refinement() -> Result<()> {
let curve = rect_curve();
let expected = rect_points();
let center = mean_point(&expected);
let moved: Vec<Point2> = expected
.iter()
.map(|p| center + (p - center) * 1.5)
.collect();
let opts = AlignOptions2 {
sigma_max: Some(1e-6),
..Default::default()
};
let outcome = points_to_surface2(
&moved,
&curve,
AlignParams2::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 curve = rect_curve();
let result = points_to_surface2(
&[],
&curve,
AlignParams2::from_origin(None),
&AlignOptions2::default(),
);
let err = result.unwrap_err().to_string();
assert!(err.contains("Failed to align"), "unexpected message: {err}");
}
#[test]
fn restore_rolls_back_params_and_residuals() {
let curve = rect_curve();
let moved = transform_points(&rect_points(), &small_disturbance());
let opts = AlignOptions2::default();
let mut problem =
PointsToSurface2::new(&moved, &curve, AlignParams2::from_origin(None), &opts);
let good = problem.params.clone();
let good_storage = good.storage();
let good_residuals = problem.residuals.clone();
problem
.params
.set_storage(AlignStorage2::new(9.0, -4.0, 1.2));
problem.move_points();
assert_ne!(problem.residuals, good_residuals);
problem.restore(good);
assert_eq!(problem.params.storage(), good_storage);
assert_eq!(problem.residuals, good_residuals);
}
#[test]
fn patience_is_validated() {
let curve = rect_curve();
let points = rect_points();
let opts = AlignOptions2 {
patience: 0,
..Default::default()
};
let err = points_to_surface2(&points, &curve, AlignParams2::from_origin(None), &opts)
.unwrap_err()
.to_string();
assert!(err.contains("patience"), "unexpected message: {err}");
}
#[test]
fn sigma_max_is_validated() {
let curve = rect_curve();
let points = rect_points();
for bad in [0.0, -1.0, f64::NAN, f64::INFINITY] {
let opts = AlignOptions2 {
sigma_max: Some(bad),
..Default::default()
};
let result =
points_to_surface2(&points, &curve, AlignParams2::from_origin(None), &opts);
assert!(result.is_err(), "sigma_max {bad} should have been rejected");
}
}
}