levenberg_marquardt_sparse/
utils.rs

1#![allow(unexpected_cfgs)]
2use crate::LeastSquaresProblem;
3use alloc::{format, string::String};
4use core::cell::RefCell;
5use nalgebra::{
6    Complex, ComplexField, DefaultAllocator, Dim, OMatrix, RealField, U1, Vector,
7    allocator::Allocator, convert, storage::RawStorage, storage::Storage,
8};
9use num_traits::float::Float;
10
11// mod derivest;
12mod finite_difference;
13
14#[cfg(feature = "RUSTC_IS_NIGHTLY")]
15pub use core::intrinsics::{likely, unlikely};
16
17#[cfg(not(feature = "RUSTC_IS_NIGHTLY"))]
18#[inline]
19pub fn likely(b: bool) -> bool {
20    b
21}
22
23#[cfg(not(feature = "RUSTC_IS_NIGHTLY"))]
24#[inline]
25pub fn unlikely(b: bool) -> bool {
26    b
27}
28
29/// Compute a numerical approximation of the Jacobian.
30///
31/// The residuals function is called approximately `$30\cdot nm$` times which
32/// can make this slow in debug builds and for larger problems.
33///
34/// The function is intended to be used for debugging or testing.
35/// You can try to check your derivative implementation of an
36/// [`LeastSquaresProblem`](trait.LeastSquaresProblem.html) with this.
37///
38/// Computing the derivatives numerically is numerically unstable: You can construct
39/// functions where the computed result is far off. If you
40/// observe large differences between the derivative computed by this function
41/// and your implementation the reason _might_ be due to instability.
42///
43/// The achieved precision by this function
44/// is lower than the floating point precision in general. So the error is bigger
45/// than `$10^{-15}$` for `f64` and bigger than `$10^{-7}$` for `f32`. See the example
46/// below for what that means in your tests. If possible use `f64` for the testing.
47///
48/// A much more precise alternative is provided by
49/// [`differentiate_holomorphic_numerically`](fn.differentiate_holomorphic_numerically.html)
50/// but it requires your residuals to be holomorphic and `LeastSquaresProblem` to be implemented
51/// for complex numbers.
52///
53/// # Example
54///
55/// You can use this function to check your derivative implementation in a unit test.
56/// For example:
57///
58/// ```rust
59/// # use levenberg_marquardt_sparse::{LeastSquaresProblem, SparseJacobian, differentiate_numerically};
60/// # use approx::assert_relative_eq;
61/// # use nalgebra::{convert, ComplexField, storage::Owned, Matrix2, Vector2, OVector, U2};
62/// #
63/// # struct ExampleProblem<F: ComplexField> {
64/// #     p: Vector2<F>,
65/// # }
66/// #
67/// # impl<F: ComplexField + Copy> LeastSquaresProblem<F, U2, U2> for ExampleProblem<F> {
68/// #     type ParameterStorage = Owned<F, U2>;
69/// #     type ResidualStorage = Owned<F, U2>;
70/// #
71/// #     fn set_params(&mut self, p: &OVector<F, U2>) {
72/// #         self.p.copy_from(p);
73/// #     }
74/// #
75/// #     fn params(&self) -> OVector<F, U2> { self.p }
76/// #
77/// #     fn residuals(&self) -> Option<Vector2<F>> {
78/// #         Some(Vector2::new(
79/// #             self.p.x * self.p.x + self.p.y - convert(11.0),
80/// #             self.p.x + self.p.y * self.p.y - convert(7.0),
81/// #         ))
82/// #     }
83/// #
84/// #     fn jacobian(&self) -> Option<SparseJacobian<F>> {
85/// #         let two: F = convert(2.);
86/// #         Some(SparseJacobian::from_dense(Matrix2::new(
87/// #             two * self.p.x,
88/// #             F::one(),
89/// #             F::one(),
90/// #             two * self.p.y,
91/// #         )))
92/// #     }
93/// # }
94/// // Let `problem` be an instance of `LeastSquaresProblem`
95/// # let mut problem = ExampleProblem::<f64> { p: Vector2::new(6., -10.), };
96/// let jacobian_numerical = differentiate_numerically(&mut problem).unwrap();
97/// let jacobian_trait = problem.jacobian().unwrap().to_dense::<_, _>();
98/// assert_relative_eq!(jacobian_numerical, jacobian_trait, epsilon = 1e-13);
99/// ```
100///
101/// The `assert_relative_eq!` macro is from the `approx` crate.
102pub fn differentiate_numerically<F, N, M, O>(problem: &mut O) -> Option<OMatrix<F, M, N>>
103where
104    F: RealField + Float + Copy,
105    N: Dim,
106    M: Dim,
107    O: LeastSquaresProblem<F, M, N>,
108    DefaultAllocator: Allocator<M, N>,
109{
110    let params = problem.params();
111    let n = params.data.shape().0;
112    let m = problem.residuals()?.data.shape().0;
113    let params = RefCell::new(params);
114    let problem = RefCell::new(problem);
115    let mut jacobian = OMatrix::<F, M, N>::zeros_generic(m, n);
116    for j in 0..n.value() {
117        let x = params.borrow()[j];
118        for i in 0..m.value() {
119            let f = |x| {
120                params.borrow_mut()[j] = x;
121                let mut problem = problem.borrow_mut();
122                problem.set_params(&params.borrow());
123                problem.residuals().map(|v| v[i])
124            };
125            jacobian[(i, j)] = finite_difference::derivative(x, f)?;
126        }
127        params.borrow_mut()[j] = x;
128    }
129    // reset the initial params
130    problem.borrow_mut().set_params(&params.borrow());
131    Some(jacobian)
132}
133
134/// Compute a numerical approximation of the Jacobian for _holomorphic_ residuals.
135///
136/// This method is _much_ more precise than
137/// [`differentiate_numerically`](fn.differentiate_numerically.html) but
138/// it requires that your residuals are holomorphic on a neighborhood of the real line.
139/// You also must provide an implementation of
140/// [`LeastSquaresProblem`](trait.LeastSquaresProblem.html) for complex numbers.
141///
142/// This method is mainly intended for testing your derivative implementation.
143///
144/// # Panics
145///
146/// The function panics if the parameters which are set when the function is
147/// called are not real.
148///
149/// # Example
150///
151/// ```rust
152/// # use levenberg_marquardt_sparse::{LeastSquaresProblem, SparseJacobian, differentiate_holomorphic_numerically};
153/// # use approx::assert_relative_eq;
154/// # use nalgebra::{storage::Owned, Complex, Matrix2, Vector2, OVector, U2};
155/// use nalgebra::{ComplexField, convert};
156///
157/// struct ExampleProblem<F: ComplexField> {
158///     params: Vector2<F>,
159/// }
160///
161/// // Implement LeastSquaresProblem to be usable with complex numbers
162/// impl<F: ComplexField + Copy> LeastSquaresProblem<F, U2, U2> for ExampleProblem<F> {
163///     // ... omitted ...
164/// #     type ParameterStorage = Owned<F, U2>;
165/// #     type ResidualStorage = Owned<F, U2>;
166/// #
167/// #     fn set_params(&mut self, params: &OVector<F, U2>) {
168/// #         self.params.copy_from(params);
169/// #     }
170/// #
171/// #     fn params(&self) -> OVector<F, U2> { self.params }
172/// #
173/// #     fn residuals(&self) -> Option<Vector2<F>> {
174/// #         Some(Vector2::new(
175/// #             self.params.x * self.params.x + self.params.y - convert(11.0),
176/// #             self.params.x + self.params.y * self.params.y - convert(7.0),
177/// #         ))
178/// #     }
179/// #
180/// #     fn jacobian(&self) -> Option<SparseJacobian<F>> {
181/// #         let two: F = convert(2.);
182/// #         Some(SparseJacobian::from_dense(Matrix2::new(
183/// #             two * self.params.x,
184/// #             F::one(),
185/// #             F::one(),
186/// #             two * self.params.y,
187/// #         )))
188/// #     }
189/// }
190///
191/// // parameters for which you want to test your derivative
192/// let x = Vector2::new(0.03877264483558185, -0.7734472300384164);
193///
194/// // instantiate f64 variant to compute the derivative we want to check
195/// let jacobian_from_trait = (ExampleProblem::<f64> { params: x })
196///     .jacobian()
197///     .unwrap()
198///     .to_dense::<_, _>();
199///
200/// // then use Complex<f64> and compute the numerical derivative
201/// let jacobian_numerically = {
202///     let mut problem = ExampleProblem::<Complex<f64>> {
203///         params: convert(x),
204///     };
205///     differentiate_holomorphic_numerically(&mut problem).unwrap()
206/// };
207///
208/// assert_relative_eq!(jacobian_from_trait, jacobian_numerically, epsilon = 1e-15);
209/// ```
210pub fn differentiate_holomorphic_numerically<F, N, M, O>(
211    problem: &mut O,
212) -> Option<OMatrix<F, M, N>>
213where
214    F: RealField + Copy,
215    N: Dim,
216    M: Dim,
217    O: LeastSquaresProblem<Complex<F>, M, N>,
218    DefaultAllocator:
219        Allocator<N, Buffer<Complex<F>> = O::ParameterStorage> + Allocator<N> + Allocator<M, N>,
220{
221    let mut params = problem.params();
222    assert!(params.iter().all(|x| x.im.is_zero()), "params must be real");
223    let n = params.data.shape().0;
224    let m = problem.residuals()?.data.shape().0;
225    let mut jacobian = OMatrix::<F, M, N>::zeros_generic(m, n);
226    for i in 0..n.value() {
227        let xi = params[i];
228        let h = Complex::<F>::from_real(F::default_epsilon()) * xi.abs();
229        params[i] = xi + Complex::<F>::i() * h;
230        problem.set_params(&params);
231        let mut residuals = problem.residuals()?;
232        residuals /= h;
233        for (dst, src) in jacobian.column_mut(i).iter_mut().zip(residuals.iter()) {
234            *dst = src.imaginary();
235        }
236        params[i] = xi;
237    }
238    problem.set_params(&params);
239    Some(jacobian)
240}
241
242#[inline]
243pub(crate) fn giant<F: Float>() -> F {
244    F::max_value()
245}
246
247#[inline]
248pub(crate) fn dwarf<F: Float>() -> F {
249    F::min_positive_value()
250}
251
252#[inline]
253pub(crate) fn enorm<F, N, VS>(v: &Vector<F, N, VS>) -> F
254where
255    F: nalgebra::RealField + Float + Copy,
256    N: Dim,
257    VS: Storage<F, N, U1>,
258{
259    let mut s1 = F::zero();
260    let mut s2 = F::zero();
261    let mut s3 = F::zero();
262    let mut x1max = F::zero();
263    let mut x3max = F::zero();
264    let agiant = Float::sqrt(giant::<F>()) / convert(v.nrows() as f64);
265    let rdwarf = Float::sqrt(dwarf());
266    for xi in v.iter() {
267        let xabs = xi.abs();
268        if unlikely(xabs.is_nan()) {
269            return xabs;
270        }
271        if unlikely(xabs >= agiant || xabs <= rdwarf) {
272            if xabs > rdwarf {
273                // sum for large components
274                if xabs > x1max {
275                    s1 = F::one() + s1 * Float::powi(x1max / xabs, 2);
276                    x1max = xabs;
277                } else {
278                    s1 += Float::powi(xabs / x1max, 2);
279                }
280            } else {
281                // sum for small components
282                if xabs > x3max {
283                    s3 = F::one() + s3 * Float::powi(x3max / xabs, 2);
284                    x3max = xabs;
285                } else if xabs != F::zero() {
286                    s3 += Float::powi(xabs / x3max, 2);
287                }
288            }
289        } else {
290            s2 += xabs * xabs;
291        }
292    }
293
294    if unlikely(!s1.is_zero()) {
295        x1max * Float::sqrt(s1 + (s2 / x1max) / x1max)
296    } else if likely(!s2.is_zero()) {
297        Float::sqrt(if likely(s2 >= x3max) {
298            s2 * (F::one() + (x3max / s2) * (x3max * s3))
299        } else {
300            x3max * ((s2 / x3max) + (x3max * s3))
301        })
302    } else {
303        x3max * Float::sqrt(s3)
304    }
305}
306
307#[allow(dead_code)]
308/// Debug helper to inspect the binary representation of  a `f64` or `f32`.
309pub(crate) fn float_repr<F: Float>(f: F) -> alloc::string::String {
310    assert!(F::one() / (F::one() + F::one()) != F::zero());
311    let bytes = core::mem::size_of::<F>();
312    let mut out;
313    if bytes == 8 {
314        out = String::with_capacity((8 * 2 + 8 - 1) + 27 + 3);
315        let f = unsafe { *(&f as *const F as *const f64) };
316        let as_int: u64 = f.to_bits();
317        for i in (0..bytes).rev() {
318            out += &format!(
319                "{:02x}{}",
320                as_int >> (8 * i) & 0xFF,
321                if i == 0 { "" } else { ":" }
322            );
323        }
324        out += &format!(" ({f:+.20E})");
325    } else if bytes == 4 {
326        out = String::with_capacity((4 * 2 + 4 - 1) + 17 + 3);
327        let f = unsafe { *(&f as *const F as *const f32) };
328        let as_int: u32 = f.to_bits();
329        for i in (0..bytes).rev() {
330            out += &format!(
331                "{:02x}{}",
332                as_int >> (8 * i) & 0xFF,
333                if i == 0 { "" } else { ":" }
334            );
335        }
336        out += &format!(" ({f:.10E})");
337    } else {
338        unimplemented!()
339    }
340    out
341}
342
343#[test]
344fn test_linear_case() {
345    use crate::lm::test_examples::LinearFullRank;
346    use approx::assert_relative_eq;
347    use nalgebra::{OVector, U5};
348    let mut x = OVector::<f64, U5>::from_element(1.);
349    x[2] = -10.;
350    let mut problem = LinearFullRank { params: x, m: 6 };
351    let jac_num = differentiate_numerically(&mut problem).unwrap();
352    let jac_trait = problem.jacobian().unwrap().to_dense::<nalgebra::Dyn, U5>();
353    assert_relative_eq!(jac_num, jac_trait, epsilon = 1e-12);
354}
355
356#[test]
357fn test_reset_parameters() {
358    use approx::assert_relative_eq;
359    use nalgebra::{Matrix2, OVector, U2, Vector2, storage::Owned};
360    #[derive(Clone)]
361    struct AllButOne {
362        params: OVector<f64, U2>,
363    }
364    impl LeastSquaresProblem<f64, U2, U2> for AllButOne {
365        type ParameterStorage = Owned<f64, U2>;
366        type ResidualStorage = Owned<f64, U2>;
367
368        fn set_params(&mut self, params: &OVector<f64, U2>) {
369            self.params.copy_from(params);
370        }
371
372        fn params(&self) -> OVector<f64, U2> {
373            self.params
374        }
375
376        fn residuals(&self) -> Option<OVector<f64, U2>> {
377            Some(Vector2::new(0.0, -100. * self.params[1].powi(2)))
378        }
379
380        #[rustfmt::skip]
381        fn jacobian(&self) -> Option<crate::SparseJacobian<f64>> {
382            Some(crate::SparseJacobian::from_dense(Matrix2::new(
383                0.,0.,
384                0.,-200. * self.params[1],
385            )))
386        }
387    }
388    let mut problem = AllButOne {
389        params: Vector2::<f64>::new(0., 1. / 3.),
390    };
391    let jac_num = differentiate_numerically(&mut problem).unwrap();
392    let jac_trait = problem.jacobian().unwrap().to_dense::<U2, U2>();
393    assert_relative_eq!(jac_num, jac_trait, epsilon = 1e-12);
394}
levenberg_marquardt_sparse/utils.rs

levenberg_marquardt_sparse/
utils.rs
