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::geom3::align3::jacobian::{point_surf_jacobian, point_surf_jacobian_rev};
use crate::geom3::align3::mesh::{AlignMesh, generate_alignment_points, interpolated_stdev};
use crate::geom3::align3::{AlignValues3, Dof6, GAPParams, MultiParams3};
use crate::geom3::mesh::MeshSurfPoint;
use crate::geom3::{Align3, MultiOutcome3, Point3, SurfacePoint3};
use crate::na::{DMatrix, DVector, Dyn, Matrix, Owned, U1, Vector};
use levenberg_marquardt::{LeastSquaresProblem, LevenbergMarquardt};
use rayon::prelude::*;
const RESIDUAL_DOF: usize = 3;
#[derive(Clone, Copy, Debug)]
pub struct MultiOptions3 {
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<Dof6>,
}
impl MultiOptions3 {
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 MeshAlignPoint {
pub mesh_i: usize,
pub mp: MeshSurfPoint,
pub ref_i: usize,
pub weight: f64,
}
impl MeshAlignPoint {
pub fn new(mesh_i: usize, mp: MeshSurfPoint, ref_i: usize, weight: f64) -> Self {
Self {
mesh_i,
mp,
ref_i,
weight,
}
}
}
pub fn multi_mesh_adjustment_with_points(
meshes: &[AlignMesh],
points: Vec<MeshAlignPoint>,
static_i: usize,
opts: &MultiOptions3,
) -> Result<MultiOutcome3> {
validate(meshes, &points, static_i, opts)?;
solve_bundle(MultiMeshProblem::new(meshes, points, static_i, opts)?, opts)
}
pub fn multi_mesh_adjustment(
meshes: &[AlignMesh],
opts: &MultiOptions3,
sample_opts: &GAPParams,
) -> Result<MultiOutcome3> {
let reference_order = reference_priority(meshes, sample_opts)?;
let static_i = reference_order[0];
let mut work_list = Vec::new();
let mut meshes_to_test = (0..meshes.len()).collect::<Vec<_>>();
for ref_i in reference_order {
meshes_to_test.retain(|j| *j != ref_i);
for &mesh_i in meshes_to_test.iter() {
work_list.push((mesh_i, ref_i));
}
}
let points = work_list
.par_iter()
.map(|&(mesh_i, ref_i)| {
let t = meshes[ref_i]
.transform()
.inv_mul(&meshes[mesh_i].transform());
let samples =
generate_alignment_points(meshes[mesh_i].mesh, meshes[ref_i].mesh, &t, sample_opts)
.map_err(|e| e.to_string())?;
Ok(samples
.into_iter()
.map(|mp| {
let weight = meshes[mesh_i].weights.map_or(1.0, |providers| {
providers.iter().map(|p| p.weight(&mp)).product()
});
MeshAlignPoint::new(mesh_i, mp, ref_i, weight)
})
.collect::<Vec<_>>())
})
.collect::<std::result::Result<Vec<_>, String>>()?
.into_iter()
.flatten()
.collect::<Vec<_>>();
multi_mesh_adjustment_with_points(meshes, points, static_i, opts)
}
fn validate(
meshes: &[AlignMesh],
points: &[MeshAlignPoint],
static_i: usize,
opts: &MultiOptions3,
) -> 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 >= meshes.len() {
return Err(format!(
"the static mesh index {} is out of range for {} meshes",
static_i,
meshes.len()
)
.into());
}
if let Some(p) = points
.iter()
.find(|p| p.mesh_i >= meshes.len() || p.ref_i >= meshes.len())
{
return Err(format!(
"an alignment point references mesh {} against mesh {}, but there are only {} meshes",
p.mesh_i,
p.ref_i,
meshes.len()
)
.into());
}
Ok(())
}
fn solve_bundle(problem: MultiMeshProblem<'_>, opts: &MultiOptions3) -> Result<MultiOutcome3> {
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 meshes 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 {
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.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(MultiOutcome3::new(problem.alignments(), solves, halt))
}
fn run<'a>(
lm: &LevenbergMarquardt<f64>,
problem: MultiMeshProblem<'a>,
) -> (MultiMeshProblem<'a>, TerminationReason) {
let (result, report) = lm.minimize(problem);
(result, report.termination)
}
fn resolve_sigma_max(opts: &MultiOptions3, problem: &MultiMeshProblem<'_>) -> Option<f64> {
match opts.sigma_max {
Some(s) => Some(s),
None => estimate_sigma_max(&problem.normalized_residuals()),
}
}
struct Moved {
p_world: Point3,
c_world: SurfacePoint3,
residual: f64,
target_weight: f64,
target_sigma: f64,
}
struct MultiMeshProblem<'a> {
meshes: &'a [AlignMesh<'a>],
handles: Vec<MeshAlignPoint>,
params: MultiParams3,
values: Vec<AlignValues3>,
moved: Vec<Point3>,
closest: Vec<SurfacePoint3>,
residuals: Vec<f64>,
test_sigma: Vec<f64>,
target_sigma: Vec<f64>,
inv_sigma: Vec<f64>,
target_weights: Vec<f64>,
magsac_weights: Vec<f64>,
max_distance: f64,
min_normal_dot: Option<f64>,
}
impl<'a> MultiMeshProblem<'a> {
fn new(
meshes: &'a [AlignMesh],
handles: Vec<MeshAlignPoint>,
static_i: usize,
opts: &MultiOptions3,
) -> Result<Self> {
let centers = meshes
.iter()
.map(|m| m.mesh.aabb().center())
.collect::<Vec<_>>();
let initial = meshes.iter().map(|m| m.transform()).collect::<Vec<_>>();
let params = MultiParams3::from_centers(static_i, ¢ers, Some(&initial), opts.dof)?;
let n = handles.len();
let test_sigma = handles
.iter()
.map(|h| sigma_at(&meshes[h.mesh_i], &h.mp))
.collect();
let values = params.compute_all_values();
let mut item = Self {
meshes,
handles,
params,
values,
moved: vec![Point3::origin(); n],
closest: vec![SurfacePoint3::new(Point3::origin(), crate::Vector3::z_axis()); n],
residuals: vec![0.0; n],
test_sigma,
target_sigma: vec![0.0; n],
inv_sigma: vec![1.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();
item.refresh_inv_sigma();
Ok(item)
}
fn move_points(&mut self) {
self.values = self.params.compute_all_values();
let values = &self.values;
let meshes = self.meshes;
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.mesh_i].transform;
let t_ref = values[h.ref_i].transform;
let p_world = t_test * h.mp.point();
let query = t_ref.inverse_transform_point(&p_world);
let mp = meshes[h.ref_i].mesh.surface_closest_to(&query);
let c_world = SurfacePoint3::new(t_ref * mp.point(), t_ref.rotation * mp.normal());
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.mp.normal();
if n_test.dot(&c_world.normal) < min_dot {
weight = 0.0;
}
}
Moved {
p_world,
c_world,
residual,
target_weight: weight,
target_sigma: sigma_at(&meshes[h.ref_i], &mp),
}
})
.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;
self.target_sigma[i] = m.target_sigma;
}
}
fn restore(&mut self, storage: &DVector<f64>) {
self.params.set_storage(storage);
self.move_points();
}
fn refresh_inv_sigma(&mut self) {
for i in 0..self.handles.len() {
let t = self.test_sigma[i];
let r = self.target_sigma[i];
let combined = (t * t + r * r).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.handles.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.handles.len())
.filter(|&i| {
let r = (self.residuals[i] * self.inv_sigma[i]).abs();
self.target_weights[i] * weighting.weight(r) > 0.0
})
.count()
}
fn alignments(&self) -> Vec<Align3> {
let mut grouped = vec![Vec::new(); self.meshes.len()];
for (i, h) in self.handles.iter().enumerate() {
grouped[h.mesh_i].push(self.residuals[i]);
}
self.values
.iter()
.enumerate()
.zip(grouped)
.map(|((i, v), residuals)| {
let body = self.params.body(i);
Align3::new(v.transform, v.align, body.local, body.offset, residuals)
})
.collect()
}
}
fn sigma_at(mesh: &AlignMesh, mp: &MeshSurfPoint) -> f64 {
let stdev = mesh.uncertainty.or_else(|| mesh.mesh.point_stdev());
stdev.map_or(0.0, |s| interpolated_stdev(mesh.mesh, mp, s))
}
impl LeastSquaresProblem<f64, Dyn, Dyn> for MultiMeshProblem<'_> {
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.mesh_i]) * scale;
self.params.add_jacobian_block(&mut jac, i, h.mesh_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(meshes: &[AlignMesh], params: &GAPParams) -> Result<Vec<usize>> {
Ok(crate::common::align::reference::reference_priority(
correspondence_matrix(meshes, params)?,
))
}
fn correspondence_matrix(meshes: &[AlignMesh], params: &GAPParams) -> Result<DMatrix<f64>> {
crate::common::align::reference::correspondence_matrix(meshes.len(), |i, j| {
let t = meshes[i].transform().inv_mul(&meshes[j].transform());
let points = generate_alignment_points(meshes[j].mesh, meshes[i].mesh, &t, params)
.map_err(|e| e.to_string())?;
Ok(points.len() as f64)
})
.map_err(|e| e.into())
}
#[cfg(test)]
mod tests {
use super::*;
use crate::geom3::Iso3;
use crate::{Mesh3, Vector3};
use approx::assert_relative_eq;
use std::f64::consts::PI;
const WIDE_GATE: f64 = 20.0;
fn test_opts() -> MultiOptions3 {
MultiOptions3::new(WIDE_GATE)
}
fn box_mesh() -> Mesh3 {
Mesh3::create_box(10.0, 6.0, 4.0, false)
}
fn chain_points(meshes: &[Mesh3], spacing: f64) -> Vec<MeshAlignPoint> {
let mut points = Vec::new();
for (i, mesh) in meshes.iter().enumerate().skip(1) {
for mp in mesh.sample_surface_poisson(spacing, None).unwrap() {
points.push(MeshAlignPoint::new(i, mp, i - 1, 1.0));
}
}
points
}
fn alignment_meshes<'a>(meshes: &'a [Mesh3], initial: &'a [Iso3]) -> Vec<AlignMesh<'a>> {
meshes
.iter()
.zip(initial.iter())
.map(|(m, t)| AlignMesh::new(m, None, Some(t), None))
.collect()
}
#[test]
fn a_disturbed_mesh_is_brought_back_onto_its_twin() -> Result<()> {
let meshes = vec![box_mesh(), box_mesh()];
let disturb = Iso3::new(Vector3::new(0.2, -0.15, 0.1), Vector3::z() * (PI / 90.0));
let initial = vec![Iso3::identity(), disturb];
let ams = alignment_meshes(&meshes, &initial);
let points = chain_points(&meshes, 0.4);
let outcome = multi_mesh_adjustment_with_points(&ams, points, 0, &test_opts())?;
assert_eq!(outcome.len(), 2);
assert_relative_eq!(
outcome.alignment(0).full_transform().to_matrix(),
Iso3::identity().to_matrix(),
epsilon = 1e-12
);
assert_relative_eq!(
outcome.alignment(1).full_transform().to_matrix(),
Iso3::identity().to_matrix(),
epsilon = 1e-6
);
Ok(())
}
#[test]
fn three_meshes_settle_together() -> Result<()> {
let meshes = vec![box_mesh(), box_mesh(), box_mesh()];
let initial = vec![
Iso3::identity(),
Iso3::new(Vector3::new(0.15, 0.0, -0.1), Vector3::y() * (PI / 120.0)),
Iso3::new(Vector3::new(-0.1, 0.12, 0.05), Vector3::x() * (PI / 100.0)),
];
let ams = alignment_meshes(&meshes, &initial);
let points = chain_points(&meshes, 0.5);
let outcome = multi_mesh_adjustment_with_points(&ams, points, 0, &test_opts())?;
for i in 0..3 {
assert_relative_eq!(
outcome.alignment(i).full_transform().to_matrix(),
Iso3::identity().to_matrix(),
epsilon = 1e-5
);
}
Ok(())
}
#[test]
fn the_static_mesh_never_moves() -> Result<()> {
let meshes = vec![box_mesh(), box_mesh()];
let initial = vec![
Iso3::translation(0.3, 0.0, 0.0),
Iso3::translation(0.0, 0.25, 0.0),
];
let ams = alignment_meshes(&meshes, &initial);
let outcome =
multi_mesh_adjustment_with_points(&ams, chain_points(&meshes, 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_mesh_they_came_from() -> Result<()> {
let meshes = vec![box_mesh(), box_mesh(), box_mesh()];
let initial = vec![Iso3::identity(); 3];
let ams = alignment_meshes(&meshes, &initial);
let points = chain_points(&meshes, 0.6);
let from_1 = points.iter().filter(|p| p.mesh_i == 1).count();
let from_2 = points.iter().filter(|p| p.mesh_i == 2).count();
let outcome = multi_mesh_adjustment_with_points(&ams, 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 meshes = vec![box_mesh(), box_mesh()];
let initial = vec![Iso3::identity(), Iso3::translation(0.3, 0.0, 0.0)];
let ams = alignment_meshes(&meshes, &initial);
let opts = MultiOptions3 {
dof: Some(Dof6::new(false, true, true, true, true, true)),
..test_opts()
};
let outcome =
multi_mesh_adjustment_with_points(&ams, chain_points(&meshes, 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 meshes = vec![box_mesh(), box_mesh()];
let initial = vec![
Iso3::identity(),
Iso3::new(Vector3::new(1.5, -1.0, 0.8), Vector3::z() * (PI / 12.0)),
];
let ams = alignment_meshes(&meshes, &initial);
let opts = MultiOptions3 {
patience: 1,
..test_opts()
};
let outcome =
multi_mesh_adjustment_with_points(&ams, chain_points(&meshes, 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 meshes = vec![box_mesh(), box_mesh()];
let disturb = Iso3::new(Vector3::new(0.2, -0.15, 0.1), Vector3::z() * (PI / 120.0));
let initial = vec![Iso3::identity(), disturb];
let ams = alignment_meshes(&meshes, &initial);
let mut points = chain_points(&meshes, 0.4);
for (k, p) in points.iter_mut().enumerate() {
if k.is_multiple_of(10) {
p.mp.sp.point += Vector3::new(0.0, 0.0, 5.0);
}
}
let naive = multi_mesh_adjustment_with_points(
&ams,
points.clone(),
0,
&MultiOptions3 {
refinement_steps: 0,
..test_opts()
},
)?;
let robust = multi_mesh_adjustment_with_points(&ams, points, 0, &test_opts())?;
let error = |o: &MultiOutcome3| {
(o.alignment(1).full_transform().to_matrix() - Iso3::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 meshes = vec![box_mesh(), box_mesh()];
let initial = vec![Iso3::identity(), Iso3::identity()];
let ams = alignment_meshes(&meshes, &initial);
let mut points = chain_points(&meshes, 0.4);
for (k, p) in points.iter_mut().enumerate() {
if k.is_multiple_of(20) {
p.mp.sp.point += Vector3::new(0.0, 0.0, 8.0);
}
}
let unweighted = |gate: f64| MultiOptions3 {
refinement_steps: 0,
..MultiOptions3::new(gate)
};
let drift = |o: &MultiOutcome3| {
(o.alignment(1).full_transform().to_matrix() - Iso3::identity().to_matrix()).norm()
};
let ungated =
multi_mesh_adjustment_with_points(&ams, points.clone(), 0, &unweighted(WIDE_GATE))?;
let gated = multi_mesh_adjustment_with_points(&ams, 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 meshes = vec![box_mesh(), box_mesh()];
let initial = vec![Iso3::identity(), Iso3::identity()];
let ams = alignment_meshes(&meshes, &initial);
let mut points = chain_points(&meshes, 0.5);
let far = points.len() / 2;
points[far].mp.sp.point += Vector3::new(0.0, 0.0, 50.0);
let opts = MultiOptions3 {
max_distance: 1.0,
refinement_steps: 0,
..test_opts()
};
let outcome = multi_mesh_adjustment_with_points(&ams, points, 0, &opts)?;
assert_relative_eq!(
outcome.alignment(1).full_transform().to_matrix(),
Iso3::identity().to_matrix(),
epsilon = 1e-6
);
Ok(())
}
#[test]
fn jacobian_matches_finite_differences() -> Result<()> {
let meshes = vec![box_mesh(), box_mesh(), box_mesh()];
let initial = vec![
Iso3::identity(),
Iso3::new(Vector3::new(0.21, -0.13, 0.09), Vector3::z() * 0.03),
Iso3::new(Vector3::new(-0.17, 0.11, -0.07), Vector3::x() * 0.04),
];
let ams = alignment_meshes(&meshes, &initial);
let mut points = Vec::new();
for body in [1usize, 2] {
for i in -2..=2 {
for j in -1..=1 {
let probe = Point3::new(i as f64 * 1.5, j as f64 * 1.5, 2.0);
let mp = meshes[body].surface_closest_to(&probe);
points.push(MeshAlignPoint::new(body, mp, body - 1, 1.0));
}
}
}
let opts = MultiOptions3 {
refinement_steps: 0,
..test_opts()
};
let mut problem = MultiMeshProblem::new(&ams, points, 0, &opts)?;
let x0 = problem.params.storage().clone();
let analytic = problem.jacobian().unwrap();
assert_eq!(analytic.ncols(), 12, "two free bodies, six 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 meshes = vec![box_mesh(), box_mesh(), box_mesh()];
let initial = vec![
Iso3::identity(),
Iso3::translation(0.2, 0.0, 0.0),
Iso3::translation(0.0, 0.2, 0.0),
];
let ams = alignment_meshes(&meshes, &initial);
let mp = meshes[2].surface_closest_to(&Point3::new(0.0, 0.0, 2.0));
let points = vec![MeshAlignPoint::new(2, mp, 1, 1.0)];
let problem = MultiMeshProblem::new(&ams, 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..6).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 meshes = vec![box_mesh(), box_mesh()];
let initial = vec![Iso3::identity(), Iso3::translation(0.3, -0.2, 0.1)];
let ams = alignment_meshes(&meshes, &initial);
let mp = meshes[1].surface_closest_to(&Point3::new(1.0, 1.0, 2.0));
let points = vec![MeshAlignPoint::new(1, mp, 1, 1.0)];
let problem = MultiMeshProblem::new(&ams, 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 meshes = vec![box_mesh(), box_mesh()];
let initial = vec![Iso3::identity(); 2];
let ams = alignment_meshes(&meshes, &initial);
let points = chain_points(&meshes, 1.0);
let bad_patience = MultiOptions3 {
patience: 0,
..test_opts()
};
assert!(multi_mesh_adjustment_with_points(&ams, points.clone(), 0, &bad_patience).is_err());
let bad_sigma = MultiOptions3 {
sigma_max: Some(-1.0),
..test_opts()
};
assert!(multi_mesh_adjustment_with_points(&ams, points.clone(), 0, &bad_sigma).is_err());
let bad_distance = MultiOptions3 {
max_distance: 0.0,
..test_opts()
};
assert!(multi_mesh_adjustment_with_points(&ams, points.clone(), 0, &bad_distance).is_err());
assert!(multi_mesh_adjustment_with_points(&ams, points, 5, &test_opts()).is_err());
}
#[test]
fn out_of_range_alignment_points_are_rejected() {
let meshes = vec![box_mesh(), box_mesh()];
let initial = vec![Iso3::identity(); 2];
let ams = alignment_meshes(&meshes, &initial);
let mut points = chain_points(&meshes, 1.0);
points[0].ref_i = 7;
let err = multi_mesh_adjustment_with_points(&ams, points, 0, &test_opts())
.unwrap_err()
.to_string();
assert!(err.contains("only 2 meshes"), "unexpected message: {err}");
}
}