- nd gaussian function
This commit is contained in:
1 parent
09cecc21f9
commit
7a0b09c4c1
11 files changed
+250
-5
No files matched your search
+186
@@ -0,0 +1,186 @@
|
||||
use crate::error::Error;
|
||||
use itertools::Itertools;
|
||||
use ndarray::{Array, Array2, ArrayView1, AsArray, Axis, Dimension, Ix1, Ix2};
|
||||
|
||||
const STP: f64 = 0.5 * std::f64::consts::FRAC_2_SQRT_PI * std::f64::consts::FRAC_1_SQRT_2;
|
||||
|
||||
/// p: I, x0, x1, ... xn, s00, s01, ..., s0n, s10, s11, ..., s1n, ..., sn0, sn1, ..., snn, o, o0, o1, ..., on
|
||||
/// x: n 1d arrays, defining coordinates in each dimension
|
||||
pub fn gaussian<'a, A, D>(x: Vec<A>, p: &[f64]) -> Result<Array<f64, D>, Error>
|
||||
where
|
||||
A: AsArray<'a, f64, Ix1>,
|
||||
D: Dimension,
|
||||
{
|
||||
let n = x.len();
|
||||
let n2 = n * n;
|
||||
debug_assert_eq!(
|
||||
p.len(),
|
||||
2 + 2 * n + n2,
|
||||
"number of parameters must be {}",
|
||||
2 + 2 * n + n2
|
||||
);
|
||||
let m = ArrayView1::from(&p[1..1 + n]);
|
||||
let s = &p[1 + n..1 + n + n2];
|
||||
let o = p[1 + n + n2];
|
||||
let q = ArrayView1::from(&p[2 + n + n2..]);
|
||||
let c = Array2::from_shape_vec((n, n), s.to_vec())?.powi(2);
|
||||
let (lu, perm) = lu_factor(&c)?;
|
||||
let c_inv = matrix_inverse(&lu, &perm);
|
||||
let c_det = matrix_determinant(&lu, &perm);
|
||||
let a = p[0] * STP.powi(n as i32) / c_det.sqrt();
|
||||
let x = x
|
||||
.into_iter()
|
||||
.map(|i| i.into())
|
||||
.collect::<Vec<ArrayView1<f64>>>();
|
||||
let shape = x.iter().map(|i| i.len()).collect::<Vec<_>>();
|
||||
Ok(Array::from_shape_vec(
|
||||
shape,
|
||||
x.into_iter()
|
||||
.zip(m)
|
||||
.map(|(xi, &mi)| (&xi - mi).to_vec())
|
||||
.multi_cartesian_product()
|
||||
.map(|i| {
|
||||
let xj = ArrayView1::from(&i).insert_axis(Axis(0));
|
||||
a * (-0.5 * xj.dot(&c_inv).dot(&xj.t())).sum().exp() + o + q.dot(&xj.t()).sum()
|
||||
})
|
||||
.collect(),
|
||||
)?
|
||||
.into_dimensionality()?)
|
||||
}
|
||||
|
||||
/// 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 `None`
|
||||
/// 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::MatrixNotInvertible);
|
||||
}
|
||||
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.
|
||||
///
|
||||
/// Returns `None` if the matrix is singular, or numerically rank-deficient.
|
||||
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.
|
||||
///
|
||||
/// Returns `None` if the matrix is singular, or numerically rank-deficient.
|
||||
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)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[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(())
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user