use dyn_stack::{MemBuffer, MemStack};
use faer::linalg::cholesky::llt;
use faer::linalg::cholesky::llt::factor::{LltError, LltRegularization};
use faer::{Mat, MatMut, MatRef};
use crate::error::{CholeskyStage, GprError};
use crate::kernel::KernelScalar;
use super::dense::add_to_diag;
use super::par::{faer_par, faer_par_dims};
pub(crate) fn retry_with_jitter(
retries: impl IntoIterator<Item = f64>,
matrix_size: usize,
stage: CholeskyStage,
mut attempt: impl FnMut(f64) -> Result<(), GprError>,
) -> Result<f64, GprError> {
match attempt(0.0) {
Err(GprError::CholeskyFailed { .. }) => {}
other => return other.map(|()| 0.0),
}
let mut last_j = 0.0;
for j in retries {
last_j = j;
match attempt(j) {
Err(GprError::CholeskyFailed { .. }) => {}
other => return other.map(|()| j),
}
}
Err(GprError::CholeskyFailed {
jitter: last_j,
matrix_size,
stage,
})
}
pub(crate) fn cholesky_lower_with_retries<T: KernelScalar>(
a: &mut Mat<T>,
scratch: &mut MemBuffer,
retries: impl IntoIterator<Item = f64>,
stage: CholeskyStage,
) -> Result<(), GprError> {
cholesky_lower_jitter(a, scratch, retries, stage).map(|_| ())
}
pub(crate) fn cholesky_lower_jitter<T: KernelScalar>(
a: &mut Mat<T>,
scratch: &mut MemBuffer,
retries: impl IntoIterator<Item = f64>,
stage: CholeskyStage,
) -> Result<f64, GprError> {
factor_from_backup(a, &mut Mat::new(), scratch, retries, stage)
}
pub(crate) fn cholesky_lower_with_backup<T: KernelScalar>(
a: &mut Mat<T>,
backup: &mut Mat<T>,
scratch: &mut MemBuffer,
retries: impl IntoIterator<Item = f64>,
stage: CholeskyStage,
) -> Result<(), GprError> {
factor_from_backup(a, backup, scratch, retries, stage).map(|_| ())
}
fn factor_from_backup<T: KernelScalar>(
a: &mut Mat<T>,
backup: &mut Mat<T>,
scratch: &mut MemBuffer,
retries: impl IntoIterator<Item = f64>,
stage: CholeskyStage,
) -> Result<f64, GprError> {
if backup.nrows() == a.nrows() && backup.ncols() == a.ncols() {
backup.copy_from(&*a);
} else {
*backup = a.clone();
}
let n = a.nrows();
retry_with_jitter(retries, n, stage, |j| {
if j != 0.0 {
a.copy_from(&*backup);
add_to_diag(a.as_mut(), j);
}
cholesky_lower(a, scratch, 0.0, stage)
})
}
pub(crate) fn cholesky_lower_owned<T: KernelScalar>(
a: &mut Mat<T>,
retries: impl IntoIterator<Item = f64>,
stage: CholeskyStage,
) -> Result<(), GprError> {
let mut scratch = llt_scratch::<T>(a.nrows());
cholesky_lower_with_retries(a, &mut scratch, retries, stage)
}
pub(crate) fn llt_scratch<T: KernelScalar>(n: usize) -> MemBuffer {
MemBuffer::new(llt::factor::cholesky_in_place_scratch::<T>(
n,
faer_par(n),
Default::default(),
))
}
pub(crate) fn cholesky_lower_faer<T: KernelScalar>(
a: &mut Mat<T>,
scratch: &mut MemBuffer,
jitter: f64,
stage: CholeskyStage,
) -> Result<(), GprError> {
let n = a.nrows();
let regularization = LltRegularization {
dynamic_regularization_delta: T::from_f64(jitter),
dynamic_regularization_epsilon: T::from_f64(0.0),
};
let stack = MemStack::new(scratch);
match llt::factor::cholesky_in_place(
a.as_mut(),
regularization,
faer_par(n),
stack,
Default::default(),
) {
Ok(_) => Ok(()),
Err(LltError::NonPositivePivot { .. }) => Err(GprError::CholeskyFailed {
jitter,
matrix_size: n,
stage,
}),
}
}
pub(crate) fn solve_llt_faer_owned<T: KernelScalar>(l: MatRef<'_, T>, rhs: MatMut<'_, T>) {
let n = l.nrows();
let n_rhs = rhs.ncols();
let par = faer_par_dims(n, n_rhs);
let req = llt::solve::solve_in_place_scratch::<T>(n, n_rhs, par);
let mut buf = MemBuffer::new(req);
llt::solve::solve_in_place(l, rhs, par, MemStack::new(&mut buf));
}
pub(crate) fn log_det_from_l<T: KernelScalar>(l: MatRef<'_, T>, n: usize) -> T {
let mut log_diag = T::from_f64(0.0);
for i in 0..n {
log_diag += KernelScalar::ln(l[(i, i)]);
}
T::from_f64(2.0) * log_diag
}
pub(crate) fn inv_diag_from_chol_l<T: KernelScalar>(l: MatRef<'_, T>, q_diag: &mut [T]) {
T::inv_diag_from_chol_l(l, q_diag);
}
#[allow(clippy::needless_range_loop)]
pub(crate) fn inv_diag_from_chol_l_f64_accum(l: MatRef<'_, f32>, q_diag: &mut [f32]) {
let n = l.nrows();
debug_assert_eq!(q_diag.len(), n);
let mut col = vec![0.0f64; n];
for i in 0..n {
for (row, slot) in col.iter_mut().enumerate() {
*slot = if row == i { 1.0 } else { 0.0 };
}
for row in 0..n {
let mut sum = col[row];
for k in 0..row {
sum -= f64::from(l[(row, k)]) * col[k];
}
col[row] = sum / f64::from(l[(row, row)]);
}
let mut q = 0.0f64;
for v in &col {
q += v * v;
}
q_diag[i] = q as f32;
}
}
pub(crate) fn inv_diag_from_chol_l_faer(l: MatRef<'_, f64>, q_diag: &mut [f64]) {
let n = l.nrows();
debug_assert_eq!(q_diag.len(), n);
let mut inv_l = Mat::from_fn(n, n, |row, col| if row == col { 1.0 } else { 0.0 });
faer::linalg::triangular_solve::solve_lower_triangular_in_place(l, inv_l.as_mut(), faer_par(n));
for (i, qi) in q_diag.iter_mut().enumerate() {
let mut q = 0.0;
for k in 0..n {
let v = inv_l[(k, i)];
q += v * v;
}
*qi = q;
}
}
pub(crate) fn cholesky_lower<T: KernelScalar>(
a: &mut Mat<T>,
scratch: &mut MemBuffer,
jitter: f64,
stage: CholeskyStage,
) -> Result<(), GprError> {
T::cholesky_lower(a, scratch, jitter, stage)
}
const F64_ACCUM_ROWS: usize = 64;
pub(crate) fn cholesky_lower_f64_accum(
a: &mut Mat<f32>,
jitter: f64,
stage: CholeskyStage,
) -> Result<(), GprError> {
let n = a.nrows();
let mut acc = [0.0f64; F64_ACCUM_ROWS];
for j in 0..n {
let mut start = j;
while start < n {
let len = F64_ACCUM_ROWS.min(n - start);
let acc = &mut acc[..len];
for (t, slot) in acc.iter_mut().enumerate() {
*slot = f64::from(a[(start + t, j)]);
}
for k in 0..j {
let ajk = f64::from(a[(j, k)]);
match a.as_ref().col(k).try_as_col_major() {
Some(col) => {
for (slot, &aik) in acc.iter_mut().zip(&col.as_slice()[start..start + len])
{
*slot -= f64::from(aik) * ajk;
}
}
None => {
for (t, slot) in acc.iter_mut().enumerate() {
*slot -= f64::from(a[(start + t, k)]) * ajk;
}
}
}
}
for (t, &sum) in acc.iter().enumerate() {
let i = start + t;
if i == j {
if sum.is_nan() || sum <= 0.0 {
return Err(GprError::CholeskyFailed {
jitter,
matrix_size: n,
stage,
});
}
a[(j, j)] = sum.sqrt() as f32;
} else {
let diag = f64::from(a[(j, j)]);
a[(i, j)] = (sum / diag) as f32;
}
}
start += len;
}
}
Ok(())
}
pub(crate) fn cholesky_and_solve<T: KernelScalar>(
a: &mut Mat<T>,
rhs: &mut Mat<T>,
scratch: &mut MemBuffer,
jitter: f64,
stage: CholeskyStage,
) -> Result<(), GprError> {
cholesky_lower(a, scratch, jitter, stage)?;
solve_llt_in_place(a.as_ref(), rhs.as_mut(), scratch);
Ok(())
}
pub(crate) fn solve_llt_in_place<T: KernelScalar>(
l: MatRef<'_, T>,
rhs: MatMut<'_, T>,
scratch: &mut MemBuffer,
) {
T::solve_llt_in_place(l, rhs, scratch);
}
pub(crate) fn solve_llt_faer(l: MatRef<'_, f64>, rhs: MatMut<'_, f64>, scratch: &mut MemBuffer) {
let n = l.nrows();
let n_rhs = rhs.ncols();
let stack = MemStack::new(scratch);
llt::solve::solve_in_place(l, rhs, faer_par_dims(n, n_rhs), stack);
}
pub(crate) fn solve_llt_f64_accum(l: MatRef<'_, f32>, mut rhs: MatMut<'_, f32>) {
let n = l.nrows();
let n_rhs = rhs.ncols();
let mut y = vec![0.0f64; n];
let mut x = vec![0.0f64; n];
for col in 0..n_rhs {
for i in 0..n {
y[i] = f64::from(rhs[(i, col)]);
}
for j in 0..n {
y[j] /= f64::from(l[(j, j)]);
let yj = y[j];
for i in (j + 1)..n {
y[i] -= f64::from(l[(i, j)]) * yj;
}
}
for i in (0..n).rev() {
let mut sum = y[i];
for j in (i + 1)..n {
sum -= f64::from(l[(j, i)]) * x[j];
}
x[i] = sum / f64::from(l[(i, i)]);
}
for i in 0..n {
rhs[(i, col)] = x[i] as f32;
}
}
}
pub(crate) fn solve_lower_transpose<T: KernelScalar>(l: MatRef<'_, T>, rhs: MatMut<'_, T>) {
let n = l.nrows();
let n_rhs = rhs.ncols();
faer::linalg::triangular_solve::solve_upper_triangular_in_place(
l.transpose(),
rhs,
faer_par_dims(n, n_rhs),
);
}
pub(crate) fn solve_lower<T: KernelScalar>(l: MatRef<'_, T>, rhs: MatMut<'_, T>) {
let n = l.nrows();
let n_rhs = rhs.ncols();
faer::linalg::triangular_solve::solve_lower_triangular_in_place(
l,
rhs,
faer_par_dims(n, n_rhs),
);
}
pub(crate) fn append_chol_border<T: KernelScalar>(l: &Mat<T>, row: &[T], ell: T) -> Mat<T> {
let m = l.nrows();
let mut out = Mat::zeros(m + 1, m + 1);
for j in 0..m {
for i in j..m {
out[(i, j)] = l[(i, j)];
}
out[(m, j)] = row[j];
}
out[(m, m)] = ell;
out
}
pub(crate) fn delete_chol_row<T: KernelScalar>(l: &Mat<T>, idx: usize) -> Mat<T> {
let m = l.nrows();
let trail = m - idx - 1;
let mut work = l.clone();
if trail > 0 {
let mut l22 = Mat::zeros(trail, trail);
let mut v = vec![T::from_f64(0.0); trail];
for j in 0..trail {
for i in j..trail {
l22[(i, j)] = work[(idx + 1 + i, idx + 1 + j)];
}
v[j] = work[(idx + 1 + j, idx)];
}
chol_rank1_update(l22.as_mut(), &mut v);
for j in 0..trail {
for i in j..trail {
work[(idx + 1 + i, idx + 1 + j)] = l22[(i, j)];
}
}
}
let mut out = Mat::zeros(m - 1, m - 1);
let mut jo = 0;
for j in 0..m {
if j == idx {
continue;
}
let mut io = 0;
for i in 0..m {
if i == idx {
continue;
}
if io >= jo {
out[(io, jo)] = work[(i, j)];
}
io += 1;
}
jo += 1;
}
out
}
pub(crate) fn solve_llt<T: KernelScalar>(l: MatRef<'_, T>, rhs: MatMut<'_, T>) {
T::solve_llt_owned_scratch(l, rhs);
}
pub(crate) fn chol_rank1_update<T: KernelScalar>(mut l: MatMut<'_, T>, v: &mut [T]) {
let n = l.nrows();
for k in 0..n {
let lkk = l[(k, k)];
let vk = v[k];
let r = (lkk * lkk + vk * vk).sqrt();
let c = r / lkk;
let s = vk / lkk;
l[(k, k)] = r;
for i in (k + 1)..n {
let li = l[(i, k)];
let vi = v[i];
l[(i, k)] = (li + s * vi) / c;
v[i] = c * vi - s * l[(i, k)];
}
}
}
pub(crate) fn chol_rank1_downdate<T: KernelScalar>(mut l: MatMut<'_, T>, v: &mut [T]) -> bool {
let n = l.nrows();
for k in 0..n {
let lkk = l[(k, k)];
let vk = v[k];
let r2 = lkk * lkk - vk * vk;
let res = {
let r2_f = r2.to_f64();
r2_f <= 0.0 || !r2_f.is_finite()
};
if res {
return false;
}
let r = r2.sqrt();
let c = r / lkk;
let s = vk / lkk;
l[(k, k)] = r;
for i in (k + 1)..n {
let li = l[(i, k)];
let vi = v[i];
l[(i, k)] = (li - s * vi) / c;
v[i] = c * vi - s * l[(i, k)];
}
}
true
}
#[cfg(test)]
mod tests {
use super::cholesky_lower_f64_accum;
use crate::error::{CholeskyStage, GprError};
use faer::Mat;
fn reference(a: &mut Mat<f32>) -> Result<(), ()> {
let n = a.nrows();
for j in 0..n {
for i in j..n {
let mut sum = f64::from(a[(i, j)]);
for k in 0..j {
sum -= f64::from(a[(i, k)]) * f64::from(a[(j, k)]);
}
if i == j {
if sum.is_nan() || sum <= 0.0 {
return Err(());
}
a[(j, j)] = sum.sqrt() as f32;
} else {
a[(i, j)] = (sum / f64::from(a[(j, j)])) as f32;
}
}
}
Ok(())
}
fn gram(n: usize, diag: f32) -> Mat<f32> {
Mat::from_fn(n, n, |i, j| {
let d = (i as f32 - j as f32) * 0.37;
(-0.5 * d * d).exp() + if i == j { diag } else { 0.0 }
})
}
#[test]
fn blocked_factor_matches_the_entry_by_entry_factor_bit_for_bit() {
for n in [1, 2, 5, 63, 64, 65, 129, 200] {
let mut blocked = gram(n, 1e-2);
let mut expect = blocked.clone();
cholesky_lower_f64_accum(&mut blocked, 0.0, CholeskyStage::Fit).expect("spd");
reference(&mut expect).expect("spd");
for j in 0..n {
for i in j..n {
assert_eq!(
blocked[(i, j)].to_bits(),
expect[(i, j)].to_bits(),
"n={n} ({i}, {j})"
);
}
}
}
}
#[test]
fn indefinite_matrix_fails_like_the_entry_by_entry_factor() {
let mut a = gram(70, 0.0);
a[(66, 66)] = -1.0;
assert!(reference(&mut a.clone()).is_err());
assert_eq!(
cholesky_lower_f64_accum(&mut a, 0.5, CholeskyStage::Fit),
Err(GprError::CholeskyFailed {
jitter: 0.5,
matrix_size: 70,
stage: CholeskyStage::Fit,
})
);
}
}