use faer::{Mat, MatMut, MatRef};
use crate::error::{CholeskyStage, GprError};
use crate::gpr::GprObjective;
use crate::kernel::ScalarOps;
use crate::kernel::{
CompiledKernel, KernelScalar, ModelKernel, SpecOf, Supply, Triangle, WeightedWalk,
};
use crate::likelihood::GaussianLikelihood;
use crate::linalg::{frobenius_lower, gemv_sym_lower, solve_lower};
use crate::optimizer::{OptResult, Optimizer};
use crate::param::Interval;
use crate::precision::{GpScalar, StoredFactor};
use crate::workspace::{FitWorkspace, HessianScratch, WorkspaceCore};
use super::factor::{
FactorPolicy, apply_compiled_to, factor_train_keeping_with_policy, factor_train_with_policy,
factor_written_k_with_policy, fill_cached_inputs, neg_mll_from_factor,
};
use super::shared::train_points;
use super::{FitBuffers, GprCore, LltStore, Policies};
use crate::policy::{DistanceCachePolicy, with_kernel_exp};
fn uses_change_indices<Obj, O: Optimizer<Obj>>(_obj: &Obj) -> bool {
O::USES_CHANGE_INDICES
}
pub(crate) fn fit_buffers<P: GpScalar, S: Supply>(
n: usize,
policies: Policies,
compiled: &CompiledKernel<P::Storage, S>,
) -> Result<FitBuffers<P>, GprError> {
let cache = if compiled.reads_distances()? || compiled.needs_ard_sq_diff() {
policies.distance_cache
} else {
DistanceCachePolicy::Uncached
};
FitBuffers::<P>::new(n, cache, policies.cholesky_buffer)
}
pub(crate) struct ExactFit<'a, P: GpScalar, K: ModelKernel> {
pub(crate) core: &'a mut GprCore<P, K>,
pub(crate) store: &'a mut LltStore<P>,
}
pub(crate) struct LeafCache<S> {
grams: Vec<Mat<S>>,
dirty: Vec<bool>,
last: Vec<f64>,
primed: bool,
}
impl<S: KernelScalar> LeafCache<S> {
pub(crate) fn new() -> Self {
Self {
grams: Vec::new(),
dirty: Vec::new(),
last: Vec::new(),
primed: false,
}
}
fn fit(&mut self, n_leaves: usize, n: usize) {
if self.grams.len() != n_leaves || self.grams.first().is_some_and(|m| m.nrows() != n) {
self.grams = (0..n_leaves).map(|_| Mat::<S>::zeros(n, n)).collect();
self.primed = false;
}
self.dirty.resize(n_leaves, true);
}
fn require_listed(&self, params: &[f64], changed: &[usize]) -> Result<(), GprError> {
for (j, (prev, next)) in self.last.iter().zip(params).enumerate() {
if prev.to_bits() != next.to_bits() && !changed.contains(&j) {
return Err(GprError::IndexOutOfRange {
reason: format!(
"coordinate {j} changed since the previous evaluation but is not in the change indices"
),
});
}
}
Ok(())
}
fn record(&mut self, params: &[f64]) {
self.last.clear();
self.last.extend_from_slice(params);
self.primed = true;
}
}
impl<P: GpScalar, K: ModelKernel> ExactFit<'_, P, K> {
pub(crate) fn reborrow(&mut self) -> ExactFit<'_, P, K> {
ExactFit {
core: &mut *self.core,
store: &mut *self.store,
}
}
pub(crate) fn num_params(&self) -> usize {
self.core.num_params()
}
fn get_params(&self, out: &mut [f64]) -> Result<(), GprError> {
self.core.get_params(out)
}
pub(crate) fn optimize<O>(&mut self, optimizer: &O) -> Result<(), GprError>
where
O: for<'b> Optimizer<GprObjective<'b, P, K>>,
{
let mut init = vec![0.0; self.num_params()];
self.get_params(&mut init)?;
let kernel_before = self.core.kernel.clone();
let likelihood_before = self.core.likelihood;
let result = {
let obj = GprObjective::new(self.reborrow());
let uses_change_indices = uses_change_indices::<_, O>(&obj);
let mut obj = obj.with_change_indices(uses_change_indices);
optimizer.minimize(&mut obj, &init)
};
self.commit_or_revert_optimize(kernel_before, likelihood_before, result)?;
self.finish()
}
pub(crate) fn refactor(&mut self) -> Result<(), GprError> {
self.factorize_current()?;
self.finish()
}
fn finish(&mut self) -> Result<(), GprError> {
self.restore_cholesky_if_overwritten()?;
self.publish_predict_alpha()
}
pub(crate) fn publish_predict_alpha(&mut self) -> Result<(), GprError> {
let jitter = self.store.buffers.core().factor_jitter;
self.core.publish_predict_alpha(
StoredFactor::Llt(self.store.l()),
jitter,
CholeskyStage::Fit,
)
}
pub(crate) fn set_params(&mut self, params: &[f64]) -> Result<(), GprError> {
crate::data::require_count(params.len(), self.num_params(), "parameters")?;
self.factor_at(params, Keep::Nothing)?;
if let Err(err) = self.finish() {
self.restore_theta();
let _ = self.factorize_current();
let _ = self.publish_predict_alpha();
return Err(err);
}
Ok(())
}
pub(crate) fn value_and_gradient_into(
&mut self,
params: &[f64],
out: &mut [f64],
) -> Result<f64, GprError> {
let nlml = self.value_and_gradient_into_fit(params, out)?;
self.finish()?;
Ok(nlml)
}
fn factor_at(&mut self, params: &[f64], keep: Keep) -> Result<KeptGrams, GprError> {
self.write_theta(params)?;
match self.factor_with(keep) {
Ok(kept) => Ok(kept),
Err(err) => {
self.restore_theta();
let _ = self.factorize_current();
Err(err)
}
}
}
fn write_theta(&mut self, params: &[f64]) -> Result<(), GprError> {
let n_kernel = self.core.kernel.num_params();
let mut likelihood = self.core.likelihood;
likelihood.set_params(¶ms[n_kernel..])?;
let prev = &mut self.store.buffers.core_mut().theta;
prev.resize(params.len(), 0.0);
self.core.get_params(prev)?;
let (kernel_prev, _) = prev.split_at(n_kernel);
self.core
.kernel
.set_params_in_place(¶ms[..n_kernel], kernel_prev)?;
if let Err(err) = self
.core
.compiled
.set_params_in_place(¶ms[..n_kernel], kernel_prev)
{
let _ = self
.core
.kernel
.set_params_in_place(kernel_prev, kernel_prev);
return Err(err);
}
self.core.likelihood = likelihood;
Ok(())
}
fn restore_theta(&mut self) {
let n_kernel = self.core.kernel.num_params();
let prev = &self.store.buffers.core().theta;
let (kernel_prev, likelihood_prev) = prev.split_at(n_kernel);
let _ = self
.core
.kernel
.set_params_in_place(kernel_prev, kernel_prev);
let _ = self
.core
.compiled
.set_params_in_place(kernel_prev, kernel_prev);
let _ = self.core.likelihood.set_params(likelihood_prev);
}
fn factor(&mut self) -> Result<(), GprError> {
self.factor_with(Keep::Nothing).map(|_| ())
}
fn factor_with(&mut self, keep: Keep) -> Result<KeptGrams, GprError> {
let products = match keep {
Keep::Nothing => 0,
Keep::FactorGrams => self.core.compiled.kept_products(),
};
self.store.release_mapped();
let n = self.core.n;
if products > 0 {
self.ensure_weighted(n, products)?;
}
let x = train_points::<P, _>(
&self.core.x,
(self.core.n, self.core.d),
&mut self.core.x_cast,
&self.core.supplied,
);
let policy = FactorPolicy {
jitter: self.core.policies.jitter,
stage: CholeskyStage::Fit,
};
let compiled = &self.core.compiled;
let noise = self.core.likelihood.noise_variance();
let buffers = &mut self.store.buffers;
let y = &self.core.y_train;
if products == 0 {
with_kernel_exp!(self.core.policies.math, M => factor_train_with_policy::<_, _, M, _>(
compiled, x, buffers, y, noise, policy,
))?;
} else {
with_kernel_exp!(self.core.policies.math, M => factor_train_keeping_with_policy::<_, _, M, _>(
compiled, x, buffers, y, noise, policy, products,
))?;
}
self.commit_factor();
Ok(KeptGrams { products })
}
fn ensure_weighted(&mut self, n: usize, products: usize) -> Result<(), GprError> {
let compiled = &self.core.compiled;
let core = self.store.buffers.core_mut();
if compiled.needs_grad_scratch() {
core.ensure_kernel_scratch(n)?;
}
core.weighted.retain(|m| m.nrows() == n && m.ncols() == n);
while core.weighted.len() < compiled.weighted_buffers(products) {
core.weighted.push(Mat::zeros(n, n));
}
Ok(())
}
pub(crate) fn value_and_gradient_into_fit(
&mut self,
params: &[f64],
out: &mut [f64],
) -> Result<f64, GprError> {
let n_kernel = self.core.kernel.num_params();
let n_params = self.num_params();
crate::data::require_count(params.len(), n_params, "parameters")?;
crate::data::require_count(out.len(), n_params, "parameters")?;
let kept = self.factor_at(params, Keep::FactorGrams)?;
let n = self.core.n;
let mut rows = P::Storage::empty_rows();
let y = P::Storage::storage_rows(&self.core.y_train, &mut rows);
let nlml = neg_mll_from_factor(
self.store.buffers.core().k_matrix.as_ref(),
y,
&self.core.factor_alpha,
n,
)
.to_f64();
self.fill_gradient_from_factor(n_kernel, n, out, kept)?;
Ok(nlml)
}
pub(crate) fn value_from_leaf_grams(
&mut self,
params: &[f64],
indices: Option<&[usize]>,
cache: &mut LeafCache<P::Storage>,
) -> Result<f64, GprError> {
let n_kernel = self.core.kernel.num_params();
let n_params = self.num_params();
crate::data::require_count(params.len(), n_params, "parameters")?;
if let Some(changed) = indices {
require_change_indices(changed, n_params)?;
}
let n = self.core.n;
let n_leaves = self.core.compiled.leaf_count();
cache.fit(n_leaves, n);
match (cache.primed, indices) {
(true, Some(changed)) => {
cache.require_listed(params, changed)?;
cache.dirty.fill(false);
for &j in changed {
if j < n_kernel {
cache.dirty[self.core.compiled.leaf_index_for_param(j)?] = true;
}
}
}
_ => cache.dirty.fill(true),
}
self.write_theta(params)?;
cache.primed = false;
if let Err(err) = self.rebuild_dirty_leaves(cache) {
self.restore_theta();
return Err(err);
}
self.store.release_mapped();
let compiled = &self.core.compiled;
let grams = &cache.grams;
if let Err(err) = factor_written_k_with_policy(
&mut self.store.buffers,
&self.core.y_train,
self.core.likelihood.noise_variance(),
FactorPolicy {
jitter: self.core.policies.jitter,
stage: CholeskyStage::Fit,
},
|ws| {
let core = ws.core_mut();
compiled.combine_from_leaf_grams(
grams,
core.k_matrix.as_mut(),
core.exp_buf.as_mut(),
&mut core.nested,
Triangle::Lower,
)
},
) {
self.restore_theta();
let _ = self.factorize_current();
return Err(err);
}
cache.record(params);
self.commit_factor();
let mut rows = P::Storage::empty_rows();
let y = P::Storage::storage_rows(&self.core.y_train, &mut rows);
Ok(neg_mll_from_factor(
self.store.buffers.core().k_matrix.as_ref(),
y,
&self.core.factor_alpha,
n,
)
.to_f64())
}
fn rebuild_dirty_leaves(&mut self, cache: &mut LeafCache<P::Storage>) -> Result<(), GprError> {
let LeafCache { grams, dirty, .. } = cache;
for (i, slot) in grams.iter_mut().enumerate() {
if dirty[i] {
let x = train_points::<P, _>(
&self.core.x,
(self.core.n, self.core.d),
&mut self.core.x_cast,
&self.core.supplied,
);
with_kernel_exp!(self.core.policies.math, M => apply_compiled_to::<_, _, M, _>(
self.core.compiled.leaf_at(i)?,
x,
&mut self.store.buffers,
slot.as_mut(),
))?;
}
}
Ok(())
}
pub(crate) fn hessian_into(&mut self, params: &[f64], out: &mut [f64]) -> Result<(), GprError> {
self.hessian_into_fit(params, out)?;
self.finish()
}
pub(crate) fn hessian_into_fit(
&mut self,
params: &[f64],
out: &mut [f64],
) -> Result<(), GprError> {
let n_kernel = self.core.kernel.num_params();
let n_params = self.num_params();
crate::data::require_count(params.len(), n_params, "parameters")?;
crate::data::require_count(out.len(), n_params * n_params, "parameters")?;
self.factor_at(params, Keep::Nothing)?;
let n = self.core.n;
self.fill_hessian_from_factor(n_kernel, n, out)
}
pub(crate) fn value_gradient_hessian_into_fit(
&mut self,
params: &[f64],
grad: &mut [f64],
hess: &mut [f64],
) -> Result<f64, GprError> {
let n_params = self.num_params();
crate::data::require_count(hess.len(), n_params * n_params, "parameters")?;
let nlml = self.value_and_gradient_into_fit(params, grad)?;
self.restore_cholesky_if_overwritten()?;
let n_kernel = self.core.kernel.num_params();
let n = self.core.n;
self.fill_hessian_from_factor(n_kernel, n, hess)?;
Ok(nlml)
}
fn fill_hessian_from_factor(
&mut self,
n_kernel: usize,
n: usize,
out: &mut [f64],
) -> Result<(), GprError> {
self.store.buffers.core_mut().ensure_kernel_scratch(n)?;
self.store
.buffers
.form_gradient_w(&self.core.factor_alpha, n);
out.fill(0.0);
let n_params = n_kernel + 1;
let noise = self.core.likelihood.noise_variance();
let thread_scratch = std::mem::take(&mut self.store.buffers.core_mut().thread_scratch);
let second = (|| {
for i in 0..n_params {
for j in i..n_params {
self.write_second_deriv(n_kernel, i, j, n)?;
let inner = frobenius_lower(
self.store.buffers.gradient_w(),
self.store.buffers.core().exp_buf.as_ref(),
n,
);
let hij = -0.5 * inner.to_f64();
out[i * n_params + j] = hij;
out[j * n_params + i] = hij;
}
}
Ok::<(), GprError>(())
})();
self.store.buffers.core_mut().thread_scratch = thread_scratch;
second?;
let mut hess = std::mem::take(&mut self.store.buffers.core_mut().hessian);
hess.ensure(n, n_params);
let first = self.add_first_order(n_kernel, n, noise, out, &mut hess);
self.store.buffers.core_mut().hessian = hess;
first
}
fn add_first_order(
&mut self,
n_kernel: usize,
n: usize,
noise: f64,
out: &mut [f64],
hess: &mut HessianScratch<P::Storage>,
) -> Result<(), GprError> {
if !self.store.buffers.has_dedicated_w() {
self.factorize_current()?;
}
let n_params = n_kernel + 1;
let thread_scratch = std::mem::take(&mut self.store.buffers.core_mut().thread_scratch);
let halves = (|| {
for i in 0..n_params {
if i < n_kernel {
self.write_first_deriv(i)?;
} else {
zero_and_maybe_noise(
self.store.buffers.core_mut().exp_buf.as_mut(),
n,
true,
noise,
);
}
let l = self.store.buffers.core().k_matrix.as_ref();
let d_a = self.store.buffers.core().exp_buf.as_ref();
gemv_sym_lower(d_a, &self.core.factor_alpha, &mut hess.u, n);
let mut v_i = hess.v.as_mut().col_mut(i);
for (row, value) in hess.u.iter().enumerate() {
v_i[row] = *value;
}
solve_lower(l, hess.v.as_mut().subcols_mut(i, 1));
let s_i = &mut hess.s[i];
for col in 0..n {
for row in col..n {
s_i[(row, col)] = d_a[(row, col)];
s_i[(col, row)] = d_a[(row, col)];
}
}
solve_lower(l, s_i.as_mut());
transpose_square_in_place(s_i.as_mut(), n);
solve_lower(l, s_i.as_mut());
}
Ok::<(), GprError>(())
})();
self.store.buffers.core_mut().thread_scratch = thread_scratch;
halves?;
for i in 0..n_params {
for j in 0..=i {
let tr = frobenius_full(hess.s[i].as_ref(), hess.s[j].as_ref(), n);
let mut v_dot = 0.0;
for row in 0..n {
v_dot += hess.v[(row, i)].to_f64() * hess.v[(row, j)].to_f64();
}
let add = -0.5 * tr + v_dot;
out[i * n_params + j] += add;
if i != j {
out[j * n_params + i] += add;
}
}
}
Ok(())
}
fn write_second_deriv(
&mut self,
n_kernel: usize,
i: usize,
j: usize,
n: usize,
) -> Result<(), GprError> {
if i >= n_kernel || j >= n_kernel {
zero_and_maybe_noise(
self.store.buffers.core_mut().exp_buf.as_mut(),
n,
i == n_kernel && j == n_kernel,
self.core.likelihood.noise_variance(),
);
return Ok(());
}
let x = train_points::<P, _>(
&self.core.x,
(self.core.n, self.core.d),
&mut self.core.x_cast,
&self.core.supplied,
);
let (core, dist) = self.store.buffers.split_fit();
let WorkspaceCore {
exp_buf,
kernel_scratch,
nested,
thread_scratch,
..
} = core;
let inputs = fill_cached_inputs(&self.core.compiled, x, dist, thread_scratch)?;
with_kernel_exp!(self.core.policies.math, M => self.core.compiled.hess_gram::<M>(
inputs,
exp_buf.as_mut(),
(i, j),
Triangle::Lower,
kernel_scratch.as_mut(),
nested,
))
}
fn write_first_deriv(&mut self, idx: usize) -> Result<(), GprError> {
let x = train_points::<P, _>(
&self.core.x,
(self.core.n, self.core.d),
&mut self.core.x_cast,
&self.core.supplied,
);
let (core, dist) = self.store.buffers.split_fit();
let WorkspaceCore {
exp_buf,
kernel_scratch,
nested,
thread_scratch,
..
} = core;
let inputs = fill_cached_inputs(&self.core.compiled, x, dist, thread_scratch)?;
with_kernel_exp!(self.core.policies.math, M => self.core.compiled.grad_gram::<M>(
inputs,
exp_buf.as_mut(),
idx,
Triangle::Lower,
kernel_scratch.as_mut(),
nested,
))
}
fn fill_gradient_from_factor(
&mut self,
n_kernel: usize,
n: usize,
out: &mut [f64],
kept: KeptGrams,
) -> Result<(), GprError> {
self.store
.buffers
.form_gradient_w(&self.core.factor_alpha, n);
self.kernel_gradient_into(n, &mut out[..n_kernel], kept)?;
let mut noise_inner = 0.0;
let d_noise = self.core.likelihood.noise_variance();
let w = self.store.buffers.gradient_w();
for i in 0..n {
noise_inner += w[(i, i)].to_f64() * d_noise;
}
out[n_kernel] = -0.5 * noise_inner;
Ok(())
}
fn kernel_gradient_into(
&mut self,
n: usize,
out: &mut [f64],
kept: KeptGrams,
) -> Result<(), GprError> {
if out.is_empty() {
return Ok(());
}
let KeptGrams { products } = kept;
self.ensure_weighted(n, products)?;
let x = train_points::<P, _>(
&self.core.x,
(self.core.n, self.core.d),
&mut self.core.x_cast,
&self.core.supplied,
);
let compiled = &self.core.compiled;
let views = self.store.buffers.split_gradient();
let (kept, bufs) = views.weighted.split_at_mut(compiled.kept_buffers(products));
let inputs = fill_cached_inputs(compiled, x, views.dist, views.thread_scratch)?;
let mut walk = WeightedWalk {
inputs,
scratch: views.kernel_scratch.as_mut(),
nested: views.nested,
kept,
kept_products: products,
fold: views.ard_fold,
};
with_kernel_exp!(self.core.policies.math, M => compiled.weighted_grads::<M>(
&mut walk,
views.w,
out,
bufs,
))?;
for value in out.iter_mut() {
*value *= -0.5;
}
Ok(())
}
pub(crate) fn overwrites_cholesky(&self) -> bool {
self.store.buffers.overwrites_cholesky()
}
pub(crate) fn restore_cholesky_if_overwritten(&mut self) -> Result<(), GprError> {
if self.overwrites_cholesky() {
self.factorize_current()?;
}
Ok(())
}
pub(crate) fn commit_or_revert_optimize(
&mut self,
kernel_before: SpecOf<K>,
likelihood_before: GaussianLikelihood,
result: Result<OptResult, GprError>,
) -> Result<(), GprError> {
match result {
Ok(opt) => {
if opt.params.len() != self.num_params() || !opt.value.is_finite() {
self.revert_theta(kernel_before, likelihood_before);
return Err(GprError::OptimizationNotConverged {
iterations: opt.iterations as usize,
});
}
if !self.holds_params(&opt.params)?
&& let Err(err) = self.factor_at(&opt.params, Keep::Nothing)
{
self.revert_theta(kernel_before, likelihood_before);
return Err(err);
}
Ok(())
}
Err(err) => {
self.revert_theta(kernel_before, likelihood_before);
Err(err)
}
}
}
fn holds_params(&mut self, params: &[f64]) -> Result<bool, GprError> {
let current = &mut self.store.buffers.core_mut().theta;
current.resize(params.len(), 0.0);
self.core.get_params(current)?;
Ok(current
.iter()
.zip(params)
.all(|(a, b)| a.to_bits() == b.to_bits()))
}
fn revert_theta(&mut self, kernel: SpecOf<K>, likelihood: GaussianLikelihood) {
self.core.kernel = kernel;
self.core.likelihood = likelihood;
self.core.compiled = self.core.kernel.compile_as::<P::Storage>();
let _ = self.factorize_current();
}
pub(crate) fn factorize_current(&mut self) -> Result<(), GprError> {
self.factor()
}
fn commit_factor(&mut self) {
let n = self.core.n;
if self.core.factor_alpha.len() != n {
self.core.factor_alpha.resize(n, P::Storage::from_f64(0.0));
}
for (i, slot) in self.core.factor_alpha.iter_mut().enumerate() {
*slot = self.store.buffers.core().rhs[(i, 0)];
}
}
pub(crate) fn fill_intervals(&self, out: &mut [Interval]) -> Result<(), GprError> {
let n = self.num_params();
if out.len() != n {
return Err(GprError::LengthMismatch {
reason: format!("expected {n} intervals, got {}", out.len()),
});
}
let n_kernel = self.core.kernel.num_params();
let mut offset = 0;
self.core
.kernel
.write_intervals(&mut out[..n_kernel], &mut offset)?;
out[n_kernel] = self.core.likelihood.bounds();
Ok(())
}
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
enum Keep {
Nothing,
FactorGrams,
}
struct KeptGrams {
products: usize,
}
fn require_change_indices(indices: &[usize], n_params: usize) -> Result<(), GprError> {
if indices.is_empty() {
return Err(GprError::IndexOutOfRange {
reason: "change indices must not be empty".to_owned(),
});
}
for (k, &i) in indices.iter().enumerate() {
if i >= n_params {
return Err(GprError::IndexOutOfRange {
reason: format!("change index {i} is out of range (n_params={n_params})"),
});
}
if indices[..k].contains(&i) {
return Err(GprError::IndexOutOfRange {
reason: format!("change index {i} is duplicated"),
});
}
}
Ok(())
}
fn zero_and_maybe_noise<T: KernelScalar>(
mut out: MatMut<'_, T>,
n: usize,
noise_diag: bool,
noise: f64,
) {
let noise_s = T::from_f64(noise);
let zero = T::from_f64(0.0);
for col in 0..n {
for row in col..n {
out[(row, col)] = if noise_diag && row == col {
noise_s
} else {
zero
};
}
}
}
fn frobenius_full<T: KernelScalar>(a: MatRef<'_, T>, b: MatRef<'_, T>, n: usize) -> f64 {
let mut sum = 0.0;
for col in 0..n {
for row in 0..n {
sum += a[(row, col)].to_f64() * b[(row, col)].to_f64();
}
}
sum
}
fn transpose_square_in_place<T: KernelScalar>(mut a: MatMut<'_, T>, n: usize) {
for col in 0..n {
for row in col + 1..n {
let upper = a[(col, row)];
a[(col, row)] = a[(row, col)];
a[(row, col)] = upper;
}
}
}