use crate::Point3;
use crate::common::align::information::{AlignDofs, AlignInfo};
use crate::geom3::align3::jacobian::point_surf_jacobian;
use crate::geom3::align3::{AlignParams3, Dof6, SurfaceTarget3};
pub type AlignInfo3 = AlignInfo<Dof6, 6>;
impl AlignDofs<6> for Dof6 {
fn free_indices(&self) -> Vec<usize> {
[self.tx, self.ty, self.tz, self.rx, self.ry, self.rz]
.iter()
.enumerate()
.filter_map(|(i, &free)| free.then_some(i))
.collect()
}
}
impl AlignInfo3 {
pub fn from_points(
points: &[Point3],
target: &impl SurfaceTarget3,
params: &AlignParams3,
) -> Self {
let current = params.compute_values();
let mut rows = Vec::with_capacity(points.len());
let mut weights = Vec::with_capacity(points.len());
for p in points {
let m = current.transform * p;
let c = target.find_align_match(&m);
rows.push(point_surf_jacobian(&m, &c, ¤t));
weights.push(if c.sigma > 0.0 {
c.weight / (c.sigma * c.sigma)
} else {
c.weight
});
}
Self::assemble(rows, weights, params.dof)
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::geom3::align3::{AlignStorage3, AlignSurfMatch3};
use crate::na::Matrix6;
use crate::{Iso3, Mesh3, UnitVec3, Vector3};
use approx::assert_relative_eq;
struct GroundPlane;
impl SurfaceTarget3 for GroundPlane {
fn find_align_match(&self, p: &Point3) -> AlignSurfMatch3 {
AlignSurfMatch3::new(
Point3::new(p.x, p.y, 0.0),
UnitVec3::new_unchecked(Vector3::z()),
true,
1.0,
)
}
}
fn plane_points() -> Vec<Point3> {
let mut points = Vec::new();
for i in -3..=3 {
for j in -3..=3 {
points.push(Point3::new(i as f64, j as f64, 1.0));
}
}
points
}
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()
}
#[test]
fn a_plane_leaves_three_motions_unconstrained() {
let info = AlignInfo3::from_points(
&plane_points(),
&GroundPlane,
&AlignParams3::from_origin(None),
);
let weak = info.weak_directions();
assert_eq!(weak.len(), 6);
let unconstrained: Vec<f64> = weak.iter().take(3).map(|(v, _)| *v).collect();
for v in &unconstrained {
assert!(v.abs() < 1e-9, "expected an unconstrained motion, got {v}");
}
assert!(
weak[3].0 > 1.0,
"expected the remaining motions to be constrained, got {}",
weak[3].0
);
for (_, direction) in weak.iter().take(3) {
for i in [2usize, 3, 4] {
assert!(
direction[i].abs() < 1e-9,
"unconstrained direction leaked into parameter {i}: {direction:?}"
);
}
}
}
#[test]
fn a_singular_problem_reports_rather_than_lying() {
let info = AlignInfo3::from_points(
&plane_points(),
&GroundPlane,
&AlignParams3::from_origin(None),
);
let err = info.marginal_precision().unwrap_err().to_string();
assert!(err.contains("singular"), "unexpected message: {err}");
assert!(info.leverage().is_err());
}
#[test]
fn locking_the_free_motions_makes_a_plane_well_conditioned() {
let dof = Dof6::new(false, false, true, true, true, false);
let params = AlignParams3::from_origin(Some(dof));
let info = AlignInfo3::from_points(&plane_points(), &GroundPlane, ¶ms);
assert_eq!(info.free_dof_count(), 3);
let precision = info.marginal_precision().unwrap();
assert_eq!(precision[0], None, "tx was locked");
assert_eq!(precision[1], None, "ty was locked");
assert_eq!(precision[5], None, "rz was locked");
for i in [2usize, 3, 4] {
assert!(
precision[i].unwrap() > 0.0,
"parameter {i} should be constrained"
);
}
}
#[test]
fn a_box_constrains_everything() {
let mesh = box_mesh();
let points = box_points(&mesh);
let params = AlignParams3::from_origin(None);
let info = AlignInfo3::from_points(&points, &mesh, ¶ms);
assert_eq!(info.free_dof_count(), 6);
let precision = info.marginal_precision().unwrap();
for (i, p) in precision.iter().enumerate() {
assert!(
p.unwrap() > 0.0,
"parameter {i} should be constrained on a closed box"
);
}
}
#[test]
fn leverage_sums_to_the_free_dof_count() {
let mesh = box_mesh();
let points = box_points(&mesh);
let info = AlignInfo3::from_points(&points, &mesh, &AlignParams3::from_origin(None));
let total: f64 = info.leverage().unwrap().iter().sum();
assert_relative_eq!(total, 6.0, epsilon = 1e-9);
}
#[test]
fn leverage_sums_to_the_free_dof_count_when_constrained() {
let mesh = box_mesh();
let points = box_points(&mesh);
let dof = Dof6::new(false, true, true, true, false, true);
let info = AlignInfo3::from_points(&points, &mesh, &AlignParams3::from_origin(Some(dof)));
assert_eq!(info.free_dof_count(), 4);
let total: f64 = info.leverage().unwrap().iter().sum();
assert_relative_eq!(total, 4.0, epsilon = 1e-9);
}
#[test]
fn selection_prefers_isolated_points_over_a_redundant_cluster() {
let mesh = box_mesh();
let mut points = Vec::new();
for i in 0..20 {
points.push(Point3::new(1.0 + i as f64 * 1e-4, 1.0, 1.0));
}
let cluster = 0..20;
let isolated = [
Point3::new(-4.0, -2.0, 1.0),
Point3::new(4.0, 2.0, -1.0),
Point3::new(5.0, 0.0, 0.0),
Point3::new(-5.0, 1.0, 0.5),
Point3::new(0.0, 2.5, -0.5),
Point3::new(2.0, -2.5, 0.5),
];
points.extend_from_slice(&isolated);
let info = AlignInfo3::from_points(&points, &mesh, &AlignParams3::from_origin(None));
let picked = info.select_d_optimal(7, None);
assert_eq!(picked.len(), 7);
let from_cluster = picked.iter().filter(|&&i| cluster.contains(&i)).count();
assert_eq!(
from_cluster, 1,
"expected exactly one cluster member among {picked:?}"
);
}
#[test]
fn selection_is_a_prefix_so_it_can_be_truncated() {
let mesh = box_mesh();
let points = box_points(&mesh);
let info = AlignInfo3::from_points(&points, &mesh, &AlignParams3::from_origin(None));
let long = info.select_d_optimal(20, None);
let short = info.select_d_optimal(8, None);
assert_eq!(&long[..8], &short[..]);
}
#[test]
fn a_selected_subset_stays_well_conditioned() {
let mesh = box_mesh();
let points = box_points(&mesh);
let params = AlignParams3::from_origin(None);
let info = AlignInfo3::from_points(&points, &mesh, ¶ms);
let n = 40;
let picked = info.select_d_optimal(n, None);
let chosen: Vec<Point3> = picked.iter().map(|&i| points[i]).collect();
let arbitrary: Vec<Point3> = points.iter().take(n).copied().collect();
let weakest = |pts: &[Point3]| -> f64 {
AlignInfo3::from_points(pts, &mesh, ¶ms).weak_directions()[0].0
};
let chosen_weakest = weakest(&chosen);
let arbitrary_weakest = weakest(&arbitrary);
assert!(
chosen_weakest > arbitrary_weakest * 10.0,
"the selected subset should be far better conditioned than an arbitrary one: \
selected {chosen_weakest}, arbitrary {arbitrary_weakest}"
);
assert!(
AlignInfo3::from_points(&chosen, &mesh, ¶ms)
.marginal_precision()
.is_ok(),
"the selected subset should leave no motion unconstrained"
);
}
#[test]
fn selection_respects_the_available_count() {
let mesh = box_mesh();
let points = box_points(&mesh);
let info = AlignInfo3::from_points(&points, &mesh, &AlignParams3::from_origin(None));
assert!(info.select_d_optimal(0, None).is_empty());
assert_eq!(
info.select_d_optimal(points.len() + 50, None).len(),
points.len()
);
}
#[test]
fn zero_weight_points_are_never_selected() {
let rows = vec![
AlignStorage3::new(1.0, 0.0, 0.0, 0.0, 0.0, 0.0),
AlignStorage3::new(0.0, 1.0, 0.0, 0.0, 0.0, 0.0),
AlignStorage3::new(0.0, 0.0, 1.0, 0.0, 0.0, 0.0),
];
let weights = vec![1.0, 0.0, 1.0];
let dof = Dof6::new(true, true, true, false, false, false);
let info = AlignInfo3::from_rows(rows, weights, dof).unwrap();
let picked = info.select_d_optimal(3, None);
assert!(!picked.contains(&1), "a zero-weight point was selected");
assert_eq!(picked.len(), 2);
}
#[test]
fn information_matrix_is_expanded_with_zeros_for_locked_dof() {
let rows = vec![AlignStorage3::new(1.0, 2.0, 3.0, 0.0, 0.0, 0.0)];
let dof = Dof6::new(true, true, true, false, false, false);
let info = AlignInfo3::from_rows(rows, vec![1.0], dof).unwrap();
let h = info.information();
assert_relative_eq!(h[(0, 0)], 1.0, epsilon = 1e-12);
assert_relative_eq!(h[(0, 1)], 2.0, epsilon = 1e-12);
assert_relative_eq!(h[(2, 2)], 9.0, epsilon = 1e-12);
for i in 3..6 {
for j in 0..6 {
assert_eq!(h[(i, j)], 0.0);
assert_eq!(h[(j, i)], 0.0);
}
}
}
#[test]
fn mismatched_rows_and_weights_are_rejected() {
let rows = vec![AlignStorage3::zeros(); 3];
let err = AlignInfo3::from_rows(rows, vec![1.0; 2], Dof6::all())
.unwrap_err()
.to_string();
assert!(err.contains("weights"), "unexpected message: {err}");
}
#[test]
fn invalid_weights_are_rejected() {
for bad in [-1.0, f64::NAN, f64::INFINITY] {
let rows = vec![AlignStorage3::zeros(); 2];
let result = AlignInfo3::from_rows(rows, vec![1.0, bad], Dof6::all());
assert!(result.is_err(), "weight {bad} should have been rejected");
}
}
#[test]
fn a_fully_locked_problem_degrades_gracefully() {
let dof = Dof6::new(false, false, false, false, false, false);
let info =
AlignInfo3::from_rows(vec![AlignStorage3::zeros(); 4], vec![1.0; 4], dof).unwrap();
assert_eq!(info.free_dof_count(), 0);
assert!(info.weak_directions().is_empty());
assert!(info.select_d_optimal(4, None).is_empty());
assert_eq!(info.information(), Matrix6::zeros());
}
#[test]
fn the_local_origin_is_respected() {
let mesh = box_mesh();
let points = box_points(&mesh);
let at_origin = AlignInfo3::from_points(&points, &mesh, &AlignParams3::from_origin(None));
let offset = AlignInfo3::from_points(
&points,
&mesh,
&AlignParams3::from_center(Point3::new(50.0, 0.0, 0.0), None),
);
let delta = (at_origin.information() - offset.information()).norm();
assert!(
delta > 1.0,
"the local origin had no effect (delta {delta})"
);
}
#[test]
fn a_transformed_pose_is_analyzed_where_it_currently_sits() {
let mesh = box_mesh();
let points = box_points(&mesh);
let here = AlignInfo3::from_points(&points, &mesh, &AlignParams3::from_origin(None));
let displaced = AlignParams3::new(
crate::geom3::align3::AlignOrigin3::Origin,
Some(Iso3::translation(0.0, 0.0, 20.0)),
None,
);
let there = AlignInfo3::from_points(&points, &mesh, &displaced);
let delta = (here.information() - there.information()).norm();
assert!(
delta > 1e-6,
"the working offset had no effect (delta {delta})"
);
}
}