use crate::Point2;
use crate::common::align::information::{AlignDofs, AlignInfo};
use crate::geom2::align2::jacobian::point_surf_jacobian;
use crate::geom2::align2::{AlignParams2, Dof3, SurfaceTarget2};
pub type AlignInfo2 = AlignInfo<Dof3, 3>;
impl AlignDofs<3> for Dof3 {
fn free_indices(&self) -> Vec<usize> {
[self.tx, self.ty, self.rz]
.iter()
.enumerate()
.filter_map(|(i, &free)| free.then_some(i))
.collect()
}
}
impl AlignInfo2 {
pub fn from_points(
points: &[Point2],
target: &impl SurfaceTarget2,
params: &AlignParams2,
) -> 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::geom2::align2::{AlignOrigin2, AlignStorage2, AlignSurfMatch2};
use crate::na::Matrix3;
use crate::{Curve2, Iso2, UnitVec2, Vector2};
use approx::assert_relative_eq;
struct GroundLine;
impl SurfaceTarget2 for GroundLine {
fn find_align_match(&self, p: &Point2) -> AlignSurfMatch2 {
AlignSurfMatch2::new(
Point2::new(p.x, 0.0),
UnitVec2::new_unchecked(Vector2::y()),
true,
1.0,
)
}
}
fn line_points() -> Vec<Point2> {
(-6..=6).map(|i| Point2::new(i as f64, 1.0)).collect()
}
fn rect_curve() -> Curve2 {
let points =
[(-5.0, -2.5), (5.0, -2.5), (5.0, 2.5), (-5.0, 2.5)].map(|(x, y)| Point2::new(x, y));
Curve2::from_points(&points, 1e-8, true).unwrap()
}
fn rect_points(curve: &Curve2) -> Vec<Point2> {
let spacing = 0.3;
let n = (curve.length() / spacing).floor() as usize;
(0..n)
.filter_map(|k| {
curve
.at_length((k as f64 + 0.5) * spacing)
.map(|s| s.point())
})
.collect()
}
#[test]
fn a_straight_line_leaves_one_motion_unconstrained() {
let info = AlignInfo2::from_points(
&line_points(),
&GroundLine,
&AlignParams2::from_origin(None),
);
let weak = info.weak_directions();
assert_eq!(weak.len(), 3);
assert!(
weak[0].0.abs() < 1e-9,
"expected an unconstrained motion, got {}",
weak[0].0
);
assert!(
weak[1].0 > 1.0,
"expected the remaining motions to be constrained, got {}",
weak[1].0
);
let direction = weak[0].1;
assert_relative_eq!(direction[0].abs(), 1.0, epsilon = 1e-9);
assert!(direction[1].abs() < 1e-9);
assert!(direction[2].abs() < 1e-9);
}
#[test]
fn a_singular_problem_reports_rather_than_lying() {
let info = AlignInfo2::from_points(
&line_points(),
&GroundLine,
&AlignParams2::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_motion_makes_a_line_well_conditioned() {
let dof = Dof3::new(false, true, true);
let params = AlignParams2::from_origin(Some(dof));
let info = AlignInfo2::from_points(&line_points(), &GroundLine, ¶ms);
assert_eq!(info.free_dof_count(), 2);
let precision = info.marginal_precision().unwrap();
assert_eq!(precision[0], None, "tx was locked");
for i in [1usize, 2] {
assert!(
precision[i].unwrap() > 0.0,
"parameter {i} should be constrained"
);
}
}
#[test]
fn a_closed_curve_constrains_everything() {
let curve = rect_curve();
let points = rect_points(&curve);
let params = AlignParams2::from_origin(None);
let info = AlignInfo2::from_points(&points, &curve, ¶ms);
assert_eq!(info.free_dof_count(), 3);
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 curve"
);
}
}
#[test]
fn leverage_sums_to_the_free_dof_count() {
let curve = rect_curve();
let points = rect_points(&curve);
let info = AlignInfo2::from_points(&points, &curve, &AlignParams2::from_origin(None));
let total: f64 = info.leverage().unwrap().iter().sum();
assert_relative_eq!(total, 3.0, epsilon = 1e-9);
}
#[test]
fn leverage_sums_to_the_free_dof_count_when_constrained() {
let curve = rect_curve();
let points = rect_points(&curve);
let dof = Dof3::new(false, true, true);
let info = AlignInfo2::from_points(&points, &curve, &AlignParams2::from_origin(Some(dof)));
assert_eq!(info.free_dof_count(), 2);
let total: f64 = info.leverage().unwrap().iter().sum();
assert_relative_eq!(total, 2.0, epsilon = 1e-9);
}
#[test]
fn selection_prefers_isolated_points_over_a_redundant_cluster() {
let curve = rect_curve();
let mut points = Vec::new();
for i in 0..20 {
points.push(Point2::new(1.0 + i as f64 * 1e-4, -2.5));
}
let cluster = 0..20;
let isolated = [
Point2::new(5.0, 0.0),
Point2::new(-5.0, 1.0),
Point2::new(0.0, 2.5),
Point2::new(-3.0, 2.5),
];
points.extend_from_slice(&isolated);
let info = AlignInfo2::from_points(&points, &curve, &AlignParams2::from_origin(None));
let picked = info.select_d_optimal(4, None);
assert_eq!(picked.len(), 4);
let from_cluster = picked.iter().filter(|&&i| cluster.contains(&i)).count();
assert_eq!(
from_cluster, 1,
"expected exactly one cluster member among {picked:?}"
);
let more = info.select_d_optimal(5, None);
assert_eq!(
more.iter().filter(|&&i| cluster.contains(&i)).count(),
2,
"expected the fifth pick to fall back to the cluster: {more:?}"
);
}
#[test]
fn selection_is_a_prefix_so_it_can_be_truncated() {
let curve = rect_curve();
let points = rect_points(&curve);
let info = AlignInfo2::from_points(&points, &curve, &AlignParams2::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 curve = rect_curve();
let points = rect_points(&curve);
let params = AlignParams2::from_origin(None);
let info = AlignInfo2::from_points(&points, &curve, ¶ms);
let n = 20;
let picked = info.select_d_optimal(n, None);
let chosen: Vec<Point2> = picked.iter().map(|&i| points[i]).collect();
let arbitrary: Vec<Point2> = points.iter().take(n).copied().collect();
let weakest = |pts: &[Point2]| -> f64 {
AlignInfo2::from_points(pts, &curve, ¶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!(
AlignInfo2::from_points(&chosen, &curve, ¶ms)
.marginal_precision()
.is_ok(),
"the selected subset should leave no motion unconstrained"
);
}
#[test]
fn selection_respects_the_available_count() {
let curve = rect_curve();
let points = rect_points(&curve);
let info = AlignInfo2::from_points(&points, &curve, &AlignParams2::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![
AlignStorage2::new(1.0, 0.0, 0.0),
AlignStorage2::new(0.0, 1.0, 0.0),
AlignStorage2::new(0.0, 0.0, 1.0),
];
let weights = vec![1.0, 0.0, 1.0];
let info = AlignInfo2::from_rows(rows, weights, Dof3::all()).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![AlignStorage2::new(1.0, 2.0, 0.0)];
let dof = Dof3::new(true, true, false);
let info = AlignInfo2::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[(1, 1)], 4.0, epsilon = 1e-12);
for j in 0..3 {
assert_eq!(h[(2, j)], 0.0);
assert_eq!(h[(j, 2)], 0.0);
}
}
#[test]
fn mismatched_rows_and_weights_are_rejected() {
let rows = vec![AlignStorage2::zeros(); 3];
let err = AlignInfo2::from_rows(rows, vec![1.0; 2], Dof3::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![AlignStorage2::zeros(); 2];
let result = AlignInfo2::from_rows(rows, vec![1.0, bad], Dof3::all());
assert!(result.is_err(), "weight {bad} should have been rejected");
}
}
#[test]
fn a_fully_locked_problem_degrades_gracefully() {
let dof = Dof3::new(false, false, false);
let info =
AlignInfo2::from_rows(vec![AlignStorage2::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(), Matrix3::zeros());
}
#[test]
fn the_local_origin_is_respected() {
let curve = rect_curve();
let points = rect_points(&curve);
let at_origin = AlignInfo2::from_points(&points, &curve, &AlignParams2::from_origin(None));
let offset = AlignInfo2::from_points(
&points,
&curve,
&AlignParams2::from_center(Point2::new(50.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 curve = rect_curve();
let points = rect_points(&curve);
let here = AlignInfo2::from_points(&points, &curve, &AlignParams2::from_origin(None));
let displaced = AlignParams2::new(
AlignOrigin2::Origin,
Some(Iso2::translation(0.0, 20.0)),
None,
);
let there = AlignInfo2::from_points(&points, &curve, &displaced);
let delta = (here.information() - there.information()).norm();
assert!(
delta > 1e-6,
"the working offset had no effect (delta {delta})"
);
}
}