use crate::common::consensus::weights::{MagsacWeight, estimate_sigma_max};
use crate::common::points::dist;
use crate::common::{PCoords, RefinementHalt, SPCoords, SolveQuality, TerminationReason};
use crate::geom2::{Boundary2, Manifold1Pos2};
use crate::na::{DVector, Dyn, Matrix, Owned, U1, Vector};
use crate::{Point2, Result, SurfacePoint2, VecDot};
use levenberg_marquardt::{LeastSquaresProblem, LevenbergMarquardt};
pub type BndBuildFn = Box<dyn Fn(&DVector<f64>) -> Result<Boundary2>>;
const DELTA: f64 = 1e-6;
fn signed_residual(m: &Manifold1Pos2, p: &impl PCoords<2>) -> f64 {
dist(m, p) * m.scalar_projection(p).signum()
}
const RESIDUAL_DOF: usize = 2;
const DEFAULT_PATIENCE: usize = 100;
#[derive(Clone, Copy, Debug)]
pub struct BoundaryFitOptions {
pub refinement_steps: usize,
pub sigma_max: Option<f64>,
pub patience: usize,
}
impl Default for BoundaryFitOptions {
fn default() -> Self {
Self {
refinement_steps: 0,
sigma_max: None,
patience: DEFAULT_PATIENCE,
}
}
}
impl BoundaryFitOptions {
pub fn robust() -> Self {
Self {
refinement_steps: 4,
..Default::default()
}
}
fn validate(&self) -> Result<()> {
if self.patience == 0 {
return Err("patience must be greater than zero".into());
}
if let Some(s) = self.sigma_max
&& (!s.is_finite() || s <= 0.0)
{
return Err(
format!("sigma_max is {s}, but must be finite and strictly positive").into(),
);
}
Ok(())
}
}
#[derive(Debug)]
pub struct BoundaryFitResult {
pub params: DVector<f64>,
pub residuals: DVector<f64>,
solves: Vec<TerminationReason>,
halt: Option<RefinementHalt>,
}
impl BoundaryFitResult {
pub(crate) fn new(
params: DVector<f64>,
residuals: DVector<f64>,
solves: Vec<TerminationReason>,
halt: Option<RefinementHalt>,
) -> Self {
Self {
params,
residuals,
solves,
halt,
}
}
pub fn solves(&self) -> &[TerminationReason] {
&self.solves
}
pub fn refinement_rounds(&self) -> usize {
self.solves.len().saturating_sub(1)
}
pub fn quality(&self) -> SolveQuality {
self.solves
.iter()
.map(SolveQuality::from_termination)
.fold(SolveQuality::Converged, SolveQuality::worse_of)
}
pub fn converged(&self) -> bool {
self.quality() == SolveQuality::Converged
}
pub fn halt(&self) -> Option<&RefinementHalt> {
self.halt.as_ref()
}
}
pub fn fit_boundary_to_points(
points: &[Point2],
builder: &BndBuildFn,
initial: DVector<f64>,
ignore_ends: bool,
opts: &BoundaryFitOptions,
) -> Result<BoundaryFitResult> {
let fitting = BoundaryToPoints::new(points, ignore_ends);
solve(&fitting, builder, initial, opts)
}
struct BoundaryToPoints<'a> {
points: &'a [Point2],
ignore_ends: bool,
}
impl<'a> BoundaryToPoints<'a> {
fn new(points: &'a [Point2], ignore_ends: bool) -> Self {
BoundaryToPoints {
points,
ignore_ends,
}
}
}
impl BoundaryFittable for BoundaryToPoints<'_> {
fn residuals_and_weights(&self, boundary: &Boundary2) -> (DVector<f64>, DVector<f64>) {
let bounds = f64::EPSILON..(boundary.length() - f64::EPSILON);
let mut res = DVector::zeros(self.points.len());
let mut weights = DVector::zeros(self.points.len());
weights.fill(1.0);
for i in 0..self.points.len() {
let (_, m) = boundary.at_closest_to_point(&self.points[i]);
if self.ignore_ends {
weights[i] = if bounds.contains(&m.l) { 1.0 } else { 0.0 };
}
res[i] = signed_residual(&m, &self.points[i]);
}
(res, weights)
}
fn residual_only(&self, sample_i: usize, boundary: &Boundary2) -> f64 {
let (_, m) = boundary.at_closest_to_point(&self.points[sample_i]);
signed_residual(&m, &self.points[sample_i])
}
}
pub fn fit_boundary_to_surface_points(
points: &[SurfacePoint2],
builder: &BndBuildFn,
initial: DVector<f64>,
weight_mode: VecDot,
ignore_ends: bool,
opts: &BoundaryFitOptions,
) -> Result<BoundaryFitResult> {
let fitting = BoundaryToSurfacePoints::new(points, weight_mode, ignore_ends);
solve(&fitting, builder, initial, opts)
}
struct BoundaryToSurfacePoints<'a> {
points: &'a [SurfacePoint2],
weight_mode: VecDot,
ignore_ends: bool,
}
impl<'a> BoundaryToSurfacePoints<'a> {
fn new(points: &'a [SurfacePoint2], weight_mode: VecDot, ignore_ends: bool) -> Self {
BoundaryToSurfacePoints {
points,
weight_mode,
ignore_ends,
}
}
}
impl BoundaryFittable for BoundaryToSurfacePoints<'_> {
fn residuals_and_weights(&self, boundary: &Boundary2) -> (DVector<f64>, DVector<f64>) {
let bounds = f64::EPSILON..(boundary.length() - f64::EPSILON);
let mut res = DVector::zeros(self.points.len());
let mut weights = DVector::zeros(self.points.len());
weights.fill(1.0);
for i in 0..self.points.len() {
let (_, m) = boundary.at_closest_to_point(&self.points[i]);
let dot = self.points[i].normal.dot(&m.normal);
weights[i] = match self.weight_mode {
VecDot::AsIs => dot,
VecDot::Abs => dot.abs(),
VecDot::ClampPos => dot.max(0.0),
};
if self.ignore_ends && !bounds.contains(&m.l) {
weights[i] = 0.0;
}
res[i] = signed_residual(&m, &self.points[i]);
}
(res, weights)
}
fn residual_only(&self, sample_i: usize, boundary: &Boundary2) -> f64 {
let (_, m) = boundary.at_closest_to_point(&self.points[sample_i]);
signed_residual(&m, &self.points[sample_i])
}
}
fn solve(
fitting: &dyn BoundaryFittable,
builder: &BndBuildFn,
initial: DVector<f64>,
opts: &BoundaryFitOptions,
) -> Result<BoundaryFitResult> {
opts.validate()?;
let lm = LevenbergMarquardt::new().with_patience(opts.patience);
let n_params = initial.len();
let problem = BoundaryFit::try_new(fitting, builder, initial)?;
let (mut problem, termination) = run(&lm, problem);
if !SolveQuality::from_termination(&termination).is_usable() {
return Err(format!("Fitting failed: {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 {
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) = run(&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 residuals = problem
.residuals
.clone()
.ok_or("the fitted parameters do not produce a valid boundary")?;
Ok(BoundaryFitResult::new(
problem.params,
residuals,
solves,
halt,
))
}
fn run<'a>(
lm: &LevenbergMarquardt<f64>,
problem: BoundaryFit<'a>,
) -> (BoundaryFit<'a>, TerminationReason) {
let (result, report) = lm.minimize(problem);
(result, report.termination)
}
fn resolve_sigma_max(opts: &BoundaryFitOptions, problem: &BoundaryFit<'_>) -> Option<f64> {
match opts.sigma_max {
Some(s) => Some(s),
None => estimate_sigma_max(problem.residuals.as_ref()?.as_slice()),
}
}
pub trait BoundaryFittable {
fn residuals_and_weights(&self, boundary: &Boundary2) -> (DVector<f64>, DVector<f64>);
fn residual_only(&self, sample_i: usize, boundary: &Boundary2) -> f64;
}
struct BoundaryFit<'a> {
fitting: &'a dyn BoundaryFittable,
params: DVector<f64>,
builder: &'a BndBuildFn,
current: Option<Boundary2>,
residuals: Option<DVector<f64>>,
weights: Option<DVector<f64>>,
magsac_weights: DVector<f64>,
}
impl<'a> BoundaryFit<'a> {
fn try_new(
fitting: &'a dyn BoundaryFittable,
builder: &'a BndBuildFn,
initial: DVector<f64>,
) -> Result<Self> {
let _ = builder(&initial)?;
let mut problem = BoundaryFit {
fitting,
params: DVector::zeros(initial.len()),
builder,
current: None,
residuals: None,
weights: None,
magsac_weights: DVector::zeros(0),
};
problem.set_params(&initial);
let n = problem.residuals.as_ref().map_or(0, |r| r.len());
problem.magsac_weights = DVector::from_element(n, 1.0);
Ok(problem)
}
fn scale(&self, weights: &DVector<f64>, i: usize) -> f64 {
weights[i] * self.magsac_weights[i].sqrt()
}
fn apply_magsac_weights(&mut self, weighting: &MagsacWeight) {
if let Some(residuals) = &self.residuals {
for i in 0..residuals.len() {
self.magsac_weights[i] = weighting.weight(residuals[i].abs());
}
}
}
fn count_if_reweighted(&self, weighting: &MagsacWeight) -> usize {
let (Some(residuals), Some(weights)) = (&self.residuals, &self.weights) else {
return 0;
};
(0..residuals.len())
.filter(|&i| weights[i].abs() * weighting.weight(residuals[i].abs()) > 0.0)
.count()
}
fn restore(&mut self, params: &DVector<f64>) {
self.set_params(params);
}
}
impl LeastSquaresProblem<f64, Dyn, Dyn> for BoundaryFit<'_> {
type ResidualStorage = Owned<f64, Dyn, U1>;
type JacobianStorage = Owned<f64, Dyn, Dyn>;
type ParameterStorage = Owned<f64, Dyn>;
fn set_params(&mut self, x: &Vector<f64, Dyn, Self::ParameterStorage>) {
self.params = x.clone();
if let Ok(boundary) = (self.builder)(&self.params) {
let (residuals, weights) = self.fitting.residuals_and_weights(&boundary);
self.residuals = Some(residuals);
self.weights = Some(weights);
self.current = Some(boundary);
} else {
self.residuals = None;
self.current = None;
}
}
fn params(&self) -> Vector<f64, Dyn, Self::ParameterStorage> {
self.params.clone()
}
fn residuals(&self) -> Option<Vector<f64, Dyn, Self::ResidualStorage>> {
let Some(residuals) = &self.residuals else {
return None;
};
let Some(weights) = &self.weights else {
return None;
};
let mut res = DVector::zeros(residuals.len());
for i in 0..residuals.len() {
res[i] = residuals[i] * self.scale(weights, i);
}
Some(res)
}
fn jacobian(&self) -> Option<Matrix<f64, Dyn, Dyn, Self::JacobianStorage>> {
let Some(residuals) = &self.residuals else {
return None;
};
let Some(weights) = &self.weights else {
return None;
};
let mut jac = Matrix::<f64, Dyn, Dyn, Self::JacobianStorage>::zeros(
residuals.len(),
self.params.len(),
);
for k in 0..self.params.len() {
let mut params = self.params.clone();
params[k] += DELTA;
let Ok(disturbed) = (self.builder)(¶ms) else {
continue;
};
for i in 0..residuals.len() {
let d = self.fitting.residual_only(i, &disturbed);
jac[(i, k)] = self.scale(weights, i) * (d - residuals[i]) / DELTA;
}
}
Some(jac)
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::Vector2;
use crate::common::linear_space;
use crate::common::points::{fill_gaps, to_points};
use crate::common::random_geometry::RandomGeometry2;
use crate::geom2::{BoundaryData2, BoundaryEditor};
use approx::assert_relative_eq;
fn ccw_square() -> Boundary2 {
let mut bdata = BoundaryData2::new_closed();
let mut cursor = bdata.get_cursor(None);
cursor.add_seg_xy(0.0, 0.0);
cursor.add_seg_xy(1.0, 0.0);
cursor.add_seg_xy(1.0, 1.0);
cursor.add_seg_xy(0.0, 1.0);
bdata.try_to_boundary().unwrap()
}
#[test]
fn the_residual_is_signed_by_which_side_of_the_boundary_a_sample_falls_on() {
let square = ccw_square();
let outside = Point2::new(0.5, -0.25);
let inside = Point2::new(0.5, 0.25);
let (_, m_out) = square.at_closest_to_point(&outside);
let (_, m_in) = square.at_closest_to_point(&inside);
let r_out = signed_residual(&m_out, &outside);
let r_in = signed_residual(&m_in, &inside);
assert!(
r_out > 0.0,
"a point outside should be positive, got {r_out}"
);
assert!(r_in < 0.0, "a point inside should be negative, got {r_in}");
assert_relative_eq!(r_out.abs(), 0.25, epsilon = 1e-12);
assert_relative_eq!(r_in.abs(), 0.25, epsilon = 1e-12);
}
#[test]
fn signing_leaves_the_size_of_the_residual_alone() {
let square = ccw_square();
let samples = fill_gaps(
&to_points(&[
[-0.4, -0.4],
[1.4, -0.4],
[1.4, 1.4],
[-0.4, 1.4],
[-0.4, -0.4],
]),
0.1,
);
for p in &samples {
let (_, m) = square.at_closest_to_point(p);
assert_relative_eq!(signed_residual(&m, p).abs(), dist(&m, p), epsilon = 1e-12);
}
}
fn triangle_case() -> (Vec<Point2>, BndBuildFn, DVector<f64>) {
let corners = to_points(&[[1.0, 1.0], [3.0, 2.0], [2.0, 4.0], [1.0, 1.0]]);
let points = fill_gaps(&corners, 0.1);
let builder: BndBuildFn = Box::new(|params: &DVector<f64>| {
let mut bdata = BoundaryData2::new_open(Point2::new(params[0], params[1]));
let mut cursor = bdata.get_cursor(None);
cursor.add_seg_xy(params[2], params[3]);
cursor.add_seg_xy(params[4], params[5]);
cursor.add_seg_xy(params[0], params[1]);
bdata.try_to_boundary()
});
let initial = DVector::from(vec![0.9, 0.9, 3.1, 2.1, 2.1, 4.1]);
(points, builder, initial)
}
#[test]
fn gross_outliers_are_rejected() {
let (mut points, builder, initial) = triangle_case();
let mut rg = RandomGeometry2::from_seed(0xb0_11d);
for p in points.iter_mut() {
*p += Vector2::new(rg.gaussian_f64(0.0, 0.01), rg.gaussian_f64(0.0, 0.01));
}
for (k, p) in points.iter_mut().enumerate() {
if k.is_multiple_of(10) {
*p += Vector2::new(0.0, 0.5);
}
}
let truth = DVector::from(vec![1.0, 1.0, 3.0, 2.0, 2.0, 4.0]);
let err = |r: &BoundaryFitResult| (&r.params - &truth).norm();
let naive = fit_boundary_to_points(
&points,
&builder,
initial.clone(),
false,
&Default::default(),
)
.unwrap();
let robust = fit_boundary_to_points(
&points,
&builder,
initial.clone(),
false,
&BoundaryFitOptions::robust(),
)
.unwrap();
assert!(
err(&robust) < 0.25 * err(&naive),
"robust weighting should have suppressed the outliers: naive {}, robust {}",
err(&naive),
err(&robust)
);
assert_eq!(robust.refinement_rounds(), 4);
assert!(robust.halt().is_none());
let explicit = fit_boundary_to_points(
&points,
&builder,
initial,
false,
&BoundaryFitOptions {
sigma_max: Some(0.02),
..BoundaryFitOptions::robust()
},
)
.unwrap();
assert!(err(&explicit) < 0.25 * err(&naive));
}
#[test]
fn refinement_is_off_by_default() {
let (points, builder, initial) = triangle_case();
let result =
fit_boundary_to_points(&points, &builder, initial, false, &Default::default()).unwrap();
assert_eq!(result.refinement_rounds(), 0);
assert_eq!(result.solves().len(), 1);
assert!(result.halt().is_none());
}
#[test]
fn refinement_on_essentially_exact_data_leaves_the_fit_alone() {
let (points, builder, initial) = triangle_case();
let result = fit_boundary_to_points(
&points,
&builder,
initial,
false,
&BoundaryFitOptions::robust(),
)
.unwrap();
let truth = DVector::from(vec![1.0, 1.0, 3.0, 2.0, 2.0, 4.0]);
assert_relative_eq!(result.params, truth, epsilon = 1.0e-5);
assert!(
result.halt().is_none(),
"unexpected halt: {:?}",
result.halt()
);
}
#[test]
fn a_patience_exhausted_initial_solve_is_reported_rather_than_raised() {
let (mut points, builder, initial) = triangle_case();
let mut rg = RandomGeometry2::from_seed(0xb0_11d);
for p in points.iter_mut() {
*p += Vector2::new(rg.gaussian_f64(0.0, 0.01), rg.gaussian_f64(0.0, 0.01));
}
let opts = BoundaryFitOptions {
patience: 1,
..Default::default()
};
let result = fit_boundary_to_points(&points, &builder, initial, false, &opts).unwrap();
assert_eq!(result.quality(), SolveQuality::Unconverged);
assert!(!result.converged());
}
#[test]
fn invalid_options_are_rejected() {
let (points, builder, initial) = triangle_case();
let bad_patience = BoundaryFitOptions {
patience: 0,
..Default::default()
};
assert!(
fit_boundary_to_points(&points, &builder, initial.clone(), false, &bad_patience)
.is_err()
);
let bad_sigma = BoundaryFitOptions {
sigma_max: Some(-1.0),
..BoundaryFitOptions::robust()
};
assert!(fit_boundary_to_points(&points, &builder, initial, false, &bad_sigma).is_err());
}
#[test]
fn simple_triangle() {
let corners = to_points(&[[1.0, 1.0], [3.0, 2.0], [2.0, 4.0], [1.0, 1.0]]);
let points = fill_gaps(&corners, 0.1);
let builder: BndBuildFn = Box::new(|params: &DVector<f64>| {
let mut bdata = BoundaryData2::new_open(Point2::new(params[0], params[1]));
let mut cursor = bdata.get_cursor(None);
cursor.add_seg_xy(params[2], params[3]);
cursor.add_seg_xy(params[4], params[5]);
cursor.add_seg_xy(params[0], params[1]);
bdata.try_to_boundary()
});
let initial = DVector::from(vec![0.0, 0.0, 4.0, 0.0, 1.0, 7.0]);
let result =
fit_boundary_to_points(&points, &builder, initial, false, &Default::default()).unwrap();
let expected = DVector::from(vec![1.0, 1.0, 3.0, 2.0, 2.0, 4.0]);
assert_relative_eq!(result.params, expected, epsilon = 1.0e-6);
}
#[test]
fn surface_normal_line() {
let good = linear_space(0.0, 10.0, 10)
.iter()
.map(|x| SurfacePoint2::new(Point2::new(*x, 0.0), Vector2::y_axis()))
.collect::<Vec<_>>();
let bad = linear_space(0.0, 10.0, 10)
.iter()
.map(|x| SurfacePoint2::new(Point2::new(*x, 1.0), Vector2::x_axis()))
.collect::<Vec<_>>();
let samples = [good, bad].concat();
let builder: BndBuildFn = Box::new(|params: &DVector<f64>| {
let mut bdata = BoundaryData2::new_open(Point2::new(0.0, params[0]));
let mut cursor = bdata.get_cursor(None);
cursor.add_seg_xy(10.0, params[1]);
bdata.try_to_boundary()
});
let initial = DVector::from(vec![1.0, 1.0]);
let result = fit_boundary_to_surface_points(
&samples,
&builder,
initial,
VecDot::Abs,
false,
&Default::default(),
)
.unwrap();
let expected = DVector::from(vec![0.0, 0.0]);
assert_relative_eq!(result.params, expected, epsilon = 1.0e-6);
}
}