use crate::Result;
use crate::na::{DMatrix, DVector, SMatrix, SVector};
const DEFAULT_RIDGE_REL: f64 = 1e-9;
pub trait AlignDofs<const N: usize>: Copy {
fn free_indices(&self) -> Vec<usize>;
}
#[derive(Clone, Debug)]
pub struct AlignInfo<D: AlignDofs<N>, const N: usize> {
rows: Vec<SVector<f64, N>>,
weights: Vec<f64>,
dof: D,
free: Vec<usize>,
h: DMatrix<f64>,
}
impl<D: AlignDofs<N>, const N: usize> AlignInfo<D, N> {
pub fn from_rows(rows: Vec<SVector<f64, N>>, weights: Vec<f64>, dof: D) -> Result<Self> {
if rows.len() != weights.len() {
return Err(format!(
"there are {} jacobian rows but {} weights",
rows.len(),
weights.len()
)
.into());
}
if let Some(i) = weights.iter().position(|w| !w.is_finite() || *w < 0.0) {
return Err(format!(
"weights[{}] is {}, but every weight must be finite and non-negative",
i, weights[i]
)
.into());
}
Ok(Self::assemble(rows, weights, dof))
}
pub(crate) fn assemble(rows: Vec<SVector<f64, N>>, weights: Vec<f64>, dof: D) -> Self {
let free = dof.free_indices();
let k = free.len();
let mut h = DMatrix::zeros(k, k);
for (row, &w) in rows.iter().zip(weights.iter()) {
let j = restrict(row, &free);
h.ger(w, &j, &j, 1.0);
}
Self {
rows,
weights,
dof,
free,
h,
}
}
pub fn len(&self) -> usize {
self.rows.len()
}
pub fn is_empty(&self) -> bool {
self.rows.is_empty()
}
pub fn dof(&self) -> D {
self.dof
}
pub fn free_dof_count(&self) -> usize {
self.free.len()
}
pub fn information(&self) -> SMatrix<f64, N, N> {
let mut full = SMatrix::zeros();
for (a, &i) in self.free.iter().enumerate() {
for (b, &j) in self.free.iter().enumerate() {
full[(i, j)] = self.h[(a, b)];
}
}
full
}
pub fn marginal_precision(&self) -> Result<[Option<f64>; N]> {
let inverse = self.h.clone().try_inverse().ok_or_else(|| {
format!(
"the information matrix is singular: the points do not constrain all {} free \
degrees of freedom. Use `weak_directions` to see which motions are free.",
self.free.len()
)
})?;
let mut out = [None; N];
for (a, &i) in self.free.iter().enumerate() {
out[i] = Some(1.0 / inverse[(a, a)]);
}
Ok(out)
}
pub fn weak_directions(&self) -> Vec<(f64, SVector<f64, N>)> {
if self.free.is_empty() {
return Vec::new();
}
let eigen = self.h.clone().symmetric_eigen();
let mut pairs: Vec<(f64, SVector<f64, N>)> = eigen
.eigenvalues
.iter()
.enumerate()
.map(|(a, &value)| {
let column = eigen.eigenvectors.column(a);
let mut direction = SVector::zeros();
for (b, &i) in self.free.iter().enumerate() {
direction[i] = column[b];
}
(value, direction)
})
.collect();
pairs.sort_by(|a, b| a.0.total_cmp(&b.0));
pairs
}
pub fn leverage(&self) -> Result<Vec<f64>> {
let inverse = self.h.clone().try_inverse().ok_or_else(|| {
format!(
"the information matrix is singular: the points do not constrain all {} free \
degrees of freedom, so leverage is undefined.",
self.free.len()
)
})?;
Ok(self
.rows
.iter()
.zip(self.weights.iter())
.map(|(row, &w)| {
let j = restrict(row, &self.free);
w * (j.transpose() * &inverse * &j)[(0, 0)]
})
.collect())
}
pub fn select_d_optimal(&self, count: usize, ridge_rel: Option<f64>) -> Vec<usize> {
let k = self.free.len();
if k == 0 || count == 0 || self.rows.is_empty() {
return Vec::new();
}
let scale = self.h.trace() / k as f64;
let ridge = ridge_rel.unwrap_or(DEFAULT_RIDGE_REL) * if scale > 0.0 { scale } else { 1.0 };
let mut inverse = DMatrix::identity(k, k) / ridge;
let restricted: Vec<DVector<f64>> =
self.rows.iter().map(|r| restrict(r, &self.free)).collect();
let mut chosen = Vec::with_capacity(count.min(self.rows.len()));
let mut taken = vec![false; self.rows.len()];
for _ in 0..count.min(self.rows.len()) {
let mut best = None;
let mut best_gain = 0.0;
for i in 0..self.rows.len() {
if taken[i] || self.weights[i] <= 0.0 {
continue;
}
let j = &restricted[i];
let gain = self.weights[i] * (j.transpose() * &inverse * j)[(0, 0)];
if gain > best_gain {
best_gain = gain;
best = Some(i);
}
}
let Some(i) = best else { break };
let j = &restricted[i];
let hj = &inverse * j;
let denom = 1.0 + best_gain;
inverse -= (&hj * hj.transpose()) * (self.weights[i] / denom);
taken[i] = true;
chosen.push(i);
}
chosen
}
}
fn restrict<const N: usize>(row: &SVector<f64, N>, free: &[usize]) -> DVector<f64> {
DVector::from_iterator(free.len(), free.iter().map(|&i| row[i]))
}