- move linear algebra functions to separate module
- finish modal_analysis, still untested
This commit is contained in:
1 parent
3f2f01d795
commit
94e4bd3eb6
3 files changed
+353
-281
No files matched your search
+83
-281
@@ -1,8 +1,8 @@
|
|||||||
use crate::error::Error;
|
use crate::error::Error;
|
||||||
use crate::error::Error::MaxIterations;
|
use crate::linalg::{linefit, lu_factor, matrix_determinant, matrix_inverse, symmetric_eigen};
|
||||||
use itertools::Itertools;
|
use itertools::Itertools;
|
||||||
use ndarray::{
|
use ndarray::{
|
||||||
Array, Array1, Array2, ArrayView1, AsArray, Axis, Dimension, Ix1, Ix2, SliceInfoElem,
|
Array, Array1, Array2, ArrayView1, AsArray, Axis, Dimension, IntoDimension, Ix1, SliceInfoElem,
|
||||||
};
|
};
|
||||||
use statrs::function::erf::erf;
|
use statrs::function::erf::erf;
|
||||||
use statrs::statistics::Statistics;
|
use statrs::statistics::Statistics;
|
||||||
@@ -160,7 +160,6 @@ where
|
|||||||
{
|
{
|
||||||
let g = g.into().into_dyn();
|
let g = g.into().into_dyn();
|
||||||
let shape = g.shape();
|
let shape = g.shape();
|
||||||
let last_index = shape.iter().map(|i| i - 1).collect::<Vec<_>>();
|
|
||||||
let ndim = shape.len();
|
let ndim = shape.len();
|
||||||
let mut x = x.to_vec();
|
let mut x = x.to_vec();
|
||||||
for _ in 0..n_iter {
|
for _ in 0..n_iter {
|
||||||
@@ -204,40 +203,93 @@ where
|
|||||||
step: 1,
|
step: 1,
|
||||||
})
|
})
|
||||||
.collect::<Vec<_>>();
|
.collect::<Vec<_>>();
|
||||||
let gs = g.slice(slice.as_slice());
|
let mut gs = g.slice(slice.as_slice()).to_owned();
|
||||||
let bg = match background_subtraction {
|
let bg = match background_subtraction {
|
||||||
BackgroundSubtraction::None => vec![0.0; 1 + ndim],
|
BackgroundSubtraction::None => vec![0.0; 1 + ndim],
|
||||||
BackgroundSubtraction::Constant => {
|
BackgroundSubtraction::Constant => {
|
||||||
let mut bg = vec![0.0; 1 + ndim];
|
let mut bg = vec![0.0; 1 + ndim];
|
||||||
bg[0] = gs
|
let mut slice = vec![
|
||||||
.indexed_iter()
|
SliceInfoElem::Slice {
|
||||||
.filter_map(|(i, v)| {
|
start: 0,
|
||||||
if i.as_array_view()
|
end: None,
|
||||||
.iter()
|
step: 1
|
||||||
.zip(&last_index)
|
};
|
||||||
.any(|(&i, &li)| (i == 0) || (i == li))
|
ndim
|
||||||
{
|
];
|
||||||
Some(v)
|
let n = shape.iter().product::<usize>()
|
||||||
} else {
|
- shape.iter().map(|i| i - 2).product::<usize>();
|
||||||
None
|
// mean of values around edges
|
||||||
}
|
bg[0] = shape.iter().enumerate().fold(0.0, |mut acc, (i, &si)| {
|
||||||
})
|
slice[i] = SliceInfoElem::Slice {
|
||||||
.mean();
|
start: 0,
|
||||||
|
end: None,
|
||||||
|
step: (si as isize - 1).max(1),
|
||||||
|
};
|
||||||
|
acc += gs.slice(slice.as_slice()).sum();
|
||||||
|
slice[i] = SliceInfoElem::Slice {
|
||||||
|
start: 1,
|
||||||
|
end: Some(si as isize - 2),
|
||||||
|
step: 1,
|
||||||
|
};
|
||||||
|
acc
|
||||||
|
}) / n as f64;
|
||||||
|
gs -= bg[0];
|
||||||
bg
|
bg
|
||||||
}
|
}
|
||||||
BackgroundSubtraction::Tilted => {
|
BackgroundSubtraction::Tilted => {
|
||||||
let mut bg = vec![0.0; 1 + ndim];
|
let mut bg = vec![0.0];
|
||||||
let (_i, v) = gs
|
let mut slice = vec![
|
||||||
.indexed_iter()
|
SliceInfoElem::Slice {
|
||||||
.filter(|(i, _)| {
|
start: 0,
|
||||||
i.as_array_view()
|
end: None,
|
||||||
.iter()
|
step: 1
|
||||||
.zip(&last_index)
|
};
|
||||||
.any(|(&i, &li)| (i == 0) || (i == li))
|
ndim
|
||||||
})
|
];
|
||||||
.collect::<(Vec<_>, Vec<f64>)>();
|
let mut perm = Vec::from_iter(1..ndim);
|
||||||
bg[0] = v.mean();
|
perm.push(0);
|
||||||
// TODO
|
let perm = perm.into_dimension();
|
||||||
|
// rotate axes, for each dimension fitting edges with y = a*x + b
|
||||||
|
bg.extend(shape.iter().zip(&x).map(|(&si, xi)| {
|
||||||
|
let mut xx = Vec::from_iter(0..si);
|
||||||
|
xx.extend(0..si);
|
||||||
|
let xx = Array1::from_vec(xx).mapv(|i| i as f64 - xi);
|
||||||
|
slice[0] = SliceInfoElem::Slice {
|
||||||
|
start: 0,
|
||||||
|
end: None,
|
||||||
|
step: (si as isize - 1).max(1),
|
||||||
|
};
|
||||||
|
let yy = gs.slice(slice.as_slice());
|
||||||
|
let (a, b) = linefit(xx.view(), yy.flatten());
|
||||||
|
gs = &gs - a * Array1::from_iter(0..si).mapv(|i| i as f64 - xi) - b;
|
||||||
|
gs.permute_axes(perm.clone());
|
||||||
|
a
|
||||||
|
}));
|
||||||
|
slice = vec![
|
||||||
|
SliceInfoElem::Slice {
|
||||||
|
start: 0,
|
||||||
|
end: None,
|
||||||
|
step: 1
|
||||||
|
};
|
||||||
|
ndim
|
||||||
|
];
|
||||||
|
let n = shape.iter().product::<usize>()
|
||||||
|
- shape.iter().map(|i| i.saturating_sub(2)).product::<usize>();
|
||||||
|
// mean of values around edges
|
||||||
|
bg[0] = shape.iter().enumerate().fold(0.0, |mut acc, (i, &si)| {
|
||||||
|
slice[i] = SliceInfoElem::Slice {
|
||||||
|
start: 0,
|
||||||
|
end: None,
|
||||||
|
step: (si as isize - 1).max(1),
|
||||||
|
};
|
||||||
|
acc += gs.slice(slice.as_slice()).sum();
|
||||||
|
slice[i] = SliceInfoElem::Slice {
|
||||||
|
start: 1,
|
||||||
|
end: Some(si as isize - 2),
|
||||||
|
step: 1,
|
||||||
|
};
|
||||||
|
acc
|
||||||
|
}) / n as f64;
|
||||||
bg
|
bg
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
@@ -271,7 +323,7 @@ where
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
Err(MaxIterations(n_iter))
|
Err(Error::MaxIterations(n_iter))
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Analytic integral of the 1D gaussian `exp(-t^2/(2w)) / sqrt(2*pi*w)`
|
/// Analytic integral of the 1D gaussian `exp(-t^2/(2w)) / sqrt(2*pi*w)`
|
||||||
@@ -294,260 +346,10 @@ fn pixel_factor(z: f64, w: f64, tol: f64) -> f64 {
|
|||||||
0.5 * (erf((z + 0.5) * k) - erf((z - 0.5) * k))
|
0.5 * (erf((z + 0.5) * k) - erf((z - 0.5) * k))
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Eigen-decomposition of a symmetric matrix using cyclic Jacobi rotations.
|
|
||||||
///
|
|
||||||
/// Returns the eigenvalues and the eigenvectors (as **columns** of the
|
|
||||||
/// second array), such that `matrix` = `eigenvectors` · diag(eigenvalues) · `eigenvectors`.t().
|
|
||||||
fn symmetric_eigen(matrix: &Array2<f64>) -> (Array1<f64>, Array2<f64>) {
|
|
||||||
println!("matrix = {:?}", matrix);
|
|
||||||
let n = matrix.nrows();
|
|
||||||
debug_assert_eq!(n, matrix.ncols(), "matrix must be square");
|
|
||||||
|
|
||||||
let mut a = matrix.to_owned();
|
|
||||||
let mut v = Array2::eye(n);
|
|
||||||
|
|
||||||
for _ in 0..100 {
|
|
||||||
// squared sum of the off-diagonal elements
|
|
||||||
let mut off = 0.0;
|
|
||||||
for i in 0..n {
|
|
||||||
for j in (i + 1)..n {
|
|
||||||
off += a[[i, j]] * a[[i, j]];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if off == 0.0 {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
// relative convergence criterion
|
|
||||||
let total: f64 = a.iter().map(|&x| x * x).sum();
|
|
||||||
if off <= (f64::EPSILON * total) * (f64::EPSILON * total) {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
// one sweep over the strictly upper triangle
|
|
||||||
for p in 0..n - 1 {
|
|
||||||
for q in (p + 1)..n {
|
|
||||||
let apq = a[[p, q]];
|
|
||||||
if apq == 0.0 {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
let app = a[[p, p]];
|
|
||||||
let aqq = a[[q, q]];
|
|
||||||
// rotation that zeroes a[p, q] (numerically stable form)
|
|
||||||
let theta = 0.5 * (aqq - app) / apq;
|
|
||||||
let t = if theta < 0.0 {
|
|
||||||
-1.0 / (-theta + (theta * theta + 1.0).sqrt())
|
|
||||||
} else {
|
|
||||||
1.0 / (theta + (theta * theta + 1.0).sqrt())
|
|
||||||
};
|
|
||||||
let c = 1.0 / (t * t + 1.0).sqrt();
|
|
||||||
let s = t * c;
|
|
||||||
a[[p, p]] = app - t * apq;
|
|
||||||
a[[q, q]] = aqq + t * apq;
|
|
||||||
a[[p, q]] = 0.0;
|
|
||||||
a[[q, p]] = 0.0;
|
|
||||||
for i in 0..n {
|
|
||||||
if i != p && i != q {
|
|
||||||
let apj = a[[i, p]];
|
|
||||||
let aqj = a[[i, q]];
|
|
||||||
a[[i, p]] = c * apj - s * aqj;
|
|
||||||
a[[i, q]] = s * apj + c * aqj;
|
|
||||||
}
|
|
||||||
// v = V * R: the rotation applies to every row,
|
|
||||||
// including rows p and q
|
|
||||||
let vpj = v[[i, p]];
|
|
||||||
let vqj = v[[i, q]];
|
|
||||||
v[[i, p]] = c * vpj - s * vqj;
|
|
||||||
v[[i, q]] = s * vpj + c * vqj;
|
|
||||||
}
|
|
||||||
for i in 0..n {
|
|
||||||
if i != p && i != q {
|
|
||||||
let apj = a[[p, i]];
|
|
||||||
let aqj = a[[q, i]];
|
|
||||||
a[[p, i]] = c * apj - s * aqj;
|
|
||||||
a[[q, i]] = s * apj + c * aqj;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
let w = (0..n).map(|i| a[[i, i]]).collect::<Array1<f64>>();
|
|
||||||
(w, v)
|
|
||||||
}
|
|
||||||
|
|
||||||
/// PA = LU factorization (Doolittle with partial pivoting).
|
|
||||||
///
|
|
||||||
/// Returns the packed LU matrix (lower unit triangular in columns
|
|
||||||
/// 0..k, upper triangular in rows k) and the row permutation, or `SingularMatrix`
|
|
||||||
/// if a pivot is singular or numerically rank-deficient.
|
|
||||||
fn lu_factor<'a, A>(matrix: A) -> Result<(Array2<f64>, Vec<usize>), Error>
|
|
||||||
where
|
|
||||||
A: AsArray<'a, f64, Ix2>,
|
|
||||||
{
|
|
||||||
let matrix = matrix.into();
|
|
||||||
let n = matrix.nrows();
|
|
||||||
debug_assert_eq!(n, matrix.ncols(), "matrix must be square");
|
|
||||||
|
|
||||||
// pivot magnitudes must exceed this relative threshold to count as significant
|
|
||||||
let max_abs = matrix.iter().fold(0.0f64, |m, &x| m.max(x.abs()));
|
|
||||||
let threshold = max_abs * (n as f64) * f64::EPSILON;
|
|
||||||
|
|
||||||
let mut lu = matrix.to_owned();
|
|
||||||
let mut perm: Vec<usize> = (0..n).collect();
|
|
||||||
|
|
||||||
for k in 0..n {
|
|
||||||
let mut pivot_row = k;
|
|
||||||
for i in (k + 1)..n {
|
|
||||||
if lu[[i, k]].abs() > lu[[pivot_row, k]].abs() {
|
|
||||||
pivot_row = i;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if lu[[pivot_row, k]].abs() <= threshold || !lu[[pivot_row, k]].is_finite() {
|
|
||||||
return Err(Error::SingularMatrix);
|
|
||||||
}
|
|
||||||
if pivot_row != k {
|
|
||||||
for j in 0..n {
|
|
||||||
lu.swap([k, j], [pivot_row, j]);
|
|
||||||
}
|
|
||||||
perm.swap(k, pivot_row);
|
|
||||||
}
|
|
||||||
|
|
||||||
for i in (k + 1)..n {
|
|
||||||
let factor = lu[[i, k]] / lu[[k, k]];
|
|
||||||
lu[[i, k]] = factor;
|
|
||||||
for j in (k + 1)..n {
|
|
||||||
lu[[i, j]] -= factor * lu[[k, j]];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
Ok((lu, perm))
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Calculate the inverse of a square matrix using LU factorization with partial
|
|
||||||
/// pivoting.
|
|
||||||
fn matrix_inverse(lu: &Array2<f64>, perm: &[usize]) -> Array2<f64> {
|
|
||||||
let n = lu.nrows();
|
|
||||||
|
|
||||||
// Solve A * inv = I column by column: first L*y = P*e_col, then U*x = y.
|
|
||||||
let mut inv = Array2::zeros((n, n));
|
|
||||||
for col in 0..n {
|
|
||||||
let mut y = vec![0.0; n];
|
|
||||||
for i in 0..n {
|
|
||||||
let rhs = if perm[i] == col { 1.0 } else { 0.0 };
|
|
||||||
let mut sum = rhs;
|
|
||||||
for j in 0..i {
|
|
||||||
sum -= lu[[i, j]] * y[j];
|
|
||||||
}
|
|
||||||
y[i] = sum;
|
|
||||||
}
|
|
||||||
for i in (0..n).rev() {
|
|
||||||
let mut sum = y[i];
|
|
||||||
for j in (i + 1)..n {
|
|
||||||
sum -= lu[[i, j]] * inv[[j, col]];
|
|
||||||
}
|
|
||||||
inv[[i, col]] = sum / lu[[i, i]];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
inv
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Calculate the determinant of a square matrix using LU factorization
|
|
||||||
/// with partial pivoting.
|
|
||||||
fn matrix_determinant(lu: &Array2<f64>, perm: &[usize]) -> f64 {
|
|
||||||
// sign of the row permutation: parity of its inversion count
|
|
||||||
let inversions = (1..perm.len())
|
|
||||||
.map(|i| (0..i).filter(|&j| perm[j] > perm[i]).count())
|
|
||||||
.sum::<usize>();
|
|
||||||
let sign = if inversions % 2 == 0 { 1.0 } else { -1.0 };
|
|
||||||
sign * (0..lu.nrows()).map(|k| lu[[k, k]]).product::<f64>()
|
|
||||||
}
|
|
||||||
|
|
||||||
#[cfg(test)]
|
#[cfg(test)]
|
||||||
mod tests {
|
mod tests {
|
||||||
use super::*;
|
use super::*;
|
||||||
|
|
||||||
#[test]
|
|
||||||
fn test_matrix_inverse() -> Result<(), Error> {
|
|
||||||
let matrix = Array2::from_shape_vec((2, 2), vec![2.12132, -3.535534, 2.12132, 3.535534])?;
|
|
||||||
let (lu, perm) = lu_factor(&matrix)?;
|
|
||||||
let inverse = matrix_inverse(&lu, &perm);
|
|
||||||
let truth = Array2::from_shape_vec((2, 2), vec![0.235702, 0.235702, -0.141421, 0.141421])?;
|
|
||||||
assert!((inverse - truth).sum().abs() < 1e-6);
|
|
||||||
Ok(())
|
|
||||||
}
|
|
||||||
|
|
||||||
#[test]
|
|
||||||
fn test_matrix_det() -> Result<(), Error> {
|
|
||||||
// trivial 1x1
|
|
||||||
let m = Array2::from_shape_vec((1, 1), vec![3.0])?;
|
|
||||||
let (lu, perm) = lu_factor(&m)?;
|
|
||||||
assert_eq!(matrix_determinant(&lu, &perm), 3.0);
|
|
||||||
// positive determinant, needs a row swap
|
|
||||||
let m = Array2::from_shape_vec((2, 2), vec![2.0, 1.0, 1.0, 3.0])?;
|
|
||||||
let (lu, perm) = lu_factor(&m)?;
|
|
||||||
assert!((matrix_determinant(&lu, &perm) - 5.0).abs() < 1e-14);
|
|
||||||
// negative determinant
|
|
||||||
let m = Array2::from_shape_vec((2, 2), vec![1.0, 2.0, 2.0, 1.0])?;
|
|
||||||
let (lu, perm) = lu_factor(&m)?;
|
|
||||||
assert!((matrix_determinant(&lu, &perm) + 3.0).abs() < 1e-14);
|
|
||||||
// 3x3 with det = 13
|
|
||||||
let m = Array2::from_shape_vec((3, 3), vec![2.0, 0.0, 1.0, 1.0, 2.0, 0.0, 0.0, 1.0, 3.0])?;
|
|
||||||
let (lu, perm) = lu_factor(&m)?;
|
|
||||||
assert!((matrix_determinant(&lu, &perm) - 13.0).abs() < 1e-13);
|
|
||||||
// 3x3 requiring row swaps (zero in the leading column), det = 2
|
|
||||||
let m = Array2::from_shape_vec((3, 3), vec![0.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0])?;
|
|
||||||
let (lu, perm) = lu_factor(&m)?;
|
|
||||||
assert!((matrix_determinant(&lu, &perm) - 2.0).abs() < 1e-12);
|
|
||||||
// singular matrix
|
|
||||||
let m = Array2::from_shape_vec((2, 2), vec![1.0, 2.0, 2.0, 4.0])?;
|
|
||||||
assert!(lu_factor(&m).is_err());
|
|
||||||
// det(A * B) == det(A) * det(B)
|
|
||||||
let a = Array2::from_shape_vec((3, 3), vec![1.0, 2.0, 3.0, 0.0, 1.0, 4.0, 5.0, 6.0, 0.0])?;
|
|
||||||
let b =
|
|
||||||
Array2::from_shape_vec((3, 3), vec![0.0, 1.0, 2.0, -1.0, 0.0, 3.0, 2.0, -1.0, 0.0])?;
|
|
||||||
let prod = a.dot(&b);
|
|
||||||
let (lu, perm) = lu_factor(&prod)?;
|
|
||||||
let ab = matrix_determinant(&lu, &perm);
|
|
||||||
let (lu, perm) = lu_factor(&a)?;
|
|
||||||
let a_det = matrix_determinant(&lu, &perm);
|
|
||||||
let (lu, perm) = lu_factor(&b)?;
|
|
||||||
let b_det = matrix_determinant(&lu, &perm);
|
|
||||||
assert!((ab - a_det * b_det).abs() < 1e-10);
|
|
||||||
Ok(())
|
|
||||||
}
|
|
||||||
|
|
||||||
#[test]
|
|
||||||
fn test_symmetric_eigen() -> Result<(), Error> {
|
|
||||||
let m = Array2::from_shape_vec(
|
|
||||||
(3, 3),
|
|
||||||
vec![5.0_f64, 1.0, 2.0, 1.0, 4.0, -1.0, 2.0, -1.0, 3.0],
|
|
||||||
)?;
|
|
||||||
let (w, v) = symmetric_eigen(&m);
|
|
||||||
// trace and determinant are preserved
|
|
||||||
assert!((w.iter().sum::<f64>() - 12.0).abs() < 1e-10);
|
|
||||||
let (lu, perm) = lu_factor(&m)?;
|
|
||||||
assert!((w.iter().product::<f64>() - matrix_determinant(&lu, &perm)).abs() < 1e-8);
|
|
||||||
// reconstruction v * diag(w) * v.t() = m
|
|
||||||
let d = Array2::from_shape_fn((3, 3), |rc| if rc.0 == rc.1 { w[rc.0] } else { 0.0 });
|
|
||||||
let recon = v.clone().dot(&d).dot(&v.t());
|
|
||||||
let err = (recon - m.clone())
|
|
||||||
.into_iter()
|
|
||||||
.fold(0.0_f64, |e, x| e.max(x.abs()));
|
|
||||||
assert!(err < 1e-8);
|
|
||||||
// eigenvectors are orthonormal
|
|
||||||
let gv = v.t().dot(&v);
|
|
||||||
let gerr = (gv - Array2::<f64>::eye(3))
|
|
||||||
.into_iter()
|
|
||||||
.fold(0.0_f64, |e, x| e.max(x.abs()));
|
|
||||||
assert!(gerr < 1e-9);
|
|
||||||
// degenerate input
|
|
||||||
let z = Array2::zeros((2, 2));
|
|
||||||
let (wz, _) = symmetric_eigen(&z);
|
|
||||||
assert!(wz.iter().all(|&x| x == 0.0));
|
|
||||||
Ok(())
|
|
||||||
}
|
|
||||||
|
|
||||||
#[test]
|
#[test]
|
||||||
fn test_pixel_factor() {
|
fn test_pixel_factor() {
|
||||||
// analytic: w = 0.5 -> exp(-t^2) / sqrt(pi)
|
// analytic: w = 0.5 -> exp(-t^2) / sqrt(pi)
|
||||||
|
|||||||
@@ -2,5 +2,6 @@ pub mod detection;
|
|||||||
pub mod error;
|
pub mod error;
|
||||||
pub mod filter;
|
pub mod filter;
|
||||||
pub mod gaussian;
|
pub mod gaussian;
|
||||||
|
mod linalg;
|
||||||
#[cfg(feature = "python")]
|
#[cfg(feature = "python")]
|
||||||
mod py;
|
mod py;
|
||||||
+269
@@ -0,0 +1,269 @@
|
|||||||
|
use crate::error::Error;
|
||||||
|
use ndarray::{Array1, Array2, ArrayView1, AsArray, CowArray, Ix1, Ix2};
|
||||||
|
use statrs::statistics::Statistics;
|
||||||
|
|
||||||
|
/// Eigen-decomposition of a symmetric matrix using cyclic Jacobi rotations.
|
||||||
|
///
|
||||||
|
/// Returns the eigenvalues and the eigenvectors (as **columns** of the
|
||||||
|
/// second array), such that `matrix` = `eigenvectors` · diag(eigenvalues) · `eigenvectors`.t().
|
||||||
|
pub(crate) fn symmetric_eigen(matrix: &Array2<f64>) -> (Array1<f64>, Array2<f64>) {
|
||||||
|
println!("matrix = {:?}", matrix);
|
||||||
|
let n = matrix.nrows();
|
||||||
|
debug_assert_eq!(n, matrix.ncols(), "matrix must be square");
|
||||||
|
|
||||||
|
let mut a = matrix.to_owned();
|
||||||
|
let mut v = Array2::eye(n);
|
||||||
|
|
||||||
|
for _ in 0..100 {
|
||||||
|
// squared sum of the off-diagonal elements
|
||||||
|
let mut off = 0.0;
|
||||||
|
for i in 0..n {
|
||||||
|
for j in (i + 1)..n {
|
||||||
|
off += a[[i, j]] * a[[i, j]];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if off == 0.0 {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
// relative convergence criterion
|
||||||
|
let total: f64 = a.iter().map(|&x| x * x).sum();
|
||||||
|
if off <= (f64::EPSILON * total) * (f64::EPSILON * total) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// one sweep over the strictly upper triangle
|
||||||
|
for p in 0..n - 1 {
|
||||||
|
for q in (p + 1)..n {
|
||||||
|
let apq = a[[p, q]];
|
||||||
|
if apq == 0.0 {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
let app = a[[p, p]];
|
||||||
|
let aqq = a[[q, q]];
|
||||||
|
// rotation that zeroes a[p, q] (numerically stable form)
|
||||||
|
let theta = 0.5 * (aqq - app) / apq;
|
||||||
|
let t = if theta < 0.0 {
|
||||||
|
-1.0 / (-theta + (theta * theta + 1.0).sqrt())
|
||||||
|
} else {
|
||||||
|
1.0 / (theta + (theta * theta + 1.0).sqrt())
|
||||||
|
};
|
||||||
|
let c = 1.0 / (t * t + 1.0).sqrt();
|
||||||
|
let s = t * c;
|
||||||
|
a[[p, p]] = app - t * apq;
|
||||||
|
a[[q, q]] = aqq + t * apq;
|
||||||
|
a[[p, q]] = 0.0;
|
||||||
|
a[[q, p]] = 0.0;
|
||||||
|
for i in 0..n {
|
||||||
|
if i != p && i != q {
|
||||||
|
let apj = a[[i, p]];
|
||||||
|
let aqj = a[[i, q]];
|
||||||
|
a[[i, p]] = c * apj - s * aqj;
|
||||||
|
a[[i, q]] = s * apj + c * aqj;
|
||||||
|
}
|
||||||
|
// v = V * R: the rotation applies to every row,
|
||||||
|
// including rows p and q
|
||||||
|
let vpj = v[[i, p]];
|
||||||
|
let vqj = v[[i, q]];
|
||||||
|
v[[i, p]] = c * vpj - s * vqj;
|
||||||
|
v[[i, q]] = s * vpj + c * vqj;
|
||||||
|
}
|
||||||
|
for i in 0..n {
|
||||||
|
if i != p && i != q {
|
||||||
|
let apj = a[[p, i]];
|
||||||
|
let aqj = a[[q, i]];
|
||||||
|
a[[p, i]] = c * apj - s * aqj;
|
||||||
|
a[[q, i]] = s * apj + c * aqj;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
let w = (0..n).map(|i| a[[i, i]]).collect::<Array1<f64>>();
|
||||||
|
(w, v)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// PA = LU factorization (Doolittle with partial pivoting).
|
||||||
|
///
|
||||||
|
/// Returns the packed LU matrix (lower unit triangular in columns
|
||||||
|
/// 0..k, upper triangular in rows k) and the row permutation, or `SingularMatrix`
|
||||||
|
/// if a pivot is singular or numerically rank-deficient.
|
||||||
|
pub(crate) fn lu_factor<'a, A>(matrix: A) -> Result<(Array2<f64>, Vec<usize>), Error>
|
||||||
|
where
|
||||||
|
A: AsArray<'a, f64, Ix2>,
|
||||||
|
{
|
||||||
|
let matrix = matrix.into();
|
||||||
|
let n = matrix.nrows();
|
||||||
|
debug_assert_eq!(n, matrix.ncols(), "matrix must be square");
|
||||||
|
|
||||||
|
// pivot magnitudes must exceed this relative threshold to count as significant
|
||||||
|
let max_abs = matrix.iter().fold(0.0f64, |m, &x| m.max(x.abs()));
|
||||||
|
let threshold = max_abs * (n as f64) * f64::EPSILON;
|
||||||
|
|
||||||
|
let mut lu = matrix.to_owned();
|
||||||
|
let mut perm: Vec<usize> = (0..n).collect();
|
||||||
|
|
||||||
|
for k in 0..n {
|
||||||
|
let mut pivot_row = k;
|
||||||
|
for i in (k + 1)..n {
|
||||||
|
if lu[[i, k]].abs() > lu[[pivot_row, k]].abs() {
|
||||||
|
pivot_row = i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if lu[[pivot_row, k]].abs() <= threshold || !lu[[pivot_row, k]].is_finite() {
|
||||||
|
return Err(Error::SingularMatrix);
|
||||||
|
}
|
||||||
|
if pivot_row != k {
|
||||||
|
for j in 0..n {
|
||||||
|
lu.swap([k, j], [pivot_row, j]);
|
||||||
|
}
|
||||||
|
perm.swap(k, pivot_row);
|
||||||
|
}
|
||||||
|
|
||||||
|
for i in (k + 1)..n {
|
||||||
|
let factor = lu[[i, k]] / lu[[k, k]];
|
||||||
|
lu[[i, k]] = factor;
|
||||||
|
for j in (k + 1)..n {
|
||||||
|
lu[[i, j]] -= factor * lu[[k, j]];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
Ok((lu, perm))
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Calculate the inverse of a square matrix using LU factorization with partial
|
||||||
|
/// pivoting.
|
||||||
|
pub(crate) fn matrix_inverse(lu: &Array2<f64>, perm: &[usize]) -> Array2<f64> {
|
||||||
|
let n = lu.nrows();
|
||||||
|
|
||||||
|
// Solve A * inv = I column by column: first L*y = P*e_col, then U*x = y.
|
||||||
|
let mut inv = Array2::zeros((n, n));
|
||||||
|
for col in 0..n {
|
||||||
|
let mut y = vec![0.0; n];
|
||||||
|
for i in 0..n {
|
||||||
|
let rhs = if perm[i] == col { 1.0 } else { 0.0 };
|
||||||
|
let mut sum = rhs;
|
||||||
|
for j in 0..i {
|
||||||
|
sum -= lu[[i, j]] * y[j];
|
||||||
|
}
|
||||||
|
y[i] = sum;
|
||||||
|
}
|
||||||
|
for i in (0..n).rev() {
|
||||||
|
let mut sum = y[i];
|
||||||
|
for j in (i + 1)..n {
|
||||||
|
sum -= lu[[i, j]] * inv[[j, col]];
|
||||||
|
}
|
||||||
|
inv[[i, col]] = sum / lu[[i, i]];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
inv
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Calculate the determinant of a square matrix using LU factorization
|
||||||
|
/// with partial pivoting.
|
||||||
|
pub(crate) fn matrix_determinant(lu: &Array2<f64>, perm: &[usize]) -> f64 {
|
||||||
|
// sign of the row permutation: parity of its inversion count
|
||||||
|
let inversions = (1..perm.len())
|
||||||
|
.map(|i| (0..i).filter(|&j| perm[j] > perm[i]).count())
|
||||||
|
.sum::<usize>();
|
||||||
|
let sign = if inversions % 2 == 0 { 1.0 } else { -1.0 };
|
||||||
|
sign * (0..lu.nrows()).map(|k| lu[[k, k]]).product::<f64>()
|
||||||
|
}
|
||||||
|
|
||||||
|
pub(crate) fn linefit(x: ArrayView1<f64>, y: CowArray<f64, Ix1>) -> (f64, f64) {
|
||||||
|
let mean_x = x.view().mean();
|
||||||
|
let mean_y = y.view().mean();
|
||||||
|
let x0 = &x - mean_x;
|
||||||
|
let n = (&x0 * (&y - mean_y)).sum();
|
||||||
|
let d = x0.powi(2).sum();
|
||||||
|
let a = n / d;
|
||||||
|
let b = mean_y - a * mean_x;
|
||||||
|
(a, b)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[cfg(test)]
|
||||||
|
mod tests {
|
||||||
|
use super::*;
|
||||||
|
|
||||||
|
#[test]
|
||||||
|
fn test_matrix_inverse() -> Result<(), Error> {
|
||||||
|
let matrix = Array2::from_shape_vec((2, 2), vec![2.12132, -3.535534, 2.12132, 3.535534])?;
|
||||||
|
let (lu, perm) = lu_factor(&matrix)?;
|
||||||
|
let inverse = matrix_inverse(&lu, &perm);
|
||||||
|
let truth = Array2::from_shape_vec((2, 2), vec![0.235702, 0.235702, -0.141421, 0.141421])?;
|
||||||
|
assert!((inverse - truth).sum().abs() < 1e-6);
|
||||||
|
Ok(())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[test]
|
||||||
|
fn test_matrix_det() -> Result<(), Error> {
|
||||||
|
// trivial 1x1
|
||||||
|
let m = Array2::from_shape_vec((1, 1), vec![3.0])?;
|
||||||
|
let (lu, perm) = lu_factor(&m)?;
|
||||||
|
assert_eq!(matrix_determinant(&lu, &perm), 3.0);
|
||||||
|
// positive determinant, needs a row swap
|
||||||
|
let m = Array2::from_shape_vec((2, 2), vec![2.0, 1.0, 1.0, 3.0])?;
|
||||||
|
let (lu, perm) = lu_factor(&m)?;
|
||||||
|
assert!((matrix_determinant(&lu, &perm) - 5.0).abs() < 1e-14);
|
||||||
|
// negative determinant
|
||||||
|
let m = Array2::from_shape_vec((2, 2), vec![1.0, 2.0, 2.0, 1.0])?;
|
||||||
|
let (lu, perm) = lu_factor(&m)?;
|
||||||
|
assert!((matrix_determinant(&lu, &perm) + 3.0).abs() < 1e-14);
|
||||||
|
// 3x3 with det = 13
|
||||||
|
let m = Array2::from_shape_vec((3, 3), vec![2.0, 0.0, 1.0, 1.0, 2.0, 0.0, 0.0, 1.0, 3.0])?;
|
||||||
|
let (lu, perm) = lu_factor(&m)?;
|
||||||
|
assert!((matrix_determinant(&lu, &perm) - 13.0).abs() < 1e-13);
|
||||||
|
// 3x3 requiring row swaps (zero in the leading column), det = 2
|
||||||
|
let m = Array2::from_shape_vec((3, 3), vec![0.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0])?;
|
||||||
|
let (lu, perm) = lu_factor(&m)?;
|
||||||
|
assert!((matrix_determinant(&lu, &perm) - 2.0).abs() < 1e-12);
|
||||||
|
// singular matrix
|
||||||
|
let m = Array2::from_shape_vec((2, 2), vec![1.0, 2.0, 2.0, 4.0])?;
|
||||||
|
assert!(lu_factor(&m).is_err());
|
||||||
|
// det(A * B) == det(A) * det(B)
|
||||||
|
let a = Array2::from_shape_vec((3, 3), vec![1.0, 2.0, 3.0, 0.0, 1.0, 4.0, 5.0, 6.0, 0.0])?;
|
||||||
|
let b =
|
||||||
|
Array2::from_shape_vec((3, 3), vec![0.0, 1.0, 2.0, -1.0, 0.0, 3.0, 2.0, -1.0, 0.0])?;
|
||||||
|
let prod = a.dot(&b);
|
||||||
|
let (lu, perm) = lu_factor(&prod)?;
|
||||||
|
let ab = matrix_determinant(&lu, &perm);
|
||||||
|
let (lu, perm) = lu_factor(&a)?;
|
||||||
|
let a_det = matrix_determinant(&lu, &perm);
|
||||||
|
let (lu, perm) = lu_factor(&b)?;
|
||||||
|
let b_det = matrix_determinant(&lu, &perm);
|
||||||
|
assert!((ab - a_det * b_det).abs() < 1e-10);
|
||||||
|
Ok(())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[test]
|
||||||
|
fn test_symmetric_eigen() -> Result<(), Error> {
|
||||||
|
let m = Array2::from_shape_vec(
|
||||||
|
(3, 3),
|
||||||
|
vec![5.0_f64, 1.0, 2.0, 1.0, 4.0, -1.0, 2.0, -1.0, 3.0],
|
||||||
|
)?;
|
||||||
|
let (w, v) = symmetric_eigen(&m);
|
||||||
|
// trace and determinant are preserved
|
||||||
|
assert!((w.iter().sum::<f64>() - 12.0).abs() < 1e-10);
|
||||||
|
let (lu, perm) = lu_factor(&m)?;
|
||||||
|
assert!((w.iter().product::<f64>() - matrix_determinant(&lu, &perm)).abs() < 1e-8);
|
||||||
|
// reconstruction v * diag(w) * v.t() = m
|
||||||
|
let d = Array2::from_shape_fn((3, 3), |rc| if rc.0 == rc.1 { w[rc.0] } else { 0.0 });
|
||||||
|
let recon = v.clone().dot(&d).dot(&v.t());
|
||||||
|
let err = (recon - m.clone())
|
||||||
|
.into_iter()
|
||||||
|
.fold(0.0_f64, |e, x| e.max(x.abs()));
|
||||||
|
assert!(err < 1e-8);
|
||||||
|
// eigenvectors are orthonormal
|
||||||
|
let gv = v.t().dot(&v);
|
||||||
|
let gerr = (gv - Array2::<f64>::eye(3))
|
||||||
|
.into_iter()
|
||||||
|
.fold(0.0_f64, |e, x| e.max(x.abs()));
|
||||||
|
assert!(gerr < 1e-9);
|
||||||
|
// degenerate input
|
||||||
|
let z = Array2::zeros((2, 2));
|
||||||
|
let (wz, _) = symmetric_eigen(&z);
|
||||||
|
assert!(wz.iter().all(|&x| x == 0.0));
|
||||||
|
Ok(())
|
||||||
|
}
|
||||||
|
}
|
||||||
Reference in new issue
Block a user