nalgebra/linalg/
cholesky.rs

1#[cfg(feature = "serde-serialize-no-std")]
2use serde::{Deserialize, Serialize};
3
4use num::{One, Zero};
5use simba::scalar::ComplexField;
6use simba::simd::SimdComplexField;
7
8use crate::allocator::Allocator;
9use crate::base::{Const, DefaultAllocator, Matrix, OMatrix, Vector};
10use crate::constraint::{SameNumberOfRows, ShapeConstraint};
11use crate::dimension::{Dim, DimAdd, DimDiff, DimSub, DimSum, U1};
12use crate::storage::{Storage, StorageMut};
13
14/// The Cholesky decomposition of a symmetric-definite-positive matrix.
15#[cfg_attr(feature = "serde-serialize-no-std", derive(Serialize, Deserialize))]
16#[cfg_attr(
17    feature = "serde-serialize-no-std",
18    serde(bound(serialize = "DefaultAllocator: Allocator<D>,
19         OMatrix<T, D, D>: Serialize"))
20)]
21#[cfg_attr(
22    feature = "serde-serialize-no-std",
23    serde(bound(deserialize = "DefaultAllocator: Allocator<D>,
24         OMatrix<T, D, D>: Deserialize<'de>"))
25)]
26#[cfg_attr(feature = "defmt", derive(defmt::Format))]
27#[derive(Clone, Debug)]
28pub struct Cholesky<T: SimdComplexField, D: Dim>
29where
30    DefaultAllocator: Allocator<D, D>,
31{
32    chol: OMatrix<T, D, D>,
33}
34
35impl<T: SimdComplexField, D: Dim> Copy for Cholesky<T, D>
36where
37    DefaultAllocator: Allocator<D, D>,
38    OMatrix<T, D, D>: Copy,
39{
40}
41
42impl<T: SimdComplexField, D: Dim> Cholesky<T, D>
43where
44    DefaultAllocator: Allocator<D, D>,
45{
46    /// Computes the Cholesky decomposition of `matrix` without checking that the matrix is definite-positive.
47    ///
48    /// If the input matrix is not definite-positive, the decomposition may contain trash values (Inf, NaN, etc.)
49    pub fn new_unchecked(mut matrix: OMatrix<T, D, D>) -> Self {
50        assert!(matrix.is_square(), "The input matrix must be square.");
51
52        let n = matrix.nrows();
53
54        for j in 0..n {
55            for k in 0..j {
56                let factor = unsafe { -matrix.get_unchecked((j, k)).clone() };
57
58                let (mut col_j, col_k) = matrix.columns_range_pair_mut(j, k);
59                let mut col_j = col_j.rows_range_mut(j..);
60                let col_k = col_k.rows_range(j..);
61                col_j.axpy(factor.simd_conjugate(), &col_k, T::one());
62            }
63
64            let diag = unsafe { matrix.get_unchecked((j, j)).clone() };
65            let denom = diag.simd_sqrt();
66
67            unsafe {
68                *matrix.get_unchecked_mut((j, j)) = denom.clone();
69            }
70
71            let mut col = matrix.view_range_mut(j + 1.., j);
72            col /= denom;
73        }
74
75        Cholesky { chol: matrix }
76    }
77
78    /// Uses the given matrix as-is without any checks or modifications as the
79    /// Cholesky decomposition.
80    ///
81    /// It is up to the user to ensure all invariants hold.
82    pub const fn pack_dirty(matrix: OMatrix<T, D, D>) -> Self {
83        Cholesky { chol: matrix }
84    }
85
86    /// Retrieves the lower-triangular factor of the Cholesky decomposition with its strictly
87    /// upper-triangular part filled with zeros.
88    pub fn unpack(mut self) -> OMatrix<T, D, D> {
89        self.chol.fill_upper_triangle(T::zero(), 1);
90        self.chol
91    }
92
93    /// Retrieves the lower-triangular factor of the Cholesky decomposition, without zeroing-out
94    /// its strict upper-triangular part.
95    ///
96    /// The values of the strict upper-triangular part are garbage and should be ignored by further
97    /// computations.
98    pub fn unpack_dirty(self) -> OMatrix<T, D, D> {
99        self.chol
100    }
101
102    /// Retrieves the lower-triangular factor of the Cholesky decomposition with its strictly
103    /// uppen-triangular part filled with zeros.
104    #[must_use]
105    pub fn l(&self) -> OMatrix<T, D, D> {
106        self.chol.lower_triangle()
107    }
108
109    /// Retrieves the lower-triangular factor of the Cholesky decomposition, without zeroing-out
110    /// its strict upper-triangular part.
111    ///
112    /// This is an allocation-less version of `self.l()`. The values of the strict upper-triangular
113    /// part are garbage and should be ignored by further computations.
114    #[must_use]
115    pub const fn l_dirty(&self) -> &OMatrix<T, D, D> {
116        &self.chol
117    }
118
119    /// Solves the system `self * x = b` where `self` is the decomposed matrix and `x` the unknown.
120    ///
121    /// The result is stored on `b`.
122    pub fn solve_mut<R2: Dim, C2: Dim, S2>(&self, b: &mut Matrix<T, R2, C2, S2>)
123    where
124        S2: StorageMut<T, R2, C2>,
125        ShapeConstraint: SameNumberOfRows<R2, D>,
126    {
127        self.chol.solve_lower_triangular_unchecked_mut(b);
128        self.chol.ad_solve_lower_triangular_unchecked_mut(b);
129    }
130
131    /// Returns the solution of the system `self * x = b` where `self` is the decomposed matrix and
132    /// `x` the unknown.
133    #[must_use = "Did you mean to use solve_mut()?"]
134    pub fn solve<R2: Dim, C2: Dim, S2>(&self, b: &Matrix<T, R2, C2, S2>) -> OMatrix<T, R2, C2>
135    where
136        S2: Storage<T, R2, C2>,
137        DefaultAllocator: Allocator<R2, C2>,
138        ShapeConstraint: SameNumberOfRows<R2, D>,
139    {
140        let mut res = b.clone_owned();
141        self.solve_mut(&mut res);
142        res
143    }
144
145    /// Computes the inverse of the decomposed matrix.
146    #[must_use]
147    pub fn inverse(&self) -> OMatrix<T, D, D> {
148        let shape = self.chol.shape_generic();
149        let mut res = OMatrix::identity_generic(shape.0, shape.1);
150
151        self.solve_mut(&mut res);
152        res
153    }
154
155    /// Computes the determinant of the decomposed matrix.
156    #[must_use]
157    pub fn determinant(&self) -> T::SimdRealField {
158        let dim = self.chol.nrows();
159        let mut prod_diag = T::one();
160        for i in 0..dim {
161            prod_diag *= unsafe { self.chol.get_unchecked((i, i)).clone() };
162        }
163        prod_diag.simd_modulus_squared()
164    }
165
166    /// Computes the natural logarithm of determinant of the decomposed matrix.
167    ///
168    /// This method is more robust than `.determinant()` to very small or very
169    /// large determinants since it returns the natural logarithm of the
170    /// determinant rather than the determinant itself.
171    #[must_use]
172    pub fn ln_determinant(&self) -> T::SimdRealField {
173        let dim = self.chol.nrows();
174        let mut sum_diag = T::SimdRealField::zero();
175        for i in 0..dim {
176            sum_diag += unsafe {
177                self.chol
178                    .get_unchecked((i, i))
179                    .clone()
180                    .simd_modulus_squared()
181                    .simd_ln()
182            };
183        }
184        sum_diag
185    }
186}
187
188impl<T: ComplexField, D: Dim> Cholesky<T, D>
189where
190    DefaultAllocator: Allocator<D, D>,
191{
192    /// Attempts to compute the Cholesky decomposition of `matrix`.
193    ///
194    /// Returns `None` if the input matrix is not definite-positive. The input matrix is assumed
195    /// to be symmetric and only the lower-triangular part is read.
196    pub fn new(matrix: OMatrix<T, D, D>) -> Option<Self> {
197        Self::new_internal(matrix, None)
198    }
199
200    /// Attempts to approximate the Cholesky decomposition of `matrix` by
201    /// replacing non-positive values on the diagonals during the decomposition
202    /// with the given `substitute`.
203    ///
204    /// [`try_sqrt`](ComplexField::try_sqrt) will be applied to the `substitute`
205    /// when it has to be used.
206    ///
207    /// If your input matrix results only in positive values on the diagonals
208    /// during the decomposition, `substitute` is unused and the result is just
209    /// the same as if you used [`new`](Cholesky::new).
210    ///
211    /// This method allows to compensate for matrices with very small or even
212    /// negative values due to numerical errors but necessarily results in only
213    /// an approximation: it is basically a hack. If you don't specifically need
214    /// Cholesky, it may be better to consider alternatives like the
215    /// [`LU`](crate::linalg::LU) decomposition/factorization.
216    pub fn new_with_substitute(matrix: OMatrix<T, D, D>, substitute: T) -> Option<Self> {
217        Self::new_internal(matrix, Some(substitute))
218    }
219
220    /// Common implementation for `new` and `new_with_substitute`.
221    fn new_internal(mut matrix: OMatrix<T, D, D>, substitute: Option<T>) -> Option<Self> {
222        assert!(matrix.is_square(), "The input matrix must be square.");
223
224        let n = matrix.nrows();
225
226        for j in 0..n {
227            for k in 0..j {
228                let factor = unsafe { -matrix.get_unchecked((j, k)).clone() };
229
230                let (mut col_j, col_k) = matrix.columns_range_pair_mut(j, k);
231                let mut col_j = col_j.rows_range_mut(j..);
232                let col_k = col_k.rows_range(j..);
233
234                col_j.axpy(factor.conjugate(), &col_k, T::one());
235            }
236
237            let sqrt_denom = |v: T| {
238                if v.is_zero() {
239                    return None;
240                }
241                v.try_sqrt()
242            };
243
244            let diag = unsafe { matrix.get_unchecked((j, j)).clone() };
245
246            if let Some(denom) =
247                sqrt_denom(diag).or_else(|| substitute.clone().and_then(sqrt_denom))
248            {
249                unsafe {
250                    *matrix.get_unchecked_mut((j, j)) = denom.clone();
251                }
252
253                let mut col = matrix.view_range_mut(j + 1.., j);
254                col /= denom;
255                continue;
256            }
257
258            // The diagonal element is either zero or its square root could not
259            // be taken (e.g. for negative real numbers).
260            return None;
261        }
262
263        Some(Cholesky { chol: matrix })
264    }
265
266    /// Given the Cholesky decomposition of a matrix `M`, a scalar `sigma` and a vector `v`,
267    /// performs a rank one update such that we end up with the decomposition of `M + sigma * (v * v.adjoint())`.
268    #[inline]
269    pub fn rank_one_update<R2: Dim, S2>(&mut self, x: &Vector<T, R2, S2>, sigma: T::RealField)
270    where
271        S2: Storage<T, R2, U1>,
272        DefaultAllocator: Allocator<R2, U1>,
273        ShapeConstraint: SameNumberOfRows<R2, D>,
274    {
275        Self::xx_rank_one_update(&mut self.chol, &mut x.clone_owned(), sigma)
276    }
277
278    /// Updates the decomposition such that we get the decomposition of a matrix with the given column `col` in the `j`th position.
279    /// Since the matrix is square, an identical row will be added in the `j`th row.
280    pub fn insert_column<R2, S2>(
281        &self,
282        j: usize,
283        col: Vector<T, R2, S2>,
284    ) -> Cholesky<T, DimSum<D, U1>>
285    where
286        D: DimAdd<U1>,
287        R2: Dim,
288        S2: Storage<T, R2, U1>,
289        DefaultAllocator: Allocator<DimSum<D, U1>, DimSum<D, U1>> + Allocator<R2>,
290        ShapeConstraint: SameNumberOfRows<R2, DimSum<D, U1>>,
291    {
292        let mut col = col.into_owned();
293        // for an explanation of the formulas, see https://en.wikipedia.org/wiki/Cholesky_decomposition#Updating_the_decomposition
294        let n = col.nrows();
295        assert_eq!(
296            n,
297            self.chol.nrows() + 1,
298            "The new column must have the size of the factored matrix plus one."
299        );
300        assert!(j < n, "j needs to be within the bound of the new matrix.");
301
302        // loads the data into a new matrix with an additional jth row/column
303        // TODO: would it be worth it to avoid the zero-initialization?
304        let mut chol = Matrix::zeros_generic(
305            self.chol.shape_generic().0.add(Const::<1>),
306            self.chol.shape_generic().1.add(Const::<1>),
307        );
308        chol.view_range_mut(..j, ..j)
309            .copy_from(&self.chol.view_range(..j, ..j));
310        chol.view_range_mut(..j, j + 1..)
311            .copy_from(&self.chol.view_range(..j, j..));
312        chol.view_range_mut(j + 1.., ..j)
313            .copy_from(&self.chol.view_range(j.., ..j));
314        chol.view_range_mut(j + 1.., j + 1..)
315            .copy_from(&self.chol.view_range(j.., j..));
316
317        // update the jth row
318        let top_left_corner = self.chol.view_range(..j, ..j);
319
320        let col_j = col[j].clone();
321        let (mut new_rowj_adjoint, mut new_colj) = col.rows_range_pair_mut(..j, j + 1..);
322        assert!(
323            top_left_corner.solve_lower_triangular_mut(&mut new_rowj_adjoint),
324            "Cholesky::insert_column : Unable to solve lower triangular system!"
325        );
326
327        new_rowj_adjoint.adjoint_to(&mut chol.view_range_mut(j, ..j));
328
329        // update the center element
330        let center_element = T::sqrt(col_j - T::from_real(new_rowj_adjoint.norm_squared()));
331        chol[(j, j)] = center_element.clone();
332
333        // update the jth column
334        let bottom_left_corner = self.chol.view_range(j.., ..j);
335        // new_colj = (col_jplus - bottom_left_corner * new_rowj.adjoint()) / center_element;
336        new_colj.gemm(
337            -T::one() / center_element.clone(),
338            &bottom_left_corner,
339            &new_rowj_adjoint,
340            T::one() / center_element,
341        );
342        chol.view_range_mut(j + 1.., j).copy_from(&new_colj);
343
344        // update the bottom right corner
345        let mut bottom_right_corner = chol.view_range_mut(j + 1.., j + 1..);
346        Self::xx_rank_one_update(
347            &mut bottom_right_corner,
348            &mut new_colj,
349            -T::RealField::one(),
350        );
351
352        Cholesky { chol }
353    }
354
355    /// Updates the decomposition such that we get the decomposition of the factored matrix with its `j`th column removed.
356    /// Since the matrix is square, the `j`th row will also be removed.
357    #[must_use]
358    pub fn remove_column(&self, j: usize) -> Cholesky<T, DimDiff<D, U1>>
359    where
360        D: DimSub<U1>,
361        DefaultAllocator: Allocator<DimDiff<D, U1>, DimDiff<D, U1>> + Allocator<D>,
362    {
363        let n = self.chol.nrows();
364        assert!(n > 0, "The matrix needs at least one column.");
365        assert!(j < n, "j needs to be within the bound of the matrix.");
366
367        // loads the data into a new matrix except for the jth row/column
368        // TODO: would it be worth it to avoid this zero initialization?
369        let mut chol = Matrix::zeros_generic(
370            self.chol.shape_generic().0.sub(Const::<1>),
371            self.chol.shape_generic().1.sub(Const::<1>),
372        );
373        chol.view_range_mut(..j, ..j)
374            .copy_from(&self.chol.view_range(..j, ..j));
375        chol.view_range_mut(..j, j..)
376            .copy_from(&self.chol.view_range(..j, j + 1..));
377        chol.view_range_mut(j.., ..j)
378            .copy_from(&self.chol.view_range(j + 1.., ..j));
379        chol.view_range_mut(j.., j..)
380            .copy_from(&self.chol.view_range(j + 1.., j + 1..));
381
382        // updates the bottom right corner
383        let mut bottom_right_corner = chol.view_range_mut(j.., j..);
384        let mut workspace = self.chol.column(j).clone_owned();
385        let mut old_colj = workspace.rows_range_mut(j + 1..);
386        Self::xx_rank_one_update(&mut bottom_right_corner, &mut old_colj, T::RealField::one());
387
388        Cholesky { chol }
389    }
390
391    /// Given the Cholesky decomposition of a matrix `M`, a scalar `sigma` and a vector `x`,
392    /// performs a rank one update such that we end up with the decomposition of `M + sigma * (x * x.adjoint())`.
393    ///
394    /// This helper method is called by `rank_one_update` but also `insert_column` and `remove_column`
395    /// where it is used on a square view of the decomposition
396    fn xx_rank_one_update<Dm, Sm, Rx, Sx>(
397        chol: &mut Matrix<T, Dm, Dm, Sm>,
398        x: &mut Vector<T, Rx, Sx>,
399        sigma: T::RealField,
400    ) where
401        //T: ComplexField,
402        Dm: Dim,
403        Rx: Dim,
404        Sm: StorageMut<T, Dm, Dm>,
405        Sx: StorageMut<T, Rx, U1>,
406    {
407        // heavily inspired by Eigen's `llt_rank_update_lower` implementation https://eigen.tuxfamily.org/dox/LLT_8h_source.html
408        let n = x.nrows();
409        assert_eq!(
410            n,
411            chol.nrows(),
412            "The input vector must be of the same size as the factorized matrix."
413        );
414
415        let mut beta = crate::one::<T::RealField>();
416
417        for j in 0..n {
418            // updates the diagonal
419            let diag = T::real(unsafe { chol.get_unchecked((j, j)).clone() });
420            let diag2 = diag.clone() * diag.clone();
421            let xj = unsafe { x.get_unchecked(j).clone() };
422            let sigma_xj2 = sigma.clone() * T::modulus_squared(xj.clone());
423            let gamma = diag2.clone() * beta.clone() + sigma_xj2.clone();
424            let new_diag = (diag2.clone() + sigma_xj2.clone() / beta.clone()).sqrt();
425            unsafe { *chol.get_unchecked_mut((j, j)) = T::from_real(new_diag.clone()) };
426            beta += sigma_xj2 / diag2;
427            // updates the terms of L
428            let mut xjplus = x.rows_range_mut(j + 1..);
429            let mut col_j = chol.view_range_mut(j + 1.., j);
430            // temp_jplus -= (wj / T::from_real(diag)) * col_j;
431            xjplus.axpy(-xj.clone() / T::from_real(diag.clone()), &col_j, T::one());
432            if gamma != crate::zero::<T::RealField>() {
433                // col_j = T::from_real(nljj / diag) * col_j  + (T::from_real(nljj * sigma / gamma) * T::conjugate(wj)) * temp_jplus;
434                col_j.axpy(
435                    T::from_real(new_diag.clone() * sigma.clone() / gamma) * T::conjugate(xj),
436                    &xjplus,
437                    T::from_real(new_diag / diag),
438                );
439            }
440        }
441    }
442}
nalgebra/linalg/cholesky.rs

nalgebra/linalg/
cholesky.rs
