use super::{CubicSpline, CubicSplineQueries};
use crate::Result;
use crate::na::{DVector, Dyn, Matrix, Owned, Point, U1, Vector};
use levenberg_marquardt::{LeastSquaresProblem, LevenbergMarquardt};
pub type SplineBuildFn<const D: usize> = Box<dyn Fn(&DVector<f64>) -> Result<CubicSpline<D>>>;
const DELTA: f64 = 1e-6;
#[derive(Clone)]
pub struct SplineFitResult {
pub params: DVector<f64>,
pub residuals: DVector<f64>,
}
impl SplineFitResult {
pub fn new(params: DVector<f64>, residuals: DVector<f64>) -> Self {
Self { params, residuals }
}
}
pub fn fit_spline_to_points<const D: usize>(
points: &[Point<f64, D>],
builder: &SplineBuildFn<D>,
initial: DVector<f64>,
) -> Result<SplineFitResult> {
let problem = SplineFit::try_new(points, builder, initial)?;
let (result, report) = LevenbergMarquardt::new().minimize(problem);
if report.termination.was_successful() {
let residuals = result.residuals.unwrap();
Ok(SplineFitResult::new(result.params, residuals))
} else {
Err(format!("Fitting failed: {:?}", report.termination).into())
}
}
struct SplineFit<'a, const D: usize> {
points: &'a [Point<f64, D>],
params: DVector<f64>,
builder: &'a SplineBuildFn<D>,
residuals: Option<DVector<f64>>,
}
impl<'a, const D: usize> SplineFit<'a, D> {
fn try_new(
points: &'a [Point<f64, D>],
builder: &'a SplineBuildFn<D>,
initial: DVector<f64>,
) -> Result<Self> {
let _ = builder(&initial)?;
let mut problem = SplineFit {
points,
params: DVector::zeros(initial.len()),
builder,
residuals: None,
};
problem.set_params(&initial);
Ok(problem)
}
fn distances(&self, queries: &CubicSplineQueries<D>) -> DVector<f64> {
let mut res = DVector::zeros(self.points.len());
for i in 0..self.points.len() {
res[i] = queries.project_point(&self.points[i]).value;
}
res
}
}
impl<const D: usize> LeastSquaresProblem<f64, Dyn, Dyn> for SplineFit<'_, D> {
type ResidualStorage = Owned<f64, Dyn, U1>;
type JacobianStorage = Owned<f64, Dyn, Dyn>;
type ParameterStorage = Owned<f64, Dyn>;
fn set_params(&mut self, x: &Vector<f64, Dyn, Self::ParameterStorage>) {
self.params = x.clone();
self.residuals = (self.builder)(&self.params)
.ok()
.map(|spline| self.distances(&spline.into_query()));
}
fn params(&self) -> Vector<f64, Dyn, Self::ParameterStorage> {
self.params.clone()
}
fn residuals(&self) -> Option<Vector<f64, Dyn, Self::ResidualStorage>> {
self.residuals.clone()
}
fn jacobian(&self) -> Option<Matrix<f64, Dyn, Dyn, Self::JacobianStorage>> {
let residuals = self.residuals.as_ref()?;
let mut jac = Matrix::<f64, Dyn, Dyn, Self::JacobianStorage>::zeros(
residuals.len(),
self.params.len(),
);
for k in 0..self.params.len() {
let mut params = self.params.clone();
params[k] += DELTA;
let Ok(disturbed) = (self.builder)(¶ms) else {
continue;
};
let queries = disturbed.into_query();
for i in 0..residuals.len() {
let d = queries.project_point(&self.points[i]).value;
jac[(i, k)] = (d - residuals[i]) / DELTA;
}
}
Some(jac)
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::Point2;
use approx::assert_relative_eq;
fn truth() -> CubicSpline<2> {
CubicSpline::new(
Point2::new(0.0, 0.0),
Point2::new(1.0, 2.0),
Point2::new(2.0, 2.0),
Point2::new(3.0, 0.0),
)
}
fn inner_builder() -> SplineBuildFn<2> {
Box::new(|p: &DVector<f64>| {
Ok(CubicSpline::new(
Point2::new(0.0, 0.0),
Point2::new(p[0], p[1]),
Point2::new(p[2], p[3]),
Point2::new(3.0, 0.0),
))
})
}
#[test]
fn recover_inner_control_points() {
let curve = truth();
let points: Vec<Point2> = (0..=20).map(|i| curve.position(i as f64 / 20.0)).collect();
let initial = DVector::from(vec![1.0, 0.0, 2.0, 0.0]);
let result = fit_spline_to_points(&points, &inner_builder(), initial).unwrap();
let expected = DVector::from(vec![1.0, 2.0, 2.0, 2.0]);
assert_relative_eq!(result.params, expected, epsilon = 1e-6);
}
#[test]
fn residuals_are_small_after_fit() {
let curve = truth();
let points: Vec<Point2> = (0..=20).map(|i| curve.position(i as f64 / 20.0)).collect();
let initial = DVector::from(vec![1.2, 0.5, 1.8, 0.5]);
let result = fit_spline_to_points(&points, &inner_builder(), initial).unwrap();
for r in result.residuals.iter() {
assert!(r.abs() < 1e-6, "residual {} too large", r);
}
}
#[test]
fn initial_guess_that_fails_is_an_error() {
let points = vec![Point2::new(0.0, 0.0), Point2::new(1.0, 0.0)];
let builder: SplineBuildFn<2> =
Box::new(|_: &DVector<f64>| Err("always fails".to_string().into()));
let initial = DVector::from(vec![0.0, 0.0, 0.0, 0.0]);
assert!(fit_spline_to_points(&points, &builder, initial).is_err());
}
}