use crate::Result;
use crate::common::align::{RefinementHalt, SolveQuality, TerminationReason};
use crate::common::consensus::weights::{MagsacWeight, estimate_sigma_max};
use crate::common::points::dist;
use crate::geom2::align2::curve::{CAPParams, CurveSurfPoint, generate_alignment_points};
use crate::geom2::align2::jacobian::{point_surf_jacobian, point_surf_jacobian_rev};
use crate::geom2::align2::{AlignValues2, Dof3, MultiParams2};
use crate::geom2::{Align2, CurveGroup2, Iso2, MultiOutcome2, Point2, SurfacePoint2, Vector2};
use crate::na::{DMatrix, DVector, Dyn, Matrix, Owned, U1, Vector};
use levenberg_marquardt::{LeastSquaresProblem, LevenbergMarquardt};
use rayon::prelude::*;
const RESIDUAL_DOF: usize = 2;
#[derive(Clone, Copy, Debug)]
pub struct MultiOptions2 {
pub max_distance: f64,
pub max_normal_angle: Option<f64>,
pub refinement_steps: usize,
pub sigma_max: Option<f64>,
pub patience: usize,
pub dof: Option<Dof3>,
}
impl MultiOptions2 {
pub fn new(max_distance: f64) -> Self {
Self {
max_distance,
max_normal_angle: None,
refinement_steps: 4,
sigma_max: None,
patience: 100,
dof: None,
}
}
}
#[derive(Clone, Debug)]
pub struct CurveAlignPoint {
pub group_i: usize,
pub cp: CurveSurfPoint,
pub ref_i: usize,
pub weight: f64,
}
impl CurveAlignPoint {
pub fn new(group_i: usize, cp: CurveSurfPoint, ref_i: usize, weight: f64) -> Self {
Self {
group_i,
cp,
ref_i,
weight,
}
}
}
pub fn multi_curve_adjustment_with_points(
groups: &[CurveGroup2],
initial: Option<&[Iso2]>,
points: Vec<CurveAlignPoint>,
static_i: usize,
opts: &MultiOptions2,
) -> Result<MultiOutcome2> {
let poses = resolve_poses(groups, initial)?;
validate(groups, &points, static_i, opts)?;
solve_bundle(
MultiCurveProblem::new(groups, poses, points, static_i, opts)?,
opts,
)
}
fn resolve_poses(groups: &[CurveGroup2], initial: Option<&[Iso2]>) -> Result<Vec<Iso2>> {
match initial {
None => Ok(vec![Iso2::identity(); groups.len()]),
Some(t) if t.len() == groups.len() => Ok(t.to_vec()),
Some(t) => Err(format!(
"there are {} bodies but {} initial transforms",
groups.len(),
t.len()
)
.into()),
}
}
pub fn multi_curve_adjustment(
groups: &[CurveGroup2],
initial: Option<&[Iso2]>,
opts: &MultiOptions2,
sample_opts: &CAPParams,
) -> Result<MultiOutcome2> {
if groups.len() < 2 {
return Err(format!(
"a multi-curve alignment needs at least two bodies, but {} were given",
groups.len()
)
.into());
}
let poses = resolve_poses(groups, initial)?;
let reference_order = reference_priority(groups, &poses, sample_opts)?;
let static_i = reference_order[0];
let mut work_list = Vec::new();
let mut bodies_to_test = (0..groups.len()).collect::<Vec<_>>();
for ref_i in reference_order {
bodies_to_test.retain(|j| *j != ref_i);
for &group_i in bodies_to_test.iter() {
work_list.push((group_i, ref_i));
}
}
let points = work_list
.par_iter()
.map(|&(group_i, ref_i)| {
let t = poses[ref_i].inv_mul(&poses[group_i]);
let samples =
generate_alignment_points(&groups[group_i], &groups[ref_i], &t, sample_opts);
samples
.into_iter()
.map(|cp| CurveAlignPoint::new(group_i, cp, ref_i, 1.0))
.collect::<Vec<_>>()
})
.flatten()
.collect::<Vec<_>>();
multi_curve_adjustment_with_points(groups, initial, points, static_i, opts)
}
fn validate(
groups: &[CurveGroup2],
points: &[CurveAlignPoint],
static_i: usize,
opts: &MultiOptions2,
) -> 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 !opts.max_distance.is_finite() || opts.max_distance <= 0.0 {
return Err(format!(
"max_distance is {}, but must be finite and strictly positive",
opts.max_distance
)
.into());
}
if static_i >= groups.len() {
return Err(format!(
"the static body index {} is out of range for {} bodies",
static_i,
groups.len()
)
.into());
}
if let Some(p) = points
.iter()
.find(|p| p.group_i >= groups.len() || p.ref_i >= groups.len())
{
return Err(format!(
"an alignment point references body {} against body {}, but there are only {} bodies",
p.group_i,
p.ref_i,
groups.len()
)
.into());
}
Ok(())
}
fn solve_bundle(problem: MultiCurveProblem<'_>, opts: &MultiOptions2) -> Result<MultiOutcome2> {
let lm = LevenbergMarquardt::<f64>::new().with_patience(opts.patience);
let n_params = problem.params.param_count();
let (mut problem, termination) = run(&lm, problem);
if !SolveQuality::from_termination(&termination).is_usable() {
return Err(format!("Failed to align bodies to each other: {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.storage().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);
}
}
}
}
Ok(MultiOutcome2::new(problem.alignments(), solves, halt))
}
fn run<'a>(
lm: &LevenbergMarquardt<f64>,
problem: MultiCurveProblem<'a>,
) -> (MultiCurveProblem<'a>, TerminationReason) {
let (result, report) = lm.minimize(problem);
(result, report.termination)
}
fn resolve_sigma_max(opts: &MultiOptions2, problem: &MultiCurveProblem<'_>) -> Option<f64> {
match opts.sigma_max {
Some(s) => Some(s),
None => estimate_sigma_max(&problem.residuals),
}
}
struct Moved {
p_world: Point2,
c_world: SurfacePoint2,
residual: f64,
target_weight: f64,
}
struct MultiCurveProblem<'a> {
groups: &'a [CurveGroup2],
handles: Vec<CurveAlignPoint>,
params: MultiParams2,
values: Vec<AlignValues2>,
moved: Vec<Point2>,
closest: Vec<SurfacePoint2>,
residuals: Vec<f64>,
target_weights: Vec<f64>,
magsac_weights: Vec<f64>,
max_distance: f64,
min_normal_dot: Option<f64>,
}
impl<'a> MultiCurveProblem<'a> {
fn new(
groups: &'a [CurveGroup2],
poses: Vec<Iso2>,
handles: Vec<CurveAlignPoint>,
static_i: usize,
opts: &MultiOptions2,
) -> Result<Self> {
let centers = groups.iter().map(|g| g.aabb().center()).collect::<Vec<_>>();
let params = MultiParams2::from_centers(static_i, ¢ers, Some(&poses), opts.dof)?;
let n = handles.len();
let values = params.compute_all_values();
let mut item = Self {
groups,
handles,
params,
values,
moved: vec![Point2::origin(); n],
closest: vec![SurfacePoint2::new(Point2::origin(), Vector2::y_axis()); n],
residuals: vec![0.0; n],
target_weights: vec![1.0; n],
magsac_weights: vec![1.0; n],
max_distance: opts.max_distance,
min_normal_dot: opts.max_normal_angle.map(f64::cos),
};
item.move_points();
Ok(item)
}
fn move_points(&mut self) {
self.values = self.params.compute_all_values();
let values = &self.values;
let groups = self.groups;
let max_distance = self.max_distance;
let min_normal_dot = self.min_normal_dot;
let collected: Vec<Moved> = self
.handles
.par_iter()
.map(|h| {
let t_test = values[h.group_i].transform;
let t_ref = values[h.ref_i].transform;
let p_world = t_test * h.cp.sp.point;
let query = t_ref.inverse_transform_point(&p_world);
let (_, station) = groups[h.ref_i].at_closest_to_point(&query);
let c_world = station.surface_point().transformed_by(&t_ref);
let d = dist(&p_world, &c_world.point);
let residual = d * c_world.scalar_projection(&p_world).signum();
let mut weight = h.weight;
if d > max_distance {
weight = 0.0;
}
if let Some(min_dot) = min_normal_dot {
let n_test = t_test.rotation * h.cp.sp.normal;
if n_test.dot(&c_world.normal) < min_dot {
weight = 0.0;
}
}
Moved {
p_world,
c_world,
residual,
target_weight: weight,
}
})
.collect();
for (i, m) in collected.into_iter().enumerate() {
self.moved[i] = m.p_world;
self.closest[i] = m.c_world;
self.residuals[i] = m.residual;
self.target_weights[i] = m.target_weight;
}
}
fn restore(&mut self, storage: &DVector<f64>) {
self.params.set_storage(storage);
self.move_points();
}
fn scale(&self, i: usize) -> f64 {
(self.target_weights[i] * self.magsac_weights[i]).sqrt()
}
fn apply_magsac_weights(&mut self, weighting: &MagsacWeight) {
for i in 0..self.handles.len() {
self.magsac_weights[i] = weighting.weight(self.residuals[i].abs());
}
}
fn count_if_reweighted(&self, weighting: &MagsacWeight) -> usize {
(0..self.handles.len())
.filter(|&i| self.target_weights[i] * weighting.weight(self.residuals[i].abs()) > 0.0)
.count()
}
fn alignments(&self) -> Vec<Align2> {
let mut grouped = vec![Vec::new(); self.groups.len()];
for (i, h) in self.handles.iter().enumerate() {
grouped[h.group_i].push(self.residuals[i]);
}
self.values
.iter()
.enumerate()
.zip(grouped)
.map(|((i, v), residuals)| {
let body = self.params.body(i);
Align2::new(v.transform, v.align, body.local, body.offset, residuals)
})
.collect()
}
}
impl LeastSquaresProblem<f64, Dyn, Dyn> for MultiCurveProblem<'_> {
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.set_storage(x);
self.move_points();
}
fn params(&self) -> Vector<f64, Dyn, Self::ParameterStorage> {
self.params.storage().clone()
}
fn residuals(&self) -> Option<Vector<f64, Dyn, Self::ResidualStorage>> {
let mut res = Matrix::<f64, Dyn, U1, Self::ResidualStorage>::zeros(self.handles.len());
for i in 0..self.handles.len() {
res[i] = self.residuals[i] * self.scale(i);
}
Some(res)
}
fn jacobian(&self) -> Option<Matrix<f64, Dyn, Dyn, Self::JacobianStorage>> {
let mut jac = Matrix::<f64, Dyn, Dyn, Self::JacobianStorage>::zeros(
self.handles.len(),
self.params.param_count(),
);
for (i, h) in self.handles.iter().enumerate() {
let scale = self.scale(i);
let p = &self.moved[i];
let c = &self.closest[i];
let fwd = point_surf_jacobian(p, c, &self.values[h.group_i]) * scale;
self.params.add_jacobian_block(&mut jac, i, h.group_i, &fwd);
let rev = point_surf_jacobian_rev(p, c, &self.values[h.ref_i]) * scale;
self.params.add_jacobian_block(&mut jac, i, h.ref_i, &rev);
}
Some(jac)
}
}
fn reference_priority(
groups: &[CurveGroup2],
poses: &[Iso2],
params: &CAPParams,
) -> Result<Vec<usize>> {
Ok(crate::common::align::reference::reference_priority(
correspondence_matrix(groups, poses, params)?,
))
}
fn correspondence_matrix(
groups: &[CurveGroup2],
poses: &[Iso2],
params: &CAPParams,
) -> Result<DMatrix<f64>> {
crate::common::align::reference::correspondence_matrix(groups.len(), |i, j| {
let t = poses[i].inv_mul(&poses[j]);
Ok(generate_alignment_points(&groups[j], &groups[i], &t, params).len() as f64)
})
.map_err(|e| e.into())
}
#[cfg(test)]
mod tests {
use super::*;
use crate::common::random_geometry::RandomGeometry2;
use crate::geom2::{Curve2, CurveGroup2, Iso2};
use approx::assert_relative_eq;
use std::f64::consts::{PI, TAU};
const WIDE_GATE: f64 = 20.0;
fn test_opts() -> MultiOptions2 {
MultiOptions2::new(WIDE_GATE)
}
fn rect_curve() -> Curve2 {
let points =
[(-5.0, -3.0), (5.0, -3.0), (5.0, 3.0), (-5.0, 3.0)].map(|(x, y)| Point2::new(x, y));
Curve2::from_points(&points, 1e-8, true).unwrap()
}
fn segment_curve(x0: f64, x1: f64) -> Curve2 {
Curve2::from_points(&[Point2::new(x0, 0.0), Point2::new(x1, 0.0)], 1e-8, false).unwrap()
}
fn chain_points(curves: &[Curve2], spacing: f64) -> Vec<CurveAlignPoint> {
let mut points = Vec::new();
for (i, curve) in curves.iter().enumerate().skip(1) {
let n = (curve.length() / spacing).floor() as usize;
for k in 0..n {
let l = (k as f64 + 0.5) * spacing;
if let Some(station) = curve.at_length(l) {
let cp = CurveSurfPoint::from_station(&station, 0);
points.push(CurveAlignPoint::new(i, cp, i - 1, 1.0));
}
}
}
points
}
fn vertical_segment(x: f64, y0: f64, y1: f64) -> Curve2 {
Curve2::from_points(&[Point2::new(x, y0), Point2::new(x, y1)], 1e-8, false).unwrap()
}
fn circle_curve(cx: f64, cy: f64, r: f64) -> Curve2 {
const N: usize = 64;
let points = (0..N)
.map(|i| {
let a = TAU * (i as f64) / (N as f64);
Point2::new(cx + r * a.cos(), cy + r * a.sin())
})
.collect::<Vec<_>>();
Curve2::from_points(&points, 1e-8, true).unwrap()
}
fn two_circle_body() -> CurveGroup2 {
CurveGroup2::new(vec![
circle_curve(-5.0, 0.0, 2.0),
circle_curve(5.0, 0.0, 2.0),
])
.unwrap()
}
fn small_disturbance(tx: f64, ty: f64, rz: f64) -> Iso2 {
Iso2::new(Vector2::new(tx, ty), rz)
}
fn groups_of_one(curves: &[Curve2]) -> Vec<CurveGroup2> {
curves.iter().cloned().map(CurveGroup2::from).collect()
}
#[test]
fn a_disturbed_curve_is_brought_back_onto_its_twin() -> Result<()> {
let curves = vec![rect_curve(), rect_curve()];
let disturb = Iso2::new(Vector2::new(0.2, -0.15), PI / 90.0);
let initial = vec![Iso2::identity(), disturb];
let groups = groups_of_one(&curves);
let points = chain_points(&curves, 0.4);
let outcome =
multi_curve_adjustment_with_points(&groups, Some(&initial), points, 0, &test_opts())?;
assert_eq!(outcome.len(), 2);
assert_relative_eq!(
outcome.alignment(0).full_transform().to_matrix(),
Iso2::identity().to_matrix(),
epsilon = 1e-12
);
assert_relative_eq!(
outcome.alignment(1).full_transform().to_matrix(),
Iso2::identity().to_matrix(),
epsilon = 1e-6
);
Ok(())
}
#[test]
fn three_curves_settle_together() -> Result<()> {
let curves = vec![rect_curve(), rect_curve(), rect_curve()];
let initial = vec![
Iso2::identity(),
Iso2::new(Vector2::new(0.15, -0.1), PI / 120.0),
Iso2::new(Vector2::new(-0.1, 0.12), -PI / 100.0),
];
let groups = groups_of_one(&curves);
let points = chain_points(&curves, 0.5);
let outcome =
multi_curve_adjustment_with_points(&groups, Some(&initial), points, 0, &test_opts())?;
for i in 0..3 {
assert_relative_eq!(
outcome.alignment(i).full_transform().to_matrix(),
Iso2::identity().to_matrix(),
epsilon = 1e-5
);
}
Ok(())
}
#[test]
fn the_static_curve_never_moves() -> Result<()> {
let curves = vec![rect_curve(), rect_curve()];
let initial = vec![Iso2::translation(0.3, 0.0), Iso2::translation(0.0, 0.25)];
let groups = groups_of_one(&curves);
let outcome = multi_curve_adjustment_with_points(
&groups,
Some(&initial),
chain_points(&curves, 0.5),
1,
&test_opts(),
)?;
assert_relative_eq!(
outcome.alignment(1).full_transform().to_matrix(),
initial[1].to_matrix(),
epsilon = 1e-12
);
Ok(())
}
#[test]
fn residuals_are_grouped_by_the_curve_they_came_from() -> Result<()> {
let curves = vec![rect_curve(), rect_curve(), rect_curve()];
let initial = vec![Iso2::identity(); 3];
let groups = groups_of_one(&curves);
let points = chain_points(&curves, 0.6);
let from_1 = points.iter().filter(|p| p.group_i == 1).count();
let from_2 = points.iter().filter(|p| p.group_i == 2).count();
let outcome =
multi_curve_adjustment_with_points(&groups, Some(&initial), points, 0, &test_opts())?;
assert_eq!(outcome.alignment(0).residuals().len(), 0);
assert_eq!(outcome.alignment(1).residuals().len(), from_1);
assert_eq!(outcome.alignment(2).residuals().len(), from_2);
Ok(())
}
#[test]
fn locked_dof_are_honored() -> Result<()> {
let curves = vec![rect_curve(), rect_curve()];
let initial = vec![Iso2::identity(), Iso2::translation(0.3, 0.0)];
let groups = groups_of_one(&curves);
let opts = MultiOptions2 {
dof: Some(Dof3::new(false, true, true)),
..test_opts()
};
let outcome = multi_curve_adjustment_with_points(
&groups,
Some(&initial),
chain_points(&curves, 0.5),
0,
&opts,
)?;
assert_relative_eq!(
outcome.alignment(1).full_transform().translation.vector.x,
0.3,
epsilon = 1e-9
);
Ok(())
}
#[test]
fn exhausted_budget_is_reported_not_raised() -> Result<()> {
let curves = vec![rect_curve(), rect_curve()];
let initial = vec![
Iso2::identity(),
Iso2::new(Vector2::new(1.5, -1.0), PI / 12.0),
];
let groups = groups_of_one(&curves);
let opts = MultiOptions2 {
patience: 1,
..test_opts()
};
let outcome = multi_curve_adjustment_with_points(
&groups,
Some(&initial),
chain_points(&curves, 0.5),
0,
&opts,
)?;
assert_eq!(outcome.quality(), SolveQuality::Unconverged);
assert!(outcome.solves().contains(&TerminationReason::LostPatience));
for a in outcome.alignments() {
assert!(a.full_transform().to_matrix().iter().all(|v| v.is_finite()));
}
Ok(())
}
#[test]
fn gross_outliers_are_rejected() -> Result<()> {
let curves = vec![rect_curve(), rect_curve()];
let disturb = Iso2::new(Vector2::new(0.2, -0.15), PI / 120.0);
let initial = vec![Iso2::identity(), disturb];
let groups = groups_of_one(&curves);
let mut points = chain_points(&curves, 0.4);
for (k, p) in points.iter_mut().enumerate() {
if k.is_multiple_of(10) {
p.cp.sp.point += Vector2::new(0.0, 5.0);
}
}
let naive = multi_curve_adjustment_with_points(
&groups,
Some(&initial),
points.clone(),
0,
&MultiOptions2 {
refinement_steps: 0,
..test_opts()
},
)?;
let robust =
multi_curve_adjustment_with_points(&groups, Some(&initial), points, 0, &test_opts())?;
let error = |o: &MultiOutcome2| {
(o.alignment(1).full_transform().to_matrix() - Iso2::identity().to_matrix()).norm()
};
assert!(
error(&robust) < error(&naive) * 0.5,
"robust weighting should have suppressed the outliers: naive {}, robust {}",
error(&naive),
error(&robust)
);
Ok(())
}
#[test]
fn without_a_distance_gate_the_unweighted_solve_is_dragged() -> Result<()> {
let curves = vec![rect_curve(), rect_curve()];
let initial = vec![Iso2::identity(), Iso2::identity()];
let groups = groups_of_one(&curves);
let mut points = chain_points(&curves, 0.4);
for (k, p) in points.iter_mut().enumerate() {
if k.is_multiple_of(20) {
p.cp.sp.point += Vector2::new(0.0, 8.0);
}
}
let unweighted = |gate: f64| MultiOptions2 {
refinement_steps: 0,
..MultiOptions2::new(gate)
};
let drift = |o: &MultiOutcome2| {
(o.alignment(1).full_transform().to_matrix() - Iso2::identity().to_matrix()).norm()
};
let ungated = multi_curve_adjustment_with_points(
&groups,
Some(&initial),
points.clone(),
0,
&unweighted(WIDE_GATE),
)?;
let gated = multi_curve_adjustment_with_points(
&groups,
Some(&initial),
points,
0,
&unweighted(1.0),
)?;
assert!(
drift(&ungated) > 1e-3,
"the ungated solve should have been pulled off the answer, but moved only {}",
drift(&ungated)
);
assert!(
drift(&gated) < drift(&ungated) * 0.1,
"the gate should have held the solve in place: ungated {}, gated {}",
drift(&ungated),
drift(&gated)
);
Ok(())
}
#[test]
fn the_distance_gate_excludes_far_correspondences() -> Result<()> {
let curves = vec![rect_curve(), rect_curve()];
let initial = vec![Iso2::identity(), Iso2::identity()];
let groups = groups_of_one(&curves);
let mut points = chain_points(&curves, 0.5);
let far = points.len() / 2;
points[far].cp.sp.point += Vector2::new(0.0, 50.0);
let opts = MultiOptions2 {
max_distance: 1.0,
refinement_steps: 0,
..test_opts()
};
let outcome =
multi_curve_adjustment_with_points(&groups, Some(&initial), points, 0, &opts)?;
assert_relative_eq!(
outcome.alignment(1).full_transform().to_matrix(),
Iso2::identity().to_matrix(),
epsilon = 1e-6
);
Ok(())
}
#[test]
fn the_normal_gate_excludes_disagreeing_correspondences() -> Result<()> {
let curves = vec![rect_curve(), rect_curve()];
let initial = [Iso2::identity(), Iso2::identity()];
let groups = groups_of_one(&curves);
let station = curves[1].at_length(5.0).unwrap();
let mut cp = CurveSurfPoint::from_station(&station, 0);
cp.sp.normal = -cp.sp.normal;
let opts = MultiOptions2 {
max_normal_angle: Some(PI / 4.0),
refinement_steps: 0,
..test_opts()
};
let points = vec![CurveAlignPoint::new(1, cp, 0, 1.0)];
let problem = MultiCurveProblem::new(&groups, initial.to_vec(), points, 0, &opts)?;
assert_eq!(problem.target_weights[0], 0.0);
Ok(())
}
#[test]
fn jacobian_matches_finite_differences() -> Result<()> {
let curves = vec![rect_curve(), rect_curve(), rect_curve()];
let initial = [
Iso2::identity(),
Iso2::new(Vector2::new(0.21, -0.13), 0.03),
Iso2::new(Vector2::new(-0.17, 0.11), -0.04),
];
let groups = groups_of_one(&curves);
let probes = [2.0, 5.0, 8.0, 12.0, 14.0, 18.0, 21.0, 24.0, 28.0, 30.0];
let mut points = Vec::new();
for body in [1usize, 2] {
for l in probes {
let cp = CurveSurfPoint::from_station(&curves[body].at_length(l).unwrap(), 0);
points.push(CurveAlignPoint::new(body, cp, body - 1, 1.0));
}
}
let opts = MultiOptions2 {
refinement_steps: 0,
..test_opts()
};
let mut problem = MultiCurveProblem::new(&groups, initial.to_vec(), points, 0, &opts)?;
let x0 = problem.params.storage().clone();
let analytic = problem.jacobian().unwrap();
assert_eq!(
analytic.ncols(),
6,
"two free bodies, three parameters each"
);
let eps = 1e-7;
for k in 0..x0.len() {
let mut lo = x0.clone();
lo[k] -= eps;
problem.set_params(&lo);
let r_lo = problem.residuals().unwrap();
let mut hi = x0.clone();
hi[k] += eps;
problem.set_params(&hi);
let r_hi = problem.residuals().unwrap();
for row in 0..r_lo.len() {
let numeric = (r_hi[row] - r_lo[row]) / (2.0 * eps);
assert_relative_eq!(analytic[(row, k)], numeric, epsilon = 1e-5);
}
}
Ok(())
}
#[test]
fn a_reference_body_receives_jacobian_columns() -> Result<()> {
let curves = vec![rect_curve(), rect_curve(), rect_curve()];
let initial = [
Iso2::identity(),
Iso2::translation(0.2, 0.0),
Iso2::translation(0.0, 0.2),
];
let groups = groups_of_one(&curves);
let cp = CurveSurfPoint::from_station(&curves[2].at_length(5.0).unwrap(), 0);
let points = vec![CurveAlignPoint::new(2, cp, 1, 1.0)];
let problem = MultiCurveProblem::new(&groups, initial.to_vec(), points, 0, &test_opts())?;
let jac = problem.jacobian().unwrap();
let body1 = problem.params.column_offset(1).unwrap();
let body2 = problem.params.column_offset(2).unwrap();
let block_norm =
|start: usize| -> f64 { (0..3).map(|k| jac[(0, start + k)].abs()).sum::<f64>() };
assert!(
block_norm(body2) > 1e-9,
"the test body should have a nonzero jacobian block"
);
assert!(
block_norm(body1) > 1e-9,
"the reference body should have a nonzero jacobian block too"
);
Ok(())
}
#[test]
fn a_self_correspondence_cancels_to_a_zero_row() -> Result<()> {
let curves = vec![rect_curve(), rect_curve()];
let initial = [Iso2::identity(), Iso2::translation(0.3, -0.2)];
let groups = groups_of_one(&curves);
let cp = CurveSurfPoint::from_station(&curves[1].at_length(5.0).unwrap(), 0);
let points = vec![CurveAlignPoint::new(1, cp, 1, 1.0)];
let problem = MultiCurveProblem::new(&groups, initial.to_vec(), points, 0, &test_opts())?;
let jac = problem.jacobian().unwrap();
for k in 0..jac.ncols() {
assert_relative_eq!(jac[(0, k)], 0.0, epsilon = 1e-12);
}
Ok(())
}
#[test]
fn invalid_options_and_indices_are_rejected() {
let curves = vec![rect_curve(), rect_curve()];
let initial = vec![Iso2::identity(); 2];
let groups = groups_of_one(&curves);
let points = chain_points(&curves, 1.0);
let bad_patience = MultiOptions2 {
patience: 0,
..test_opts()
};
assert!(
multi_curve_adjustment_with_points(
&groups,
Some(&initial),
points.clone(),
0,
&bad_patience
)
.is_err()
);
let bad_sigma = MultiOptions2 {
sigma_max: Some(-1.0),
..test_opts()
};
assert!(
multi_curve_adjustment_with_points(
&groups,
Some(&initial),
points.clone(),
0,
&bad_sigma
)
.is_err()
);
let bad_distance = MultiOptions2 {
max_distance: 0.0,
..test_opts()
};
assert!(
multi_curve_adjustment_with_points(
&groups,
Some(&initial),
points.clone(),
0,
&bad_distance
)
.is_err()
);
assert!(
multi_curve_adjustment_with_points(&groups, Some(&initial), points, 5, &test_opts())
.is_err()
);
}
#[test]
fn out_of_range_alignment_points_are_rejected() {
let curves = vec![rect_curve(), rect_curve()];
let initial = vec![Iso2::identity(); 2];
let groups = groups_of_one(&curves);
let mut points = chain_points(&curves, 1.0);
points[0].ref_i = 7;
let err =
multi_curve_adjustment_with_points(&groups, Some(&initial), points, 0, &test_opts())
.unwrap_err()
.to_string();
assert!(err.contains("only 2 bodies"), "unexpected message: {err}");
}
#[test]
fn a_mismatched_initial_slice_is_rejected() {
let curves = vec![rect_curve(), rect_curve()];
let groups = groups_of_one(&curves);
let short = [Iso2::identity(); 1];
let err = multi_curve_adjustment_with_points(
&groups,
Some(&short),
chain_points(&curves, 1.0),
0,
&test_opts(),
)
.unwrap_err()
.to_string();
assert!(
err.contains("initial transforms"),
"unexpected message: {err}"
);
}
#[test]
fn sampled_correspondences_settle_three_curves_together() -> Result<()> {
let curves = vec![rect_curve(), rect_curve(), rect_curve()];
let initial = vec![
Iso2::identity(),
Iso2::new(Vector2::new(0.15, -0.1), PI / 120.0),
Iso2::new(Vector2::new(-0.1, 0.12), -PI / 100.0),
];
let groups = groups_of_one(&curves);
let outcome = multi_curve_adjustment(
&groups,
Some(&initial),
&test_opts(),
&CAPParams::defaults(0.5),
)?;
assert_eq!(outcome.len(), 3);
let t: Vec<Iso2> = (0..3)
.map(|i| *outcome.alignment(i).full_transform())
.collect();
for i in 0..3 {
for j in 0..3 {
assert_relative_eq!(
t[i].inv_mul(&t[j]).to_matrix(),
Iso2::identity().to_matrix(),
epsilon = 1e-5
);
}
}
Ok(())
}
#[test]
fn the_static_curve_is_the_one_the_others_reference_most() {
let left = segment_curve(0.0, 6.0);
let middle = segment_curve(4.0, 12.0);
let right = segment_curve(10.0, 16.0);
let curves = [left, middle, right];
let initial = [Iso2::identity(); 3];
let groups = groups_of_one(&curves);
let order = reference_priority(
&groups,
&initial,
&CAPParams::new(0.25, PI / 3.0, 0.25, None),
)
.unwrap();
assert_eq!(order[0], 1, "the middle curve should be the static one");
}
#[test]
fn a_correspondence_matrix_is_symmetric_and_empty_on_the_diagonal() {
let curves = [
segment_curve(0.0, 6.0),
segment_curve(4.0, 12.0),
segment_curve(20.0, 26.0),
];
let initial = [Iso2::identity(); 3];
let groups = groups_of_one(&curves);
let m = correspondence_matrix(
&groups,
&initial,
&CAPParams::new(0.25, PI / 3.0, 0.25, None),
)
.unwrap();
for i in 0..3 {
assert_eq!(m[(i, i)], 0.0);
for j in 0..3 {
assert_eq!(m[(i, j)], m[(j, i)]);
}
}
assert!(m[(0, 1)] > 0.0);
assert_eq!(m[(0, 2)], 0.0);
assert_eq!(m[(1, 2)], 0.0);
}
#[test]
fn fewer_than_two_curves_is_rejected() {
let curves = vec![rect_curve()];
let initial = vec![Iso2::identity()];
let groups = groups_of_one(&curves);
let err = multi_curve_adjustment(
&groups,
Some(&initial),
&test_opts(),
&CAPParams::defaults(0.5),
)
.unwrap_err()
.to_string();
assert!(
err.contains("at least two bodies"),
"unexpected message: {err}"
);
let empty: Vec<CurveGroup2> = Vec::new();
assert!(
multi_curve_adjustment(&empty, None, &test_opts(), &CAPParams::defaults(0.5)).is_err()
);
}
#[test]
fn a_two_member_body_recovers_a_motion_neither_member_could() -> Result<()> {
let disturb = small_disturbance(0.1, 0.08, 0.0);
let leftover_x = |groups: &[CurveGroup2]| -> Result<f64> {
let initial = vec![Iso2::identity(), disturb];
let outcome = multi_curve_adjustment(
groups,
Some(&initial),
&test_opts(),
&CAPParams::defaults(0.5),
)?;
Ok(outcome
.alignment(1)
.full_transform()
.translation
.vector
.x
.abs())
};
let cross = || {
CurveGroup2::new(vec![
segment_curve(-6.0, 6.0),
vertical_segment(10.0, -6.0, 6.0),
])
.unwrap()
};
let pair = vec![cross(), cross()];
let lone = vec![
CurveGroup2::from(segment_curve(-6.0, 6.0)),
CurveGroup2::from(segment_curve(-6.0, 6.0)),
];
let with_two = leftover_x(&pair)?;
let with_one = leftover_x(&lone)?;
assert!(
with_two < 1e-5,
"the two member body should have taken the x displacement out, \
but {with_two} was left"
);
assert!(
with_one > 0.5 * 0.1,
"a lone horizontal segment cannot see an x displacement, so most of the 0.1 \
should survive, but only {with_one} did"
);
Ok(())
}
#[test]
fn both_members_of_a_body_are_brought_onto_their_twins() -> Result<()> {
let groups = vec![two_circle_body(), two_circle_body()];
let disturb = small_disturbance(0.08, 0.06, -0.015);
let initial = vec![Iso2::identity(), disturb];
let outcome = multi_curve_adjustment(
&groups,
Some(&initial),
&test_opts(),
&CAPParams::defaults(0.25),
)?;
let t = outcome.alignment(1).full_transform();
for (i, member) in groups[1].curves().iter().enumerate() {
let moved = member.new_transformed_by(t);
for p in moved.points() {
let (m, station) = groups[0].at_closest_to_point(p);
assert_eq!(m, i, "member {i} drifted onto member {m} of the reference");
assert!(
dist(p, &station.point()) < 1e-4,
"member {i} did not settle back onto its twin"
);
}
}
Ok(())
}
#[test]
fn a_body_may_mix_open_and_closed_members() -> Result<()> {
let body = || CurveGroup2::new(vec![rect_curve(), segment_curve(-4.0, 4.0)]).unwrap();
let groups = vec![body(), body()];
let disturb = small_disturbance(0.05, 0.04, 0.01);
let initial = vec![Iso2::identity(), disturb];
let samples =
generate_alignment_points(&groups[1], &groups[0], &disturb, &CAPParams::defaults(0.5));
assert!(samples.iter().any(|cp| cp.member == 0));
assert!(samples.iter().any(|cp| cp.member == 1));
let outcome = multi_curve_adjustment(
&groups,
Some(&initial),
&test_opts(),
&CAPParams::defaults(0.5),
)?;
assert_relative_eq!(
outcome.alignment(1).full_transform().to_matrix(),
Iso2::identity().to_matrix(),
epsilon = 1e-5
);
Ok(())
}
#[test]
fn stress_two_member_bodies_recover_small_disturbances() -> Result<()> {
let mut rg = RandomGeometry2::from_seed(0x9a1_c0de);
for _ in 0..20 {
let groups = vec![two_circle_body(), two_circle_body()];
let disturb = small_disturbance(rg.f64_sym(0.1), rg.f64_sym(0.1), rg.f64_sym(0.02));
let initial = vec![Iso2::identity(), disturb];
let outcome = multi_curve_adjustment(
&groups,
Some(&initial),
&test_opts(),
&CAPParams::defaults(0.25),
)?;
assert_relative_eq!(
outcome.alignment(1).full_transform().to_matrix(),
Iso2::identity().to_matrix(),
epsilon = 1e-4
);
}
Ok(())
}
#[test]
fn static_selection_counts_a_multi_member_body_across_its_members() {
let groups = vec![
CurveGroup2::from(segment_curve(0.0, 6.0)),
CurveGroup2::from(segment_curve(20.0, 26.0)),
CurveGroup2::new(vec![segment_curve(1.0, 5.0), segment_curve(21.0, 25.0)]).unwrap(),
];
let poses = [Iso2::identity(); 3];
let order =
reference_priority(&groups, &poses, &CAPParams::new(0.25, PI / 3.0, 0.25, None))
.unwrap();
assert_eq!(order[0], 2, "the two member body should be the static one");
}
}