From f8c0195f0f3ec543aa55ac344b71fd96ea3ee1b3 Mon Sep 17 00:00:00 2001
From: russellb23 <bertrussell23@yahoo.in>
Date: Mon, 24 Jun 2019 07:56:35 +0530
Subject: [PATCH 1/7] QR factorization with column pivoting

---
 src/linalg/qrp.rs | 328 ++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 328 insertions(+)
 create mode 100644 src/linalg/qrp.rs

diff --git a/src/linalg/qrp.rs b/src/linalg/qrp.rs
new file mode 100644
index 00000000..d251cc5d
--- /dev/null
+++ b/src/linalg/qrp.rs
@@ -0,0 +1,328 @@
+#[cfg(feature = "serde-serialize")]
+use serde::{Deserialize, Serialize};
+use num::Zero;
+
+use alga::general::ComplexField;
+use crate::allocator::{Allocator, Reallocator};
+use crate::base::{DefaultAllocator, Matrix, MatrixMN, MatrixN, Unit, VectorN};
+use crate::constraint::{SameNumberOfRows, ShapeConstraint};
+use crate::dimension::{Dim, DimMin, DimMinimum, U1};
+use crate::storage::{Storage, StorageMut};
+
+use crate::geometry::Reflection;
+use crate::linalg::householder;
+
+//=============================================================================
+use alga::general::RealField;
+use crate::linalg::PermutationSequence;
+use crate::base::VecStorage;
+use crate::base::Dynamic;
+use crate::base::DVector;
+//=============================================================================
+
+/// The QRP decomposition of a general matrix.
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[cfg_attr(
+    feature = "serde-serialize",
+    serde(bound(
+        serialize = "DefaultAllocator: Allocator<N, R, C> +
+                           Allocator<N, DimMinimum<R, C>>,
+         MatrixMN<N, R, C>: Serialize,
+         PermutationSequence<DimMinimum<R, C>>: Serialize,
+         VectorN<N, DimMinimum<R, C>>: Serialize"
+    ))
+)]
+#[cfg_attr(
+    feature = "serde-serialize",
+    serde(bound(
+        deserialize = "DefaultAllocator: Allocator<N, R, C> +
+                           Allocator<N, DimMinimum<R, C>>,
+         MatrixMN<N, R, C>: Deserialize<'de>,
+         PermutationSequence<DimMinimum<R, C>>: Deserialize<'de>,
+         VectorN<N, DimMinimum<R, C>>: Deserialize<'de>"
+    ))
+)]
+#[derive(Clone, Debug)]
+pub struct QRP<N: ComplexField, R: DimMin<C>, C: Dim>
+where DefaultAllocator: Allocator<N, R, C> + Allocator<N, DimMinimum<R, C>> +
+    Allocator<(usize, usize), DimMinimum<R, C>>,
+{
+    qrp: MatrixMN<N, R, C>,
+    p: PermutationSequence<DimMinimum<R, C>>,
+    diag: VectorN<N, DimMinimum<R, C>>,
+}
+
+impl<N: ComplexField, R: DimMin<C>, C: Dim> Copy for QRP<N, R, C>
+where
+    DefaultAllocator: Allocator<N, R, C> + Allocator<N, DimMinimum<R, C>> +
+    Allocator<(usize, usize), DimMinimum<R, C>>,
+    MatrixMN<N, R, C>: Copy,
+    PermutationSequence<DimMinimum<R, C>>: Copy,
+    VectorN<N, DimMinimum<R, C>>: Copy,
+{}
+
+impl<N: ComplexField, R: DimMin<C>, C: Dim> QRP<N, R, C>
+where DefaultAllocator: Allocator<N, R, C> + Allocator<N, R> + Allocator<N, DimMinimum<R, C>> + Allocator<(usize, usize), DimMinimum<R, C>>
+{
+    /// Computes the QRP decomposition using householder reflections.
+    pub fn new(mut matrix: MatrixMN<N, R, C>) -> Self {
+        let (nrows, ncols) = matrix.data.shape();
+        let min_nrows_ncols = nrows.min(ncols);
+        let mut p = PermutationSequence::identity_generic(min_nrows_ncols);
+        let mut diag = unsafe { MatrixMN::new_uninitialized_generic(min_nrows_ncols, U1) };
+
+        if min_nrows_ncols.value() == 0 {
+            return QRP {
+                qrp: matrix,
+                p: p,
+                diag: diag,
+            };
+        }
+
+        for ite in 0..min_nrows_ncols.value() {
+            let piv = matrix.slice_range(ite.., ite..).icamax_full();
+            let col_piv = piv.1 + ite;
+            matrix.swap_columns(ite, col_piv);
+            p.append_permutation(ite, col_piv);
+
+            householder::clear_column_unchecked(&mut matrix, &mut diag[ite], ite, 0, None);
+        }
+
+        QRP {
+            qrp: matrix,
+            p: p,
+            diag: diag,
+        }
+    }
+
+    /// Retrieves the upper trapezoidal submatrix `R` of this decomposition.
+    #[inline]
+    pub fn r(&self) -> MatrixMN<N, DimMinimum<R, C>, C>
+    where
+        DefaultAllocator: Allocator<N, DimMinimum<R, C>, C>,
+    {
+        let (nrows, ncols) = self.qrp.data.shape();
+        let mut res = self.qrp.rows_generic(0, nrows.min(ncols)).upper_triangle();
+        res.set_partial_diagonal(self.diag.iter().map(|e| N::from_real(e.modulus())));
+        res
+    }
+
+    /// Retrieves the upper trapezoidal submatrix `R` of this decomposition.
+    ///
+    /// This is usually faster than `r` but consumes `self`.
+    #[inline]
+    pub fn unpack_r(self) -> MatrixMN<N, DimMinimum<R, C>, C>
+    where
+        DefaultAllocator: Reallocator<N, R, C, DimMinimum<R, C>, C>,
+    {
+        let (nrows, ncols) = self.qrp.data.shape();
+        let mut res = self.qrp.resize_generic(nrows.min(ncols), ncols, N::zero());
+        res.fill_lower_triangle(N::zero(), 1);
+        res.set_partial_diagonal(self.diag.iter().map(|e| N::from_real(e.modulus())));
+        res
+    }
+
+    /// Computes the orthogonal matrix `Q` of this decomposition.
+    pub fn q(&self) -> MatrixMN<N, R, DimMinimum<R, C>>
+    where DefaultAllocator: Allocator<N, R, DimMinimum<R, C>> {
+        let (nrows, ncols) = self.qrp.data.shape();
+
+        // NOTE: we could build the identity matrix and call q_mul on it.
+        // Instead we don't so that we take in account the matrix sparseness.
+        let mut res = Matrix::identity_generic(nrows, nrows.min(ncols));
+        let dim = self.diag.len();
+
+        for i in (0..dim).rev() {
+            let axis = self.qrp.slice_range(i.., i);
+            // FIXME: sometimes, the axis might have a zero magnitude.
+            let refl = Reflection::new(Unit::new_unchecked(axis), N::zero());
+
+            let mut res_rows = res.slice_range_mut(i.., i..);
+            refl.reflect_with_sign(&mut res_rows, self.diag[i].signum());
+        }
+
+        res
+    }
+    /// The column permutations of this decomposition.
+    #[inline]
+    pub fn p(&self) -> &PermutationSequence<DimMinimum<R, C>> {
+        &self.p
+    }
+
+    /// Unpacks this decomposition into its two matrix factors.
+    pub fn unpack(
+        self,
+    ) -> (
+        MatrixMN<N, R, DimMinimum<R, C>>,
+        MatrixMN<N, DimMinimum<R, C>, C>,
+    )
+    where
+        DimMinimum<R, C>: DimMin<C, Output = DimMinimum<R, C>>,
+        DefaultAllocator:
+            Allocator<N, R, DimMinimum<R, C>> + Reallocator<N, R, C, DimMinimum<R, C>, C>,
+    {
+        (self.q(), self.unpack_r())
+    }
+
+    #[doc(hidden)]
+    pub fn qrp_internal(&self) -> &MatrixMN<N, R, C> {
+        &self.qrp
+    }
+
+    /// Multiplies the provided matrix by the transpose of the `Q` matrix of this decomposition.
+    pub fn q_tr_mul<R2: Dim, C2: Dim, S2>(&self, rhs: &mut Matrix<N, R2, C2, S2>)
+    // FIXME: do we need a static constraint on the number of rows of rhs?
+    where S2: StorageMut<N, R2, C2> {
+        let dim = self.diag.len();
+
+        for i in 0..dim {
+            let axis = self.qrp.slice_range(i.., i);
+            let refl = Reflection::new(Unit::new_unchecked(axis), N::zero());
+
+            let mut rhs_rows = rhs.rows_range_mut(i..);
+            refl.reflect_with_sign(&mut rhs_rows, self.diag[i].signum().conjugate());
+        }
+    }
+}
+
+impl<N: ComplexField, D: DimMin<D, Output = D>> QRP<N, D, D>
+where DefaultAllocator: Allocator<N, D, D> + Allocator<N, D> + 
+    Allocator<(usize, usize), DimMinimum<D, D>>
+{
+    /// Solves the linear system `self * x = b`, where `x` is the unknown to be determined.
+    ///
+    /// Returns `None` if `self` is not invertible.
+    pub fn solve<R2: Dim, C2: Dim, S2>(
+        &self,
+        b: &Matrix<N, R2, C2, S2>,
+    ) -> Option<MatrixMN<N, R2, C2>>
+    where
+        S2: StorageMut<N, R2, C2>,
+        ShapeConstraint: SameNumberOfRows<R2, D>,
+        DefaultAllocator: Allocator<N, R2, C2>,
+    {
+        let mut res = b.clone_owned();
+
+        if self.solve_mut(&mut res) {
+            Some(res)
+        } else {
+            None
+        }
+    }
+
+    /// Solves the linear system `self * x = b`, where `x` is the unknown to be determined.
+    ///
+    /// If the decomposed matrix is not invertible, this returns `false` and its input `b` is
+    /// overwritten with garbage.
+    pub fn solve_mut<R2: Dim, C2: Dim, S2>(&self, b: &mut Matrix<N, R2, C2, S2>) -> bool
+    where
+        S2: StorageMut<N, R2, C2>,
+        ShapeConstraint: SameNumberOfRows<R2, D>,
+    {
+        assert_eq!(
+            self.qrp.nrows(),
+            b.nrows(),
+            "QRP solve matrix dimension mismatch."
+        );
+        assert!(
+            self.qrp.is_square(),
+            "QRP solve: unable to solve a non-square system."
+        );
+
+        self.q_tr_mul(b);
+        self.solve_upper_triangular_mut(b)
+    }
+
+    // FIXME: duplicate code from the `solve` module.
+    fn solve_upper_triangular_mut<R2: Dim, C2: Dim, S2>(
+        &self,
+        b: &mut Matrix<N, R2, C2, S2>,
+    ) -> bool
+    where
+        S2: StorageMut<N, R2, C2>,
+        ShapeConstraint: SameNumberOfRows<R2, D>,
+    {
+        let dim = self.qrp.nrows();
+
+        for k in 0..b.ncols() {
+            let mut b = b.column_mut(k);
+            for i in (0..dim).rev() {
+                let coeff;
+
+                unsafe {
+                    let diag = self.diag.vget_unchecked(i).modulus();
+
+                    if diag.is_zero() {
+                        return false;
+                    }
+
+                    coeff = b.vget_unchecked(i).unscale(diag);
+                    *b.vget_unchecked_mut(i) = coeff;
+                }
+
+                b.rows_range_mut(..i)
+                    .axpy(-coeff, &self.qrp.slice_range(..i, i), N::one());
+            }
+        }
+
+        true
+    }
+
+    /// Computes the inverse of the decomposed matrix.
+    ///
+    /// Returns `None` if the decomposed matrix is not invertible.
+    pub fn try_inverse(&self) -> Option<MatrixN<N, D>> {
+        assert!(
+            self.qrp.is_square(),
+            "QRP inverse: unable to compute the inverse of a non-square matrix."
+        );
+
+        // FIXME: is there a less naive method ?
+        let (nrows, ncols) = self.qrp.data.shape();
+        let mut res = MatrixN::identity_generic(nrows, ncols);
+
+        if self.solve_mut(&mut res) {
+            Some(res)
+        } else {
+            None
+        }
+    }
+
+    /// Indicates if the decomposed matrix is invertible.
+    pub fn is_invertible(&self) -> bool {
+        assert!(
+            self.qrp.is_square(),
+            "QRP: unable to test the invertibility of a non-square matrix."
+        );
+
+        for i in 0..self.diag.len() {
+            if self.diag[i].is_zero() {
+                return false;
+            }
+        }
+
+        true
+    }
+
+    // /// Computes the determinant of the decomposed matrix.
+    // pub fn determinant(&self) -> N {
+    //     let dim = self.qrp.nrows();
+    //     assert!(self.qrp.is_square(), "QRP determinant: unable to compute the determinant of a non-square matrix.");
+
+    //     let mut res = N::one();
+    //     for i in 0 .. dim {
+    //         res *= unsafe { *self.diag.vget_unchecked(i) };
+    //     }
+
+    //     res self.q_determinant()
+    // }
+}
+
+impl<N: ComplexField, R: DimMin<C>, C: Dim, S: Storage<N, R, C>> Matrix<N, R, C, S>
+where DefaultAllocator: Allocator<N, R, C> + Allocator<N, R> + Allocator<N, DimMinimum<R, C>> + Allocator<(usize, usize), DimMinimum<R, C>>
+{
+    /// Computes the QRP decomposition of this matrix.
+    pub fn qrp(self) -> QRP<N, R, C> {
+        QRP::new(self.into_owned())
+    }
+}

From a2f3e1ac26ceff96d5ac0a0e2e2204c92374ca59 Mon Sep 17 00:00:00 2001
From: russellb23 <bertrussell23@yahoo.in>
Date: Mon, 24 Jun 2019 08:49:57 +0530
Subject: [PATCH 2/7] Inverted sign in householder

---
 src/linalg/householder.rs | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/linalg/householder.rs b/src/linalg/householder.rs
index ebbffd30..3692fe31 100644
--- a/src/linalg/householder.rs
+++ b/src/linalg/householder.rs
@@ -34,7 +34,7 @@ pub fn reflection_axis_mut<N: ComplexField, D: Dim, S: StorageMut<N, D>>(
 
     if !factor.is_zero() {
         column.unscale_mut(factor.sqrt());
-        (-signed_norm, true)
+        (signed_norm, true)
     } else {
         // TODO: not sure why we don't have a - sign here.
         (signed_norm, false)

From 13161336255ac3aa59aec80c176831604cb96204 Mon Sep 17 00:00:00 2001
From: russellb23 <bertrussell23@yahoo.in>
Date: Mon, 24 Jun 2019 09:20:44 +0530
Subject: [PATCH 3/7] Removed unused imports

---
 src/linalg/qrp.rs | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/src/linalg/qrp.rs b/src/linalg/qrp.rs
index d251cc5d..c71ac578 100644
--- a/src/linalg/qrp.rs
+++ b/src/linalg/qrp.rs
@@ -13,11 +13,7 @@ use crate::geometry::Reflection;
 use crate::linalg::householder;
 
 //=============================================================================
-use alga::general::RealField;
 use crate::linalg::PermutationSequence;
-use crate::base::VecStorage;
-use crate::base::Dynamic;
-use crate::base::DVector;
 //=============================================================================
 
 /// The QRP decomposition of a general matrix.

From 63a34528e020edf037ab86b2cdf95642a01dc43d Mon Sep 17 00:00:00 2001
From: russellb23 <bertrussell23@yahoo.in>
Date: Mon, 24 Jun 2019 12:20:14 +0530
Subject: [PATCH 4/7] Added test for QR factorization and fixed unpack issue

---
 src/linalg/qrp.rs   |  38 ++++++-----
 tests/linalg/qrp.rs | 156 ++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 178 insertions(+), 16 deletions(-)
 create mode 100644 tests/linalg/qrp.rs

diff --git a/src/linalg/qrp.rs b/src/linalg/qrp.rs
index c71ac578..c7d2f27b 100644
--- a/src/linalg/qrp.rs
+++ b/src/linalg/qrp.rs
@@ -11,10 +11,7 @@ use crate::storage::{Storage, StorageMut};
 
 use crate::geometry::Reflection;
 use crate::linalg::householder;
-
-//=============================================================================
 use crate::linalg::PermutationSequence;
-//=============================================================================
 
 /// The QRP decomposition of a general matrix.
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -65,7 +62,9 @@ where DefaultAllocator: Allocator<N, R, C> + Allocator<N, R> + Allocator<N, DimM
         let (nrows, ncols) = matrix.data.shape();
         let min_nrows_ncols = nrows.min(ncols);
         let mut p = PermutationSequence::identity_generic(min_nrows_ncols);
+
         let mut diag = unsafe { MatrixMN::new_uninitialized_generic(min_nrows_ncols, U1) };
+        println!("diag: {:?}", &diag);
 
         if min_nrows_ncols.value() == 0 {
             return QRP {
@@ -76,12 +75,18 @@ where DefaultAllocator: Allocator<N, R, C> + Allocator<N, R> + Allocator<N, DimM
         }
 
         for ite in 0..min_nrows_ncols.value() {
+            let mut col_norm = Vec::new();
+            for column in matrix.column_iter() {
+                col_norm.push(column.norm_squared());
+            }
+            
             let piv = matrix.slice_range(ite.., ite..).icamax_full();
             let col_piv = piv.1 + ite;
             matrix.swap_columns(ite, col_piv);
             p.append_permutation(ite, col_piv);
 
             householder::clear_column_unchecked(&mut matrix, &mut diag[ite], ite, 0, None);
+            println!("matrix: {:?}", &matrix.data);
         }
 
         QRP {
@@ -139,7 +144,7 @@ where DefaultAllocator: Allocator<N, R, C> + Allocator<N, R> + Allocator<N, DimM
 
         res
     }
-    /// The column permutations of this decomposition.
+    /// Retrieves the column permutation of this decomposition.
     #[inline]
     pub fn p(&self) -> &PermutationSequence<DimMinimum<R, C>> {
         &self.p
@@ -151,13 +156,14 @@ where DefaultAllocator: Allocator<N, R, C> + Allocator<N, R> + Allocator<N, DimM
     ) -> (
         MatrixMN<N, R, DimMinimum<R, C>>,
         MatrixMN<N, DimMinimum<R, C>, C>,
+        PermutationSequence<DimMinimum<R, C>>,
     )
     where
         DimMinimum<R, C>: DimMin<C, Output = DimMinimum<R, C>>,
         DefaultAllocator:
-            Allocator<N, R, DimMinimum<R, C>> + Reallocator<N, R, C, DimMinimum<R, C>, C>,
+            Allocator<N, R, DimMinimum<R, C>> + Reallocator<N, R, C, DimMinimum<R, C>, C> + Allocator<(usize, usize), DimMinimum<R, C>>,
     {
-        (self.q(), self.unpack_r())
+        (self.q(), self.r(), self.p)
     }
 
     #[doc(hidden)]
@@ -300,18 +306,18 @@ where DefaultAllocator: Allocator<N, D, D> + Allocator<N, D> +
         true
     }
 
-    // /// Computes the determinant of the decomposed matrix.
-    // pub fn determinant(&self) -> N {
-    //     let dim = self.qrp.nrows();
-    //     assert!(self.qrp.is_square(), "QRP determinant: unable to compute the determinant of a non-square matrix.");
+     /// Computes the determinant of the decomposed matrix.
+     pub fn determinant(&self) -> N {
+         let dim = self.qrp.nrows();
+         assert!(self.qrp.is_square(), "QRP determinant: unable to compute the determinant of a non-square matrix.");
 
-    //     let mut res = N::one();
-    //     for i in 0 .. dim {
-    //         res *= unsafe { *self.diag.vget_unchecked(i) };
-    //     }
+         let mut res = N::one();
+         for i in 0 .. dim {
+             res *= unsafe { *self.diag.vget_unchecked(i) };
+         }
 
-    //     res self.q_determinant()
-    // }
+         res * self.p.determinant()
+     }
 }
 
 impl<N: ComplexField, R: DimMin<C>, C: Dim, S: Storage<N, R, C>> Matrix<N, R, C, S>
diff --git a/tests/linalg/qrp.rs b/tests/linalg/qrp.rs
new file mode 100644
index 00000000..78b96b74
--- /dev/null
+++ b/tests/linalg/qrp.rs
@@ -0,0 +1,156 @@
+#[cfg_attr(rustfmt, rustfmt_skip)]
+
+use na::Matrix4;
+
+#[test]
+fn qrp() {
+    let m = Matrix4::new (
+            1.0, -1.0, 2.0, 1.0,
+            -1.0, 3.0, -1.0, -1.0,
+            3.0, -5.0, 5.0, 3.0,
+            1.0, 2.0, 1.0, -2.0);
+    let qrp = m.qrp();
+    assert!(relative_eq!(qrp.determinant(), 0.0, epsilon = 1.0e-7));
+
+    let (q, r, p) = qrp.unpack();
+
+    let mut qr = q * r;
+    p.inv_permute_columns(& mut qr);
+
+    assert!(relative_eq!(m, qr, epsilon = 1.0e-7));
+}
+
+#[cfg(feature = "arbitrary")]
+mod quickcheck_tests {
+    macro_rules! gen_tests(
+        ($module: ident, $scalar: ty) => {
+            mod $module {
+                use na::{DMatrix, DVector, Matrix3x5, Matrix4, Matrix4x3, Matrix5x3, Vector4};
+                use std::cmp;
+                #[allow(unused_imports)]
+                use crate::core::helper::{RandScalar, RandComplex};
+    
+                quickcheck! {
+                    fn qrp(m: DMatrix<$scalar>) -> bool {
+                        let m = m.map(|e| e.0);
+                        let qrp = m.clone().qrp();
+                        let q  = qrp.q();
+                        let r  = qrp.r();
+    
+                        println!("m: {}", m);
+                        println!("qrp: {}", &q * &r);
+    
+                        relative_eq!(m, &q * r, epsilon = 1.0e-7) &&
+                        q.is_orthogonal(1.0e-7)
+                    }
+    
+                    fn qrp_static_5_3(m: Matrix5x3<$scalar>) -> bool {
+                        let m = m.map(|e| e.0);
+                        let qrp = m.qrp();
+                        let q  = qrp.q();
+                        let r  = qrp.r();
+    
+                        relative_eq!(m, q * r, epsilon = 1.0e-7) &&
+                        q.is_orthogonal(1.0e-7)
+                    }
+    
+                    fn qrp_static_3_5(m: Matrix3x5<$scalar>) -> bool {
+                        let m = m.map(|e| e.0);
+                        let qrp = m.qrp();
+                        let q  = qrp.q();
+                        let r  = qrp.r();
+
+                    relative_eq!(m, q * r, epsilon = 1.0e-7) &&
+                    q.is_orthogonal(1.0e-7)
+                }
+
+                    fn qrp_static_square(m: Matrix4<$scalar>) -> bool {
+                        let m = m.map(|e| e.0);
+                        let qrp = m.qrp();
+                        let q  = qrp.q();
+                        let r  = qrp.r();
+    
+                        println!("{}{}{}{}", q, r, q * r, m);
+    
+                        relative_eq!(m, q * r, epsilon = 1.0e-7) &&
+                        q.is_orthogonal(1.0e-7)
+                    }
+    
+                    fn qrp_solve(n: usize, nb: usize) -> bool {
+                        if n != 0 && nb != 0 {
+                            let n  = cmp::min(n, 50);  // To avoid slowing down the test too much.
+                            let nb = cmp::min(nb, 50); // To avoid slowing down the test too much.
+                            let m  = DMatrix::<$scalar>::new_random(n, n).map(|e| e.0);
+    
+                            let qrp = m.clone().qrp();
+                            let b1 = DVector::<$scalar>::new_random(n).map(|e| e.0);
+                            let b2 = DMatrix::<$scalar>::new_random(n, nb).map(|e| e.0);
+    
+                            if qrp.is_invertible() {
+                                let sol1 = qrp.solve(&b1).unwrap();
+                                let sol2 = qrp.solve(&b2).unwrap();
+    
+                                return relative_eq!(&m * sol1, b1, epsilon = 1.0e-6) &&
+                                    relative_eq!(&m * sol2, b2, epsilon = 1.0e-6)
+                            }
+                        }
+    
+                        return true;
+                    }
+    
+                    fn qrp_solve_static(m: Matrix4<$scalar>) -> bool {
+                         let m = m.map(|e| e.0);
+                         let qrp = m.qrp();
+                         let b1 = Vector4::<$scalar>::new_random().map(|e| e.0);
+                         let b2 = Matrix4x3::<$scalar>::new_random().map(|e| e.0);
+    
+                         if qrp.is_invertible() {
+                             let sol1 = qrp.solve(&b1).unwrap();
+                             let sol2 = qrp.solve(&b2).unwrap();
+    
+                             relative_eq!(m * sol1, b1, epsilon = 1.0e-6) &&
+                             relative_eq!(m * sol2, b2, epsilon = 1.0e-6)
+                         }
+                         else {
+                             false
+                         }
+                    }
+
+                    fn qrp_inverse(n: usize) -> bool {
+                        let n = cmp::max(1, cmp::min(n, 15)); // To avoid slowing down the test too much.
+                        let m = DMatrix::<$scalar>::new_random(n, n).map(|e| e.0);
+    
+                        if let Some(m1) = m.clone().qrp().try_inverse() {
+                            let id1 = &m  * &m1;
+                            let id2 = &m1 * &m;
+    
+                            id1.is_identity(1.0e-5) && id2.is_identity(1.0e-5)
+                        }
+                        else {
+                            true
+                        }
+                    }
+    
+                    fn qrp_inverse_static(m: Matrix4<$scalar>) -> bool {
+                        let m  = m.map(|e| e.0);
+                        let qrp = m.qrp();
+    
+                        if let Some(m1) = qrp.try_inverse() {
+                            let id1 = &m  * &m1;
+                            let id2 = &m1 * &m;
+    
+                            id1.is_identity(1.0e-5) && id2.is_identity(1.0e-5)
+                        }
+                        else {
+                            true
+                        }
+                    }
+                }
+            }
+        }
+    );
+
+    gen_tests!(complex, RandComplex<f64>);
+    gen_tests!(f64, RandScalar<f64>);
+}
+

From 308d95386e1f775f437001331eecf2393d992668 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= <developer@crozet.re>
Date: Thu, 25 Feb 2021 12:06:04 +0100
Subject: [PATCH 5/7] Fix all tests and the ColPivQR::solve.

---
 src/linalg/{qrp.rs => col_piv_qr.rs}   | 200 +++++++++++++------------
 src/linalg/householder.rs              |   2 +-
 src/linalg/mod.rs                      |   2 +
 src/linalg/qr.rs                       |   4 +-
 tests/linalg/{qrp.rs => col_piv_qr.rs} | 163 ++++++++++----------
 tests/linalg/mod.rs                    |   1 +
 6 files changed, 198 insertions(+), 174 deletions(-)
 rename src/linalg/{qrp.rs => col_piv_qr.rs} (60%)
 rename tests/linalg/{qrp.rs => col_piv_qr.rs} (52%)

diff --git a/src/linalg/qrp.rs b/src/linalg/col_piv_qr.rs
similarity index 60%
rename from src/linalg/qrp.rs
rename to src/linalg/col_piv_qr.rs
index c7d2f27b..3f8ecda1 100644
--- a/src/linalg/qrp.rs
+++ b/src/linalg/col_piv_qr.rs
@@ -1,98 +1,94 @@
+use num::Zero;
 #[cfg(feature = "serde-serialize")]
 use serde::{Deserialize, Serialize};
-use num::Zero;
 
-use alga::general::ComplexField;
 use crate::allocator::{Allocator, Reallocator};
 use crate::base::{DefaultAllocator, Matrix, MatrixMN, MatrixN, Unit, VectorN};
 use crate::constraint::{SameNumberOfRows, ShapeConstraint};
 use crate::dimension::{Dim, DimMin, DimMinimum, U1};
 use crate::storage::{Storage, StorageMut};
+use crate::ComplexField;
 
 use crate::geometry::Reflection;
-use crate::linalg::householder;
-use crate::linalg::PermutationSequence;
+use crate::linalg::{householder, PermutationSequence};
 
-/// The QRP decomposition of a general matrix.
+/// The QR decomposition (with column pivoting) of a general matrix.
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[cfg_attr(
     feature = "serde-serialize",
-    serde(bound(
-        serialize = "DefaultAllocator: Allocator<N, R, C> +
+    serde(bound(serialize = "DefaultAllocator: Allocator<N, R, C> +
                            Allocator<N, DimMinimum<R, C>>,
          MatrixMN<N, R, C>: Serialize,
          PermutationSequence<DimMinimum<R, C>>: Serialize,
-         VectorN<N, DimMinimum<R, C>>: Serialize"
-    ))
+         VectorN<N, DimMinimum<R, C>>: Serialize"))
 )]
 #[cfg_attr(
     feature = "serde-serialize",
-    serde(bound(
-        deserialize = "DefaultAllocator: Allocator<N, R, C> +
+    serde(bound(deserialize = "DefaultAllocator: Allocator<N, R, C> +
                            Allocator<N, DimMinimum<R, C>>,
          MatrixMN<N, R, C>: Deserialize<'de>,
          PermutationSequence<DimMinimum<R, C>>: Deserialize<'de>,
-         VectorN<N, DimMinimum<R, C>>: Deserialize<'de>"
-    ))
+         VectorN<N, DimMinimum<R, C>>: Deserialize<'de>"))
 )]
 #[derive(Clone, Debug)]
-pub struct QRP<N: ComplexField, R: DimMin<C>, C: Dim>
-where DefaultAllocator: Allocator<N, R, C> + Allocator<N, DimMinimum<R, C>> +
-    Allocator<(usize, usize), DimMinimum<R, C>>,
+pub struct ColPivQR<N: ComplexField, R: DimMin<C>, C: Dim>
+where
+    DefaultAllocator: Allocator<N, R, C>
+        + Allocator<N, DimMinimum<R, C>>
+        + Allocator<(usize, usize), DimMinimum<R, C>>,
 {
-    qrp: MatrixMN<N, R, C>,
+    col_piv_qr: MatrixMN<N, R, C>,
     p: PermutationSequence<DimMinimum<R, C>>,
     diag: VectorN<N, DimMinimum<R, C>>,
 }
 
-impl<N: ComplexField, R: DimMin<C>, C: Dim> Copy for QRP<N, R, C>
+impl<N: ComplexField, R: DimMin<C>, C: Dim> Copy for ColPivQR<N, R, C>
 where
-    DefaultAllocator: Allocator<N, R, C> + Allocator<N, DimMinimum<R, C>> +
-    Allocator<(usize, usize), DimMinimum<R, C>>,
+    DefaultAllocator: Allocator<N, R, C>
+        + Allocator<N, DimMinimum<R, C>>
+        + Allocator<(usize, usize), DimMinimum<R, C>>,
     MatrixMN<N, R, C>: Copy,
     PermutationSequence<DimMinimum<R, C>>: Copy,
     VectorN<N, DimMinimum<R, C>>: Copy,
-{}
-
-impl<N: ComplexField, R: DimMin<C>, C: Dim> QRP<N, R, C>
-where DefaultAllocator: Allocator<N, R, C> + Allocator<N, R> + Allocator<N, DimMinimum<R, C>> + Allocator<(usize, usize), DimMinimum<R, C>>
 {
-    /// Computes the QRP decomposition using householder reflections.
+}
+
+impl<N: ComplexField, R: DimMin<C>, C: Dim> ColPivQR<N, R, C>
+where
+    DefaultAllocator: Allocator<N, R, C>
+        + Allocator<N, R>
+        + Allocator<N, DimMinimum<R, C>>
+        + Allocator<(usize, usize), DimMinimum<R, C>>,
+{
+    /// Computes the ColPivQR decomposition using householder reflections.
     pub fn new(mut matrix: MatrixMN<N, R, C>) -> Self {
         let (nrows, ncols) = matrix.data.shape();
         let min_nrows_ncols = nrows.min(ncols);
         let mut p = PermutationSequence::identity_generic(min_nrows_ncols);
 
         let mut diag = unsafe { MatrixMN::new_uninitialized_generic(min_nrows_ncols, U1) };
-        println!("diag: {:?}", &diag);
 
         if min_nrows_ncols.value() == 0 {
-            return QRP {
-                qrp: matrix,
-                p: p,
-                diag: diag,
+            return ColPivQR {
+                col_piv_qr: matrix,
+                p,
+                diag,
             };
         }
 
-        for ite in 0..min_nrows_ncols.value() {
-            let mut col_norm = Vec::new();
-            for column in matrix.column_iter() {
-                col_norm.push(column.norm_squared());
-            }
-            
-            let piv = matrix.slice_range(ite.., ite..).icamax_full();
-            let col_piv = piv.1 + ite;
-            matrix.swap_columns(ite, col_piv);
-            p.append_permutation(ite, col_piv);
+        for i in 0..min_nrows_ncols.value() {
+            let piv = matrix.slice_range(i.., i..).icamax_full();
+            let col_piv = piv.1 + i;
+            matrix.swap_columns(i, col_piv);
+            p.append_permutation(i, col_piv);
 
-            householder::clear_column_unchecked(&mut matrix, &mut diag[ite], ite, 0, None);
-            println!("matrix: {:?}", &matrix.data);
+            householder::clear_column_unchecked(&mut matrix, &mut diag[i], i, 0, None);
         }
 
-        QRP {
-            qrp: matrix,
-            p: p,
-            diag: diag,
+        ColPivQR {
+            col_piv_qr: matrix,
+            p,
+            diag,
         }
     }
 
@@ -102,8 +98,11 @@ where DefaultAllocator: Allocator<N, R, C> + Allocator<N, R> + Allocator<N, DimM
     where
         DefaultAllocator: Allocator<N, DimMinimum<R, C>, C>,
     {
-        let (nrows, ncols) = self.qrp.data.shape();
-        let mut res = self.qrp.rows_generic(0, nrows.min(ncols)).upper_triangle();
+        let (nrows, ncols) = self.col_piv_qr.data.shape();
+        let mut res = self
+            .col_piv_qr
+            .rows_generic(0, nrows.min(ncols))
+            .upper_triangle();
         res.set_partial_diagonal(self.diag.iter().map(|e| N::from_real(e.modulus())));
         res
     }
@@ -116,8 +115,10 @@ where DefaultAllocator: Allocator<N, R, C> + Allocator<N, R> + Allocator<N, DimM
     where
         DefaultAllocator: Reallocator<N, R, C, DimMinimum<R, C>, C>,
     {
-        let (nrows, ncols) = self.qrp.data.shape();
-        let mut res = self.qrp.resize_generic(nrows.min(ncols), ncols, N::zero());
+        let (nrows, ncols) = self.col_piv_qr.data.shape();
+        let mut res = self
+            .col_piv_qr
+            .resize_generic(nrows.min(ncols), ncols, N::zero());
         res.fill_lower_triangle(N::zero(), 1);
         res.set_partial_diagonal(self.diag.iter().map(|e| N::from_real(e.modulus())));
         res
@@ -125,8 +126,10 @@ where DefaultAllocator: Allocator<N, R, C> + Allocator<N, R> + Allocator<N, DimM
 
     /// Computes the orthogonal matrix `Q` of this decomposition.
     pub fn q(&self) -> MatrixMN<N, R, DimMinimum<R, C>>
-    where DefaultAllocator: Allocator<N, R, DimMinimum<R, C>> {
-        let (nrows, ncols) = self.qrp.data.shape();
+    where
+        DefaultAllocator: Allocator<N, R, DimMinimum<R, C>>,
+    {
+        let (nrows, ncols) = self.col_piv_qr.data.shape();
 
         // NOTE: we could build the identity matrix and call q_mul on it.
         // Instead we don't so that we take in account the matrix sparseness.
@@ -134,8 +137,8 @@ where DefaultAllocator: Allocator<N, R, C> + Allocator<N, R> + Allocator<N, DimM
         let dim = self.diag.len();
 
         for i in (0..dim).rev() {
-            let axis = self.qrp.slice_range(i.., i);
-            // FIXME: sometimes, the axis might have a zero magnitude.
+            let axis = self.col_piv_qr.slice_range(i.., i);
+            // TODO: sometimes, the axis might have a zero magnitude.
             let refl = Reflection::new(Unit::new_unchecked(axis), N::zero());
 
             let mut res_rows = res.slice_range_mut(i.., i..);
@@ -160,25 +163,27 @@ where DefaultAllocator: Allocator<N, R, C> + Allocator<N, R> + Allocator<N, DimM
     )
     where
         DimMinimum<R, C>: DimMin<C, Output = DimMinimum<R, C>>,
-        DefaultAllocator:
-            Allocator<N, R, DimMinimum<R, C>> + Reallocator<N, R, C, DimMinimum<R, C>, C> + Allocator<(usize, usize), DimMinimum<R, C>>,
+        DefaultAllocator: Allocator<N, R, DimMinimum<R, C>>
+            + Reallocator<N, R, C, DimMinimum<R, C>, C>
+            + Allocator<(usize, usize), DimMinimum<R, C>>,
     {
         (self.q(), self.r(), self.p)
     }
 
     #[doc(hidden)]
-    pub fn qrp_internal(&self) -> &MatrixMN<N, R, C> {
-        &self.qrp
+    pub fn col_piv_qr_internal(&self) -> &MatrixMN<N, R, C> {
+        &self.col_piv_qr
     }
 
     /// Multiplies the provided matrix by the transpose of the `Q` matrix of this decomposition.
     pub fn q_tr_mul<R2: Dim, C2: Dim, S2>(&self, rhs: &mut Matrix<N, R2, C2, S2>)
-    // FIXME: do we need a static constraint on the number of rows of rhs?
-    where S2: StorageMut<N, R2, C2> {
+    where
+        S2: StorageMut<N, R2, C2>,
+    {
         let dim = self.diag.len();
 
         for i in 0..dim {
-            let axis = self.qrp.slice_range(i.., i);
+            let axis = self.col_piv_qr.slice_range(i.., i);
             let refl = Reflection::new(Unit::new_unchecked(axis), N::zero());
 
             let mut rhs_rows = rhs.rows_range_mut(i..);
@@ -187,9 +192,10 @@ where DefaultAllocator: Allocator<N, R, C> + Allocator<N, R> + Allocator<N, DimM
     }
 }
 
-impl<N: ComplexField, D: DimMin<D, Output = D>> QRP<N, D, D>
-where DefaultAllocator: Allocator<N, D, D> + Allocator<N, D> + 
-    Allocator<(usize, usize), DimMinimum<D, D>>
+impl<N: ComplexField, D: DimMin<D, Output = D>> ColPivQR<N, D, D>
+where
+    DefaultAllocator:
+        Allocator<N, D, D> + Allocator<N, D> + Allocator<(usize, usize), DimMinimum<D, D>>,
 {
     /// Solves the linear system `self * x = b`, where `x` is the unknown to be determined.
     ///
@@ -222,20 +228,23 @@ where DefaultAllocator: Allocator<N, D, D> + Allocator<N, D> +
         ShapeConstraint: SameNumberOfRows<R2, D>,
     {
         assert_eq!(
-            self.qrp.nrows(),
+            self.col_piv_qr.nrows(),
             b.nrows(),
-            "QRP solve matrix dimension mismatch."
+            "ColPivQR solve matrix dimension mismatch."
         );
         assert!(
-            self.qrp.is_square(),
-            "QRP solve: unable to solve a non-square system."
+            self.col_piv_qr.is_square(),
+            "ColPivQR solve: unable to solve a non-square system."
         );
 
         self.q_tr_mul(b);
-        self.solve_upper_triangular_mut(b)
+        let solved = self.solve_upper_triangular_mut(b);
+        self.p.inv_permute_rows(b);
+
+        solved
     }
 
-    // FIXME: duplicate code from the `solve` module.
+    // TODO: duplicate code from the `solve` module.
     fn solve_upper_triangular_mut<R2: Dim, C2: Dim, S2>(
         &self,
         b: &mut Matrix<N, R2, C2, S2>,
@@ -244,7 +253,7 @@ where DefaultAllocator: Allocator<N, D, D> + Allocator<N, D> +
         S2: StorageMut<N, R2, C2>,
         ShapeConstraint: SameNumberOfRows<R2, D>,
     {
-        let dim = self.qrp.nrows();
+        let dim = self.col_piv_qr.nrows();
 
         for k in 0..b.ncols() {
             let mut b = b.column_mut(k);
@@ -263,7 +272,7 @@ where DefaultAllocator: Allocator<N, D, D> + Allocator<N, D> +
                 }
 
                 b.rows_range_mut(..i)
-                    .axpy(-coeff, &self.qrp.slice_range(..i, i), N::one());
+                    .axpy(-coeff, &self.col_piv_qr.slice_range(..i, i), N::one());
             }
         }
 
@@ -275,12 +284,12 @@ where DefaultAllocator: Allocator<N, D, D> + Allocator<N, D> +
     /// Returns `None` if the decomposed matrix is not invertible.
     pub fn try_inverse(&self) -> Option<MatrixN<N, D>> {
         assert!(
-            self.qrp.is_square(),
-            "QRP inverse: unable to compute the inverse of a non-square matrix."
+            self.col_piv_qr.is_square(),
+            "ColPivQR inverse: unable to compute the inverse of a non-square matrix."
         );
 
-        // FIXME: is there a less naive method ?
-        let (nrows, ncols) = self.qrp.data.shape();
+        // TODO: is there a less naive method ?
+        let (nrows, ncols) = self.col_piv_qr.data.shape();
         let mut res = MatrixN::identity_generic(nrows, ncols);
 
         if self.solve_mut(&mut res) {
@@ -293,8 +302,8 @@ where DefaultAllocator: Allocator<N, D, D> + Allocator<N, D> +
     /// Indicates if the decomposed matrix is invertible.
     pub fn is_invertible(&self) -> bool {
         assert!(
-            self.qrp.is_square(),
-            "QRP: unable to test the invertibility of a non-square matrix."
+            self.col_piv_qr.is_square(),
+            "ColPivQR: unable to test the invertibility of a non-square matrix."
         );
 
         for i in 0..self.diag.len() {
@@ -306,25 +315,32 @@ where DefaultAllocator: Allocator<N, D, D> + Allocator<N, D> +
         true
     }
 
-     /// Computes the determinant of the decomposed matrix.
-     pub fn determinant(&self) -> N {
-         let dim = self.qrp.nrows();
-         assert!(self.qrp.is_square(), "QRP determinant: unable to compute the determinant of a non-square matrix.");
+    /// Computes the determinant of the decomposed matrix.
+    pub fn determinant(&self) -> N {
+        let dim = self.col_piv_qr.nrows();
+        assert!(
+            self.col_piv_qr.is_square(),
+            "ColPivQR determinant: unable to compute the determinant of a non-square matrix."
+        );
 
-         let mut res = N::one();
-         for i in 0 .. dim {
-             res *= unsafe { *self.diag.vget_unchecked(i) };
-         }
+        let mut res = N::one();
+        for i in 0..dim {
+            res *= unsafe { *self.diag.vget_unchecked(i) };
+        }
 
-         res * self.p.determinant()
-     }
+        res * self.p.determinant()
+    }
 }
 
 impl<N: ComplexField, R: DimMin<C>, C: Dim, S: Storage<N, R, C>> Matrix<N, R, C, S>
-where DefaultAllocator: Allocator<N, R, C> + Allocator<N, R> + Allocator<N, DimMinimum<R, C>> + Allocator<(usize, usize), DimMinimum<R, C>>
+where
+    DefaultAllocator: Allocator<N, R, C>
+        + Allocator<N, R>
+        + Allocator<N, DimMinimum<R, C>>
+        + Allocator<(usize, usize), DimMinimum<R, C>>,
 {
-    /// Computes the QRP decomposition of this matrix.
-    pub fn qrp(self) -> QRP<N, R, C> {
-        QRP::new(self.into_owned())
+    /// Computes the QR decomposition (with column pivoting) of this matrix.
+    pub fn col_piv_qr(self) -> ColPivQR<N, R, C> {
+        ColPivQR::new(self.into_owned())
     }
 }
diff --git a/src/linalg/householder.rs b/src/linalg/householder.rs
index 3692fe31..ebbffd30 100644
--- a/src/linalg/householder.rs
+++ b/src/linalg/householder.rs
@@ -34,7 +34,7 @@ pub fn reflection_axis_mut<N: ComplexField, D: Dim, S: StorageMut<N, D>>(
 
     if !factor.is_zero() {
         column.unscale_mut(factor.sqrt());
-        (signed_norm, true)
+        (-signed_norm, true)
     } else {
         // TODO: not sure why we don't have a - sign here.
         (signed_norm, false)
diff --git a/src/linalg/mod.rs b/src/linalg/mod.rs
index c602bfa7..81cf1f38 100644
--- a/src/linalg/mod.rs
+++ b/src/linalg/mod.rs
@@ -19,6 +19,7 @@ mod inverse;
 mod lu;
 mod permutation_sequence;
 mod qr;
+mod col_piv_qr;
 mod schur;
 mod solve;
 mod svd;
@@ -39,6 +40,7 @@ pub use self::hessenberg::*;
 pub use self::lu::*;
 pub use self::permutation_sequence::*;
 pub use self::qr::*;
+pub use self::col_piv_qr::*;
 pub use self::schur::*;
 pub use self::svd::*;
 pub use self::symmetric_eigen::*;
diff --git a/src/linalg/qr.rs b/src/linalg/qr.rs
index f404aa5a..fdf6b70a 100644
--- a/src/linalg/qr.rs
+++ b/src/linalg/qr.rs
@@ -60,8 +60,8 @@ where
             return QR { qr: matrix, diag };
         }
 
-        for ite in 0..min_nrows_ncols.value() {
-            householder::clear_column_unchecked(&mut matrix, &mut diag[ite], ite, 0, None);
+        for i in 0..min_nrows_ncols.value() {
+            householder::clear_column_unchecked(&mut matrix, &mut diag[i], i, 0, None);
         }
 
         QR { qr: matrix, diag }
diff --git a/tests/linalg/qrp.rs b/tests/linalg/col_piv_qr.rs
similarity index 52%
rename from tests/linalg/qrp.rs
rename to tests/linalg/col_piv_qr.rs
index 78b96b74..e9ffba86 100644
--- a/tests/linalg/qrp.rs
+++ b/tests/linalg/col_piv_qr.rs
@@ -3,19 +3,21 @@
 use na::Matrix4;
 
 #[test]
-fn qrp() {
-    let m = Matrix4::new (
-            1.0, -1.0, 2.0, 1.0,
-            -1.0, 3.0, -1.0, -1.0,
-            3.0, -5.0, 5.0, 3.0,
-            1.0, 2.0, 1.0, -2.0);
-    let qrp = m.qrp();
-    assert!(relative_eq!(qrp.determinant(), 0.0, epsilon = 1.0e-7));
+fn col_piv_qr() {
+    let m = Matrix4::new(
+        1.0, -1.0, 2.0, 1.0, -1.0, 3.0, -1.0, -1.0, 3.0, -5.0, 5.0, 3.0, 1.0, 2.0, 1.0, -2.0,
+    );
+    let col_piv_qr = m.col_piv_qr();
+    assert!(relative_eq!(
+        col_piv_qr.determinant(),
+        0.0,
+        epsilon = 1.0e-7
+    ));
 
-    let (q, r, p) = qrp.unpack();
+    let (q, r, p) = col_piv_qr.unpack();
 
     let mut qr = q * r;
-    p.inv_permute_columns(& mut qr);
+    p.inv_permute_columns(&mut qr);
 
     assert!(relative_eq!(m, qr, epsilon = 1.0e-7));
 }
@@ -29,85 +31,89 @@ mod quickcheck_tests {
                 use std::cmp;
                 #[allow(unused_imports)]
                 use crate::core::helper::{RandScalar, RandComplex};
-    
+
                 quickcheck! {
-                    fn qrp(m: DMatrix<$scalar>) -> bool {
+                    fn col_piv_qr(m: DMatrix<$scalar>) -> bool {
                         let m = m.map(|e| e.0);
-                        let qrp = m.clone().qrp();
-                        let q  = qrp.q();
-                        let r  = qrp.r();
-    
+                        let col_piv_qr = m.clone().col_piv_qr();
+                        let (q, r, p) = col_piv_qr.unpack();
+                        let mut qr = &q * &r;
+                        p.inv_permute_columns(&mut qr);
+
                         println!("m: {}", m);
-                        println!("qrp: {}", &q * &r);
-    
-                        relative_eq!(m, &q * r, epsilon = 1.0e-7) &&
-                        q.is_orthogonal(1.0e-7)
-                    }
-    
-                    fn qrp_static_5_3(m: Matrix5x3<$scalar>) -> bool {
-                        let m = m.map(|e| e.0);
-                        let qrp = m.qrp();
-                        let q  = qrp.q();
-                        let r  = qrp.r();
-    
-                        relative_eq!(m, q * r, epsilon = 1.0e-7) &&
-                        q.is_orthogonal(1.0e-7)
-                    }
-    
-                    fn qrp_static_3_5(m: Matrix3x5<$scalar>) -> bool {
-                        let m = m.map(|e| e.0);
-                        let qrp = m.qrp();
-                        let q  = qrp.q();
-                        let r  = qrp.r();
+                        println!("col_piv_qr: {}", &q * &r);
 
-                    relative_eq!(m, q * r, epsilon = 1.0e-7) &&
-                    q.is_orthogonal(1.0e-7)
-                }
-
-                    fn qrp_static_square(m: Matrix4<$scalar>) -> bool {
-                        let m = m.map(|e| e.0);
-                        let qrp = m.qrp();
-                        let q  = qrp.q();
-                        let r  = qrp.r();
-    
-                        println!("{}{}{}{}", q, r, q * r, m);
-    
-                        relative_eq!(m, q * r, epsilon = 1.0e-7) &&
+                        relative_eq!(m, &qr, epsilon = 1.0e-7) &&
                         q.is_orthogonal(1.0e-7)
                     }
-    
-                    fn qrp_solve(n: usize, nb: usize) -> bool {
+
+                    fn col_piv_qr_static_5_3(m: Matrix5x3<$scalar>) -> bool {
+                        let m = m.map(|e| e.0);
+                        let col_piv_qr = m.col_piv_qr();
+                        let (q, r, p) = col_piv_qr.unpack();
+                        let mut qr = q * r;
+                        p.inv_permute_columns(&mut qr);
+
+                        relative_eq!(m, qr, epsilon = 1.0e-7) &&
+                        q.is_orthogonal(1.0e-7)
+                    }
+
+                    fn col_piv_qr_static_3_5(m: Matrix3x5<$scalar>) -> bool {
+                        let m = m.map(|e| e.0);
+                        let col_piv_qr = m.col_piv_qr();
+                        let (q, r, p) = col_piv_qr.unpack();
+                        let mut qr = q * r;
+                        p.inv_permute_columns(&mut qr);
+
+                        relative_eq!(m, qr, epsilon = 1.0e-7) &&
+                        q.is_orthogonal(1.0e-7)
+                    }
+
+                    fn col_piv_qr_static_square(m: Matrix4<$scalar>) -> bool {
+                        let m = m.map(|e| e.0);
+                        let col_piv_qr = m.col_piv_qr();
+                        let (q, r, p) = col_piv_qr.unpack();
+                        let mut qr = q * r;
+                        p.inv_permute_columns(&mut qr);
+
+                        println!("{}{}{}{}", q, r, qr, m);
+
+                        relative_eq!(m, qr, epsilon = 1.0e-7) &&
+                        q.is_orthogonal(1.0e-7)
+                    }
+
+                    fn col_piv_qr_solve(n: usize, nb: usize) -> bool {
                         if n != 0 && nb != 0 {
                             let n  = cmp::min(n, 50);  // To avoid slowing down the test too much.
                             let nb = cmp::min(nb, 50); // To avoid slowing down the test too much.
                             let m  = DMatrix::<$scalar>::new_random(n, n).map(|e| e.0);
-    
-                            let qrp = m.clone().qrp();
+
+                            let col_piv_qr = m.clone().col_piv_qr();
                             let b1 = DVector::<$scalar>::new_random(n).map(|e| e.0);
                             let b2 = DMatrix::<$scalar>::new_random(n, nb).map(|e| e.0);
-    
-                            if qrp.is_invertible() {
-                                let sol1 = qrp.solve(&b1).unwrap();
-                                let sol2 = qrp.solve(&b2).unwrap();
-    
+
+                            if col_piv_qr.is_invertible() {
+                                let sol1 = col_piv_qr.solve(&b1).unwrap();
+                                let sol2 = col_piv_qr.solve(&b2).unwrap();
+
                                 return relative_eq!(&m * sol1, b1, epsilon = 1.0e-6) &&
                                     relative_eq!(&m * sol2, b2, epsilon = 1.0e-6)
                             }
                         }
-    
+
                         return true;
                     }
-    
-                    fn qrp_solve_static(m: Matrix4<$scalar>) -> bool {
+
+                    fn col_piv_qr_solve_static(m: Matrix4<$scalar>) -> bool {
                          let m = m.map(|e| e.0);
-                         let qrp = m.qrp();
+                         let col_piv_qr = m.col_piv_qr();
                          let b1 = Vector4::<$scalar>::new_random().map(|e| e.0);
                          let b2 = Matrix4x3::<$scalar>::new_random().map(|e| e.0);
-    
-                         if qrp.is_invertible() {
-                             let sol1 = qrp.solve(&b1).unwrap();
-                             let sol2 = qrp.solve(&b2).unwrap();
-    
+
+                         if col_piv_qr.is_invertible() {
+                             let sol1 = col_piv_qr.solve(&b1).unwrap();
+                             let sol2 = col_piv_qr.solve(&b2).unwrap();
+
                              relative_eq!(m * sol1, b1, epsilon = 1.0e-6) &&
                              relative_eq!(m * sol2, b2, epsilon = 1.0e-6)
                          }
@@ -116,29 +122,29 @@ mod quickcheck_tests {
                          }
                     }
 
-                    fn qrp_inverse(n: usize) -> bool {
+                    fn col_piv_qr_inverse(n: usize) -> bool {
                         let n = cmp::max(1, cmp::min(n, 15)); // To avoid slowing down the test too much.
                         let m = DMatrix::<$scalar>::new_random(n, n).map(|e| e.0);
-    
-                        if let Some(m1) = m.clone().qrp().try_inverse() {
+
+                        if let Some(m1) = m.clone().col_piv_qr().try_inverse() {
                             let id1 = &m  * &m1;
                             let id2 = &m1 * &m;
-    
+
                             id1.is_identity(1.0e-5) && id2.is_identity(1.0e-5)
                         }
                         else {
                             true
                         }
                     }
-    
-                    fn qrp_inverse_static(m: Matrix4<$scalar>) -> bool {
+
+                    fn col_piv_qr_inverse_static(m: Matrix4<$scalar>) -> bool {
                         let m  = m.map(|e| e.0);
-                        let qrp = m.qrp();
-    
-                        if let Some(m1) = qrp.try_inverse() {
+                        let col_piv_qr = m.col_piv_qr();
+
+                        if let Some(m1) = col_piv_qr.try_inverse() {
                             let id1 = &m  * &m1;
                             let id2 = &m1 * &m;
-    
+
                             id1.is_identity(1.0e-5) && id2.is_identity(1.0e-5)
                         }
                         else {
@@ -153,4 +159,3 @@ mod quickcheck_tests {
     gen_tests!(complex, RandComplex<f64>);
     gen_tests!(f64, RandScalar<f64>);
 }
-
diff --git a/tests/linalg/mod.rs b/tests/linalg/mod.rs
index 7fc01396..66a0c06a 100644
--- a/tests/linalg/mod.rs
+++ b/tests/linalg/mod.rs
@@ -1,6 +1,7 @@
 mod balancing;
 mod bidiagonal;
 mod cholesky;
+mod col_piv_qr;
 mod convolution;
 mod eigen;
 mod exp;

From 693e6d003534b5b4db8b9cb1fd610761baa934c6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= <developer@crozet.re>
Date: Thu, 25 Feb 2021 12:59:14 +0100
Subject: [PATCH 6/7] Run cargo fmt.

---
 src/linalg/mod.rs | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/linalg/mod.rs b/src/linalg/mod.rs
index 81cf1f38..df9ae7de 100644
--- a/src/linalg/mod.rs
+++ b/src/linalg/mod.rs
@@ -8,6 +8,7 @@ mod determinant;
 // TODO: this should not be needed. However, the exp uses
 // explicit float operations on `f32` and `f64`. We need to
 // get rid of these to allow exp to be used on a no-std context.
+mod col_piv_qr;
 mod decomposition;
 #[cfg(feature = "std")]
 mod exp;
@@ -19,7 +20,6 @@ mod inverse;
 mod lu;
 mod permutation_sequence;
 mod qr;
-mod col_piv_qr;
 mod schur;
 mod solve;
 mod svd;
@@ -32,6 +32,7 @@ mod symmetric_tridiagonal;
 
 pub use self::bidiagonal::*;
 pub use self::cholesky::*;
+pub use self::col_piv_qr::*;
 pub use self::convolution::*;
 #[cfg(feature = "std")]
 pub use self::exp::*;
@@ -40,7 +41,6 @@ pub use self::hessenberg::*;
 pub use self::lu::*;
 pub use self::permutation_sequence::*;
 pub use self::qr::*;
-pub use self::col_piv_qr::*;
 pub use self::schur::*;
 pub use self::svd::*;
 pub use self::symmetric_eigen::*;

From 598c217d759c77b8e92d03f899eb4d041d6dec86 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= <developer@crozet.re>
Date: Thu, 25 Feb 2021 13:28:42 +0100
Subject: [PATCH 7/7] Move the col_piv_qr method to the decomposition module.

---
 src/linalg/col_piv_qr.rs    | 13 -------------
 src/linalg/decomposition.rs | 21 +++++++++++++++++----
 2 files changed, 17 insertions(+), 17 deletions(-)

diff --git a/src/linalg/col_piv_qr.rs b/src/linalg/col_piv_qr.rs
index 3f8ecda1..4fa97482 100644
--- a/src/linalg/col_piv_qr.rs
+++ b/src/linalg/col_piv_qr.rs
@@ -331,16 +331,3 @@ where
         res * self.p.determinant()
     }
 }
-
-impl<N: ComplexField, R: DimMin<C>, C: Dim, S: Storage<N, R, C>> Matrix<N, R, C, S>
-where
-    DefaultAllocator: Allocator<N, R, C>
-        + Allocator<N, R>
-        + Allocator<N, DimMinimum<R, C>>
-        + Allocator<(usize, usize), DimMinimum<R, C>>,
-{
-    /// Computes the QR decomposition (with column pivoting) of this matrix.
-    pub fn col_piv_qr(self) -> ColPivQR<N, R, C> {
-        ColPivQR::new(self.into_owned())
-    }
-}
diff --git a/src/linalg/decomposition.rs b/src/linalg/decomposition.rs
index 67cc4c6a..7890748c 100644
--- a/src/linalg/decomposition.rs
+++ b/src/linalg/decomposition.rs
@@ -1,8 +1,8 @@
 use crate::storage::Storage;
 use crate::{
-    Allocator, Bidiagonal, Cholesky, ComplexField, DefaultAllocator, Dim, DimDiff, DimMin,
-    DimMinimum, DimSub, FullPivLU, Hessenberg, Matrix, Schur, SymmetricEigen, SymmetricTridiagonal,
-    LU, QR, SVD, U1,
+    Allocator, Bidiagonal, Cholesky, ColPivQR, ComplexField, DefaultAllocator, Dim, DimDiff,
+    DimMin, DimMinimum, DimSub, FullPivLU, Hessenberg, Matrix, Schur, SymmetricEigen,
+    SymmetricTridiagonal, LU, QR, SVD, U1,
 };
 
 /// # Rectangular matrix decomposition
@@ -13,8 +13,9 @@ use crate::{
 /// | Decomposition            | Factors             | Details |
 /// | -------------------------|---------------------|--------------|
 /// | QR                       | `Q * R`             | `Q` is an unitary matrix, and `R` is upper-triangular. |
+/// | QR with column pivoting  | `Q * R * P⁻¹`       | `Q` is an unitary matrix, and `R` is upper-triangular. `P` is a permutation matrix. |
 /// | LU with partial pivoting | `P⁻¹ * L * U`       | `L` is lower-triangular with a diagonal filled with `1` and `U` is upper-triangular. `P` is a permutation matrix. |
-/// | LU with full pivoting    | `P⁻¹ * L * U ~ Q⁻¹` | `L` is lower-triangular with a diagonal filled with `1` and `U` is upper-triangular. `P` and `Q` are permutation matrices. |
+/// | LU with full pivoting    | `P⁻¹ * L * U * Q⁻¹` | `L` is lower-triangular with a diagonal filled with `1` and `U` is upper-triangular. `P` and `Q` are permutation matrices. |
 /// | SVD                      | `U * Σ * Vᵀ`        | `U` and `V` are two orthogonal matrices and `Σ` is a diagonal matrix containing the singular values. |
 impl<N: ComplexField, R: Dim, C: Dim, S: Storage<N, R, C>> Matrix<N, R, C, S> {
     /// Computes the bidiagonalization using householder reflections.
@@ -60,6 +61,18 @@ impl<N: ComplexField, R: Dim, C: Dim, S: Storage<N, R, C>> Matrix<N, R, C, S> {
         QR::new(self.into_owned())
     }
 
+    /// Computes the QR decomposition (with column pivoting) of this matrix.
+    pub fn col_piv_qr(self) -> ColPivQR<N, R, C>
+    where
+        R: DimMin<C>,
+        DefaultAllocator: Allocator<N, R, C>
+            + Allocator<N, R>
+            + Allocator<N, DimMinimum<R, C>>
+            + Allocator<(usize, usize), DimMinimum<R, C>>,
+    {
+        ColPivQR::new(self.into_owned())
+    }
+
     /// Computes the Singular Value Decomposition using implicit shift.
     pub fn svd(self, compute_u: bool, compute_v: bool) -> SVD<N, R, C>
     where