Implement complete orthogonal decomposition in Eigen.

6 files changed, 633 insertions, 13 deletions

diff --git a/Eigen/QR b/Eigen/QR
index f74f365f1..25c781cc1 100644
--- a/Eigen/QR
+++ b/Eigen/QR
@@ -34,6 +34,7 @@
 #include "src/QR/HouseholderQR.h"
 #include "src/QR/FullPivHouseholderQR.h"
 #include "src/QR/ColPivHouseholderQR.h"
+#include "src/QR/CompleteOrthogonalDecomposition.h"
 #ifdef EIGEN_USE_LAPACKE
 #include "src/QR/HouseholderQR_MKL.h"
 #include "src/QR/ColPivHouseholderQR_MKL.h"
diff --git a/Eigen/src/Core/MatrixBase.h b/Eigen/src/Core/MatrixBase.h
index 3770ab257..1e66b4e1b 100644
--- a/Eigen/src/Core/MatrixBase.h
+++ b/Eigen/src/Core/MatrixBase.h
@@ -66,7 +66,7 @@ template<typename Derived> class MatrixBase
     using Base::MaxSizeAtCompileTime;
     using Base::IsVectorAtCompileTime;
     using Base::Flags;
-    
+
     using Base::derived;
     using Base::const_cast_derived;
     using Base::rows;
@@ -175,7 +175,7 @@ template<typename Derived> class MatrixBase
 #endif
 
     template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC 
+    EIGEN_DEVICE_FUNC
     const Product<Derived,OtherDerived,LazyProduct>
     lazyProduct(const MatrixBase<OtherDerived> &other) const;
 
@@ -214,7 +214,7 @@ template<typename Derived> class MatrixBase
     typedef Diagonal<Derived> DiagonalReturnType;
     EIGEN_DEVICE_FUNC
     DiagonalReturnType diagonal();
-    
+
     typedef typename internal::add_const<Diagonal<const Derived> >::type ConstDiagonalReturnType;
     EIGEN_DEVICE_FUNC
     ConstDiagonalReturnType diagonal() const;
@@ -222,14 +222,14 @@ template<typename Derived> class MatrixBase
     template<int Index> struct DiagonalIndexReturnType { typedef Diagonal<Derived,Index> Type; };
     template<int Index> struct ConstDiagonalIndexReturnType { typedef const Diagonal<const Derived,Index> Type; };
 
-    template<int Index> 
+    template<int Index>
     EIGEN_DEVICE_FUNC
     typename DiagonalIndexReturnType<Index>::Type diagonal();
 
     template<int Index>
     EIGEN_DEVICE_FUNC
     typename ConstDiagonalIndexReturnType<Index>::Type diagonal() const;
-    
+
     typedef Diagonal<Derived,DynamicIndex> DiagonalDynamicIndexReturnType;
     typedef typename internal::add_const<Diagonal<const Derived,DynamicIndex> >::type ConstDiagonalDynamicIndexReturnType;
 
@@ -251,7 +251,7 @@ template<typename Derived> class MatrixBase
     template<unsigned int UpLo> struct SelfAdjointViewReturnType { typedef SelfAdjointView<Derived, UpLo> Type; };
     template<unsigned int UpLo> struct ConstSelfAdjointViewReturnType { typedef const SelfAdjointView<const Derived, UpLo> Type; };
 
-    template<unsigned int UpLo> 
+    template<unsigned int UpLo>
     EIGEN_DEVICE_FUNC
     typename SelfAdjointViewReturnType<UpLo>::Type selfadjointView();
     template<unsigned int UpLo>
@@ -340,7 +340,7 @@ template<typename Derived> class MatrixBase
 
     EIGEN_DEVICE_FUNC
     inline const Inverse<Derived> inverse() const;
-    
+
     template<typename ResultType>
     inline void computeInverseAndDetWithCheck(
       ResultType& inverse,
@@ -366,6 +366,7 @@ template<typename Derived> class MatrixBase
     inline const HouseholderQR<PlainObject> householderQr() const;
     inline const ColPivHouseholderQR<PlainObject> colPivHouseholderQr() const;
     inline const FullPivHouseholderQR<PlainObject> fullPivHouseholderQr() const;
+    inline const CompleteOrthogonalDecomposition<PlainObject> completeOrthogonalDecomposition() const;
 
 /////////// Eigenvalues module ///////////
 
@@ -394,23 +395,23 @@ template<typename Derived> class MatrixBase
     inline PlainObject
 #endif
     cross(const MatrixBase<OtherDerived>& other) const;
-    
+
     template<typename OtherDerived>
     EIGEN_DEVICE_FUNC
     inline PlainObject cross3(const MatrixBase<OtherDerived>& other) const;
-    
+
     EIGEN_DEVICE_FUNC
     inline PlainObject unitOrthogonal(void) const;
-    
+
     inline Matrix<Scalar,3,1> eulerAngles(Index a0, Index a1, Index a2) const;
-    
+
     inline ScalarMultipleReturnType operator*(const UniformScaling<Scalar>& s) const;
     // put this as separate enum value to work around possible GCC 4.3 bug (?)
     enum { HomogeneousReturnTypeDirection = ColsAtCompileTime==1&&RowsAtCompileTime==1 ? ((internal::traits<Derived>::Flags&RowMajorBit)==RowMajorBit ? Horizontal : Vertical)
                                           : ColsAtCompileTime==1 ? Vertical : Horizontal };
     typedef Homogeneous<Derived, HomogeneousReturnTypeDirection> HomogeneousReturnType;
     inline HomogeneousReturnType homogeneous() const;
-    
+
     enum {
       SizeMinusOne = SizeAtCompileTime==Dynamic ? Dynamic : SizeAtCompileTime-1
     };
diff --git a/Eigen/src/Core/util/ForwardDeclarations.h b/Eigen/src/Core/util/ForwardDeclarations.h
index 31c7088e7..f09632375 100644
--- a/Eigen/src/Core/util/ForwardDeclarations.h
+++ b/Eigen/src/Core/util/ForwardDeclarations.h
@@ -95,7 +95,7 @@ template<typename Decomposition, typename Rhstype>        class Solve;
 template<typename XprType>                                class Inverse;
 
 template<typename Lhs, typename Rhs, int Option = DefaultProduct> class Product;
-         
+
 template<typename Derived> class DiagonalBase;
 template<typename _DiagonalVectorType> class DiagonalWrapper;
 template<typename _Scalar, int SizeAtCompileTime, int MaxSizeAtCompileTime=SizeAtCompileTime> class DiagonalMatrix;
@@ -248,6 +248,7 @@ template<typename MatrixType> struct inverse_impl;
 template<typename MatrixType> class HouseholderQR;
 template<typename MatrixType> class ColPivHouseholderQR;
 template<typename MatrixType> class FullPivHouseholderQR;
+template<typename MatrixType> class CompleteOrthogonalDecomposition;
 template<typename MatrixType, int QRPreconditioner = ColPivHouseholderQRPreconditioner> class JacobiSVD;
 template<typename MatrixType> class BDCSVD;
 template<typename MatrixType, int UpLo = Lower> class LLT;
diff --git a/Eigen/src/QR/ColPivHouseholderQR.h b/Eigen/src/QR/ColPivHouseholderQR.h
index efeb1f438..7c559f952 100644
--- a/Eigen/src/QR/ColPivHouseholderQR.h
+++ b/Eigen/src/QR/ColPivHouseholderQR.h
@@ -404,6 +404,8 @@ template<typename _MatrixType> class ColPivHouseholderQR
 
   protected:
 
+    friend class CompleteOrthogonalDecomposition<MatrixType>;
+
     static void check_template_parameters()
     {
       EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
diff --git a/Eigen/src/QR/CompleteOrthogonalDecomposition.h b/Eigen/src/QR/CompleteOrthogonalDecomposition.h
new file mode 100644
index 000000000..b81bb7433
--- /dev/null
+++ b/Eigen/src/QR/CompleteOrthogonalDecomposition.h
@@ -0,0 +1,538 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2016 Rasmus Munk Larsen <rmlarsen@google.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_COMPLETEORTHOGONALDECOMPOSITION_H
+#define EIGEN_COMPLETEORTHOGONALDECOMPOSITION_H
+
+namespace Eigen {
+
+namespace internal {
+template <typename _MatrixType>
+struct traits<CompleteOrthogonalDecomposition<_MatrixType> >
+    : traits<_MatrixType> {
+  enum { Flags = 0 };
+};
+
+}  // end namespace internal
+
+/** \ingroup QR_Module
+  *
+  * \class CompleteOrthogonalDecomposition
+  *
+  * \brief Complete orthogonal decomposition (COD) of a matrix.
+  *
+  * \param MatrixType the type of the matrix of which we are computing the COD.
+  *
+  * This class performs a rank-revealing complete ortogonal decomposition of a
+  * matrix  \b A into matrices \b P, \b Q, \b T, and \b Z such that
+  * \f[
+  *  \mathbf{A} \, \mathbf{P} = \mathbf{Q} \, \begin{matrix} \mathbf{T} &
+  *  \mathbf{0} \\ \mathbf{0} & \mathbf{0} \end{matrix} \, \mathbf{Z}
+  * \f]
+  * by using Householder transformations. Here, \b P is a permutation matrix,
+  * \b Q and \b Z are unitary matrices and \b T an upper triangular matrix of
+  * size rank-by-rank. \b A may be rank deficient.
+  *
+  * \sa MatrixBase::completeOrthogonalDecomposition()
+  */
+template <typename _MatrixType>
+class CompleteOrthogonalDecomposition {
+ public:
+  typedef _MatrixType MatrixType;
+  enum {
+    RowsAtCompileTime = MatrixType::RowsAtCompileTime,
+    ColsAtCompileTime = MatrixType::ColsAtCompileTime,
+    Options = MatrixType::Options,
+    MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+  };
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename MatrixType::RealScalar RealScalar;
+  typedef typename MatrixType::StorageIndex StorageIndex;
+  typedef Matrix<Scalar, RowsAtCompileTime, RowsAtCompileTime, Options,
+                 MaxRowsAtCompileTime, MaxRowsAtCompileTime>
+      MatrixQType;
+  typedef typename internal::plain_diag_type<MatrixType>::type HCoeffsType;
+  typedef PermutationMatrix<ColsAtCompileTime, MaxColsAtCompileTime>
+      PermutationType;
+  typedef typename internal::plain_row_type<MatrixType, Index>::type
+      IntRowVectorType;
+  typedef typename internal::plain_row_type<MatrixType>::type RowVectorType;
+  typedef typename internal::plain_row_type<MatrixType, RealScalar>::type
+      RealRowVectorType;
+  typedef HouseholderSequence<
+      MatrixType, typename internal::remove_all<
+                      typename HCoeffsType::ConjugateReturnType>::type>
+      HouseholderSequenceType;
+
+ private:
+  typedef typename PermutationType::Index PermIndexType;
+
+ public:
+  /**
+   * \brief Default Constructor.
+   *
+   * The default constructor is useful in cases in which the user intends to
+   * perform decompositions via
+   * \c CompleteOrthogonalDecomposition::compute(const* MatrixType&).
+   */
+  CompleteOrthogonalDecomposition() : m_cpqr(), m_zCoeffs(), m_temp() {}
+
+  /** \brief Default Constructor with memory preallocation
+   *
+   * Like the default constructor but with preallocation of the internal data
+   * according to the specified problem \a size.
+   * \sa CompleteOrthogonalDecomposition()
+   */
+  CompleteOrthogonalDecomposition(Index rows, Index cols)
+      : m_cpqr(rows, cols), m_zCoeffs((std::min)(rows, cols)), m_temp(cols) {}
+
+  /** \brief Constructs a complete orthogonal decomposition from a given
+   * matrix.
+   *
+   * This constructor computes the complete orthogonal decomposition of the
+   * matrix \a matrix by calling the method compute(). The default
+   * threshold for rank determination will be used. It is a short cut for:
+   *
+   * \code
+   * CompleteOrthogonalDecomposition<MatrixType> cod(matrix.rows(),
+   *                                                 matrix.cols());
+   * cod.setThreshold(Default);
+   * cod.compute(matrix);
+   * \endcode
+   *
+   * \sa compute()
+   */
+  template <typename InputType>
+  explicit CompleteOrthogonalDecomposition(const EigenBase<InputType>& matrix)
+      : m_cpqr(matrix.rows(), matrix.cols()),
+        m_zCoeffs((std::min)(matrix.rows(), matrix.cols())),
+        m_temp(matrix.cols()) {
+    compute(matrix.derived());
+  }
+
+  /** This method computes the minimum-norm solution X to a least squares
+   * problem \f[\mathrm{minimize} ||A X - B|| \f], where \b A is the matrix of
+   * which \c *this is the complete orthogonal decomposition.
+   *
+   * \param B the right-hand sides of the problem to solve.
+   *
+   * \returns a solution.
+   *
+   */
+  template <typename Rhs>
+  inline const Solve<CompleteOrthogonalDecomposition, Rhs> solve(
+      const MatrixBase<Rhs>& b) const {
+    eigen_assert(m_cpqr.m_isInitialized &&
+                 "CompleteOrthogonalDecomposition is not initialized.");
+    return Solve<CompleteOrthogonalDecomposition, Rhs>(*this, b.derived());
+  }
+
+  HouseholderSequenceType householderQ(void) const;
+  HouseholderSequenceType matrixQ(void) const { return m_cpqr.householderQ(); }
+
+  /** Overwrites \b rhs with \f$ \mathbf{Z}^* * \mathbf{rhs} \f$.
+   */
+  template <typename Rhs>
+  void applyZAdjointOnTheLeftInPlace(Rhs& rhs) const;
+
+  /** \returns the matrix \b Z.
+   */
+  MatrixType matrixZ() const {
+    MatrixType Z = MatrixType::Identity(m_cpqr.cols(), m_cpqr.cols());
+    applyZAdjointOnTheLeftInPlace(Z);
+    return Z.adjoint();
+  }
+
+  /** \returns a reference to the matrix where the complete orthogonal
+   * decomposition is stored
+   */
+  const MatrixType& matrixQTZ() const { return m_cpqr.matrixQR(); }
+
+  /** \returns a reference to the matrix where the complete orthogonal
+   * decomposition is stored.
+   * \warning The strict lower part and \code cols() - rank() \endcode right
+   * columns of this matrix contains internal values.
+   * Only the upper triangular part should be referenced. To get it, use
+   * \code matrixT().template triangularView<Upper>() \endcode
+   * For rank-deficient matrices, use
+   * \code
+   * matrixR().topLeftCorner(rank(), rank()).template triangularView<Upper>()
+   * \endcode
+   */
+  const MatrixType& matrixT() const { return m_cpqr.matrixQR(); }
+
+  template <typename InputType>
+  CompleteOrthogonalDecomposition& compute(const EigenBase<InputType>& matrix);
+
+  /** \returns a const reference to the column permutation matrix */
+  const PermutationType& colsPermutation() const {
+    return m_cpqr.colsPermutation();
+  }
+
+  /** \returns the absolute value of the determinant of the matrix of which
+   * *this is the complete orthogonal decomposition. It has only linear
+   * complexity (that is, O(n) where n is the dimension of the square matrix)
+   * as the complete orthogonal decomposition has already been computed.
+   *
+   * \note This is only for square matrices.
+   *
+   * \warning a determinant can be very big or small, so for matrices
+   * of large enough dimension, there is a risk of overflow/underflow.
+   * One way to work around that is to use logAbsDeterminant() instead.
+   *
+   * \sa logAbsDeterminant(), MatrixBase::determinant()
+   */
+  typename MatrixType::RealScalar absDeterminant() const;
+
+  /** \returns the natural log of the absolute value of the determinant of the
+   * matrix of which *this is the complete orthogonal decomposition. It has
+   * only linear complexity (that is, O(n) where n is the dimension of the
+   * square matrix) as the complete orthogonal decomposition has already been
+   * computed.
+   *
+   * \note This is only for square matrices.
+   *
+   * \note This method is useful to work around the risk of overflow/underflow
+   * that's inherent to determinant computation.
+   *
+   * \sa absDeterminant(), MatrixBase::determinant()
+   */
+  typename MatrixType::RealScalar logAbsDeterminant() const;
+
+  /** \returns the rank of the matrix of which *this is the complete orthogonal
+   * decomposition.
+   *
+   * \note This method has to determine which pivots should be considered
+   * nonzero. For that, it uses the threshold value that you can control by
+   * calling setThreshold(const RealScalar&).
+   */
+  inline Index rank() const { return m_cpqr.rank(); }
+
+  /** \returns the dimension of the kernel of the matrix of which *this is the
+   * complete orthogonal decomposition.
+   *
+   * \note This method has to determine which pivots should be considered
+   * nonzero. For that, it uses the threshold value that you can control by
+   * calling setThreshold(const RealScalar&).
+   */
+  inline Index dimensionOfKernel() const { return m_cpqr.dimensionOfKernel(); }
+
+  /** \returns true if the matrix of which *this is the decomposition represents
+   * an injective linear map, i.e. has trivial kernel; false otherwise.
+   *
+   * \note This method has to determine which pivots should be considered
+   * nonzero. For that, it uses the threshold value that you can control by
+   * calling setThreshold(const RealScalar&).
+   */
+  inline bool isInjective() const { return m_cpqr.isInjective(); }
+
+  /** \returns true if the matrix of which *this is the decomposition represents
+   * a surjective linear map; false otherwise.
+   *
+   * \note This method has to determine which pivots should be considered
+   * nonzero. For that, it uses the threshold value that you can control by
+   * calling setThreshold(const RealScalar&).
+   */
+  inline bool isSurjective() const { return m_cpqr.isSurjective(); }
+
+  /** \returns true if the matrix of which *this is the complete orthogonal
+   * decomposition is invertible.
+   *
+   * \note This method has to determine which pivots should be considered
+   * nonzero. For that, it uses the threshold value that you can control by
+   * calling setThreshold(const RealScalar&).
+   */
+  inline bool isInvertible() const { return m_cpqr.isInvertible(); }
+
+  /** \returns the inverse of the matrix of which *this is the complete
+   * orthogonal decomposition.
+   *
+   * \note If this matrix is not invertible, the returned matrix has undefined
+   * coefficients. Use isInvertible() to first determine whether this matrix is
+   * invertible.
+   */
+
+  // TODO(rmlarsen): Add method for pseudo-inverse.
+  // inline const
+  // internal::solve_retval<CompleteOrthogonalDecomposition, typename
+  // MatrixType::IdentityReturnType>
+  // inverse() const
+  // {
+  //   eigen_assert(m_isInitialized && "CompleteOrthogonalDecomposition is not
+  //   initialized.");
+  //   return internal::solve_retval<CompleteOrthogonalDecomposition,typename
+  //   MatrixType::IdentityReturnType>
+  //            (*this, MatrixType::Identity(m_cpqr.rows(), m_cpqr.cols()));
+  // }
+
+  inline Index rows() const { return m_cpqr.rows(); }
+  inline Index cols() const { return m_cpqr.cols(); }
+
+  /** \returns a const reference to the vector of Householder coefficients used
+   * to represent the factor \c Q.
+   *
+   * For advanced uses only.
+   */
+  inline const HCoeffsType& hCoeffs() const { return m_cpqr.hCoeffs(); }
+
+  /** \returns a const reference to the vector of Householder coefficients
+   * used to represent the factor \c Z.
+   *
+   * For advanced uses only.
+   */
+  const HCoeffsType& zCoeffs() const { return m_zCoeffs; }
+
+  /** Allows to prescribe a threshold to be used by certain methods, such as
+   * rank(), who need to determine when pivots are to be considered nonzero.
+   * Most be called before calling compute().
+   *
+   * When it needs to get the threshold value, Eigen calls threshold(). By
+   * default, this uses a formula to automatically determine a reasonable
+   * threshold. Once you have called the present method
+   * setThreshold(const RealScalar&), your value is used instead.
+   *
+   * \param threshold The new value to use as the threshold.
+   *
+   * A pivot will be considered nonzero if its absolute value is strictly
+   * greater than
+   *  \f$ \vert pivot \vert \leqslant threshold \times \vert maxpivot \vert \f$
+   * where maxpivot is the biggest pivot.
+   *
+   * If you want to come back to the default behavior, call
+   * setThreshold(Default_t)
+   */
+  CompleteOrthogonalDecomposition& setThreshold(const RealScalar& threshold) {
+    m_cpqr.setThreshold(threshold);
+    return *this;
+  }
+
+  /** Allows to come back to the default behavior, letting Eigen use its default
+   * formula for determining the threshold.
+   *
+   * You should pass the special object Eigen::Default as parameter here.
+   * \code qr.setThreshold(Eigen::Default); \endcode
+   *
+   * See the documentation of setThreshold(const RealScalar&).
+   */
+  CompleteOrthogonalDecomposition& setThreshold(Default_t) {
+    m_cpqr.setThreshold(Default);
+    return *this;
+  }
+
+  /** Returns the threshold that will be used by certain methods such as rank().
+   *
+   * See the documentation of setThreshold(const RealScalar&).
+   */
+  RealScalar threshold() const { return m_cpqr.threshold(); }
+
+  /** \returns the number of nonzero pivots in the complete orthogonal
+   * decomposition.
+   * Here nonzero is meant in the exact sense, not in a fuzzy sense.
+   * So that notion isn't really intrinsically interesting, but it is
+   * still useful when implementing algorithms.
+   *
+   * \sa rank()
+   */
+  inline Index nonzeroPivots() const { return m_cpqr.nonzeroPivots(); }
+
+  /** \returns the absolute value of the biggest pivot, i.e. the biggest
+   *          diagonal coefficient of R.
+   */
+  inline RealScalar maxPivot() const { return m_cpqr.maxPivot(); }
+
+  /** \brief Reports whether the complete orthogonal decomposition was
+   * succesful.
+   *
+   * \note This function always returns \c Success. It is provided for
+   * compatibility
+   * with other factorization routines.
+   * \returns \c Success
+   */
+  ComputationInfo info() const {
+    eigen_assert(m_cpqr.m_isInitialized && "Decomposition is not initialized.");
+    return Success;
+  }
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  template <typename RhsType, typename DstType>
+  EIGEN_DEVICE_FUNC void _solve_impl(const RhsType& rhs, DstType& dst) const;
+#endif
+
+ protected:
+  static void check_template_parameters() {
+    EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
+  }
+
+  ColPivHouseholderQR<MatrixType> m_cpqr;
+  HCoeffsType m_zCoeffs;
+  RowVectorType m_temp;
+};
+
+template <typename MatrixType>
+typename MatrixType::RealScalar
+CompleteOrthogonalDecomposition<MatrixType>::absDeterminant() const {
+  return m_cpqr.absDeterminant();
+}
+
+template <typename MatrixType>
+typename MatrixType::RealScalar
+CompleteOrthogonalDecomposition<MatrixType>::logAbsDeterminant() const {
+  return m_cpqr.logAbsDeterminant();
+}
+
+/** Performs the complete orthogonal decomposition of the given matrix \a
+ * matrix. The result of the factorization is stored into \c *this, and a
+ * reference to \c *this is returned.
+ *
+ * \sa class CompleteOrthogonalDecomposition,
+ * CompleteOrthogonalDecomposition(const MatrixType&)
+ */
+template <typename MatrixType>
+template <typename InputType>
+CompleteOrthogonalDecomposition<MatrixType>& CompleteOrthogonalDecomposition<
+    MatrixType>::compute(const EigenBase<InputType>& matrix) {
+  check_template_parameters();
+
+  // the column permutation is stored as int indices, so just to be sure:
+  eigen_assert(matrix.cols() <= NumTraits<int>::highest());
+
+  // Compute the column pivoted QR factorization A P = Q R.
+  m_cpqr.compute(matrix);
+
+  const Index rank = m_cpqr.rank();
+  const Index cols = matrix.cols();
+  if (rank < cols) {
+    // We have reduced the (permuted) matrix to the form
+    //   [R11 R12]
+    //   [ 0  R22]
+    // where R11 is r-by-r (r = rank) upper triangular, R12 is
+    // r-by-(n-r), and R22 is empty or the norm of R22 is negligible.
+    // We now compute the complete orthogonal decomposition by applying
+    // Householder transformations from the right to the upper trapezoidal
+    // matrix X = [R11 R12] to zero out R12 and obtain the factorization
+    // [R11 R12] = [T11 0] * Z, where T11 is r-by-r upper triangular and
+    // Z = Z(0) * Z(1) ... Z(r-1) is an n-by-n orthogonal matrix.
+    // We store the data representing Z in R12 and m_zCoeffs.
+    for (Index k = rank - 1; k >= 0; --k) {
+      if (k != rank - 1) {
+        // Given the API for Householder reflectors, it is more convenient if
+        // we swap the leading parts of columns k and r-1 (zero-based) to form
+        // the matrix X_k = [X(0:k, k), X(0:k, r:n)]
+        m_cpqr.m_qr.col(k).head(k + 1).swap(
+            m_cpqr.m_qr.col(rank - 1).head(k + 1));
+      }
+      // Construct Householder reflector Z(k) to zero out the last row of X_k,
+      // i.e. choose Z(k) such that
+      // [X(k, k), X(k, r:n)] * Z(k) = [beta, 0, .., 0].
+      RealScalar beta;
+      m_cpqr.m_qr.row(k)
+          .tail(cols - rank + 1)
+          .makeHouseholderInPlace(m_zCoeffs(k), beta);
+      m_cpqr.m_qr(k, rank - 1) = beta;
+      if (k > 0) {
+        // Apply Z(k) to the first k rows of X_k
+        m_cpqr.m_qr.topRightCorner(k, cols - rank + 1)
+            .applyHouseholderOnTheRight(
+                m_cpqr.m_qr.row(k).tail(cols - rank).transpose(), m_zCoeffs(k),
+                &m_temp(0));
+      }
+      if (k != rank - 1) {
+        // Swap X(0:k,k) back to its proper location.
+        m_cpqr.m_qr.col(k).head(k + 1).swap(
+            m_cpqr.m_qr.col(rank - 1).head(k + 1));
+      }
+    }
+  }
+  return *this;
+}
+
+template <typename MatrixType>
+template <typename Rhs>
+void CompleteOrthogonalDecomposition<MatrixType>::applyZAdjointOnTheLeftInPlace(
+    Rhs& rhs) const {
+  const Index cols = this->cols();
+  const Index nrhs = rhs.cols();
+  const Index rank = this->rank();
+  Matrix<typename MatrixType::Scalar, Dynamic, 1> temp((std::max)(cols, nrhs));
+  for (Index k = 0; k < rank; ++k) {
+    if (k != rank - 1) {
+      rhs.row(k).swap(rhs.row(rank - 1));
+    }
+    rhs.middleRows(rank - 1, cols - rank + 1)
+        .applyHouseholderOnTheLeft(
+            matrixQTZ().row(k).tail(cols - rank).adjoint(), zCoeffs()(k),
+            &temp(0));
+    if (k != rank - 1) {
+      rhs.row(k).swap(rhs.row(rank - 1));
+    }
+  }
+}
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+template <typename _MatrixType>
+template <typename RhsType, typename DstType>
+void CompleteOrthogonalDecomposition<_MatrixType>::_solve_impl(
+    const RhsType& rhs, DstType& dst) const {
+  eigen_assert(rhs().rows() == this->rows());
+
+  const Index rank = this->rank();
+  if (rank == 0) {
+    dst.setZero();
+    return;
+  }
+
+  // Compute c = Q^* * rhs
+  // Note that the matrix Q = H_0^* H_1^*... so its inverse is
+  // Q^* = (H_0 H_1 ...)^T
+  typename RhsType::PlainObject c(rhs());
+  c.applyOnTheLeft(
+      householderSequence(matrixQTZ(), hCoeffs()).setLength(rank).transpose());
+
+  // Solve T z = c(1:rank, :)
+  dst.topRows(rank) = matrixT()
+                          .topLeftCorner(rank, rank)
+                          .template triangularView<Upper>()
+                          .solve(c.topRows(rank));
+
+  const Index cols = this->cols();
+  if (rank < cols) {
+    // Compute y = Z^* * [ z ]
+    //                   [ 0 ]
+    dst.bottomRows(cols - rank).setZero();
+    applyZAdjointOnTheLeftInPlace(dst);
+  }
+
+  // Undo permutation to get x = P^{-1} * y.
+  dst = colsPermutation() * dst;
+}
+#endif
+
+/** \returns the matrix Q as a sequence of householder transformations */
+template <typename MatrixType>
+typename CompleteOrthogonalDecomposition<MatrixType>::HouseholderSequenceType
+CompleteOrthogonalDecomposition<MatrixType>::householderQ() const {
+  return m_cpqr.householderQ();
+}
+
+#ifndef __CUDACC__
+/** \return the complete orthogonal decomposition of \c *this.
+  *
+  * \sa class CompleteOrthogonalDecomposition
+  */
+template <typename Derived>
+const CompleteOrthogonalDecomposition<typename MatrixBase<Derived>::PlainObject>
+MatrixBase<Derived>::completeOrthogonalDecomposition() const {
+  return CompleteOrthogonalDecomposition<PlainObject>(eval());
+}
+#endif  // __CUDACC__
+
+}  // end namespace Eigen
+
+#endif  // EIGEN_COMPLETEORTHOGONALDECOMPOSITION_H
diff --git a/test/qr_colpivoting.cpp b/test/qr_colpivoting.cpp
index 9c989823e..648250af6 100644
--- a/test/qr_colpivoting.cpp
+++ b/test/qr_colpivoting.cpp
@@ -10,6 +10,83 @@
 
 #include "main.h"
 #include <Eigen/QR>
+#include <Eigen/SVD>
+
+template <typename MatrixType>
+void cod() {
+  typedef typename MatrixType::Index Index;
+
+  Index rows = internal::random<Index>(2, EIGEN_TEST_MAX_SIZE);
+  Index cols = internal::random<Index>(2, EIGEN_TEST_MAX_SIZE);
+  Index cols2 = internal::random<Index>(2, EIGEN_TEST_MAX_SIZE);
+  Index rank = internal::random<Index>(1, (std::min)(rows, cols) - 1);
+
+  typedef typename MatrixType::Scalar Scalar;
+  typedef Matrix<Scalar, MatrixType::RowsAtCompileTime,
+                 MatrixType::RowsAtCompileTime>
+      MatrixQType;
+  MatrixType matrix;
+  createRandomPIMatrixOfRank(rank, rows, cols, matrix);
+  CompleteOrthogonalDecomposition<MatrixType> cod(matrix);
+  VERIFY(rank == cod.rank());
+  VERIFY(cols - cod.rank() == cod.dimensionOfKernel());
+  VERIFY(!cod.isInjective());
+  VERIFY(!cod.isInvertible());
+  VERIFY(!cod.isSurjective());
+
+  MatrixQType q = cod.householderQ();
+  VERIFY_IS_UNITARY(q);
+
+  MatrixType z = cod.matrixZ();
+  VERIFY_IS_UNITARY(z);
+
+  MatrixType t;
+  t.setZero(rows, cols);
+  t.topLeftCorner(rank, rank) =
+      cod.matrixT().topLeftCorner(rank, rank).template triangularView<Upper>();
+
+  MatrixType c = q * t * z * cod.colsPermutation().inverse();
+  VERIFY_IS_APPROX(matrix, c);
+
+  MatrixType exact_solution = MatrixType::Random(cols, cols2);
+  MatrixType rhs = matrix * exact_solution;
+  MatrixType cod_solution = cod.solve(rhs);
+  VERIFY_IS_APPROX(rhs, matrix * cod_solution);
+
+  // Verify that we get the same minimum-norm solution as the SVD.
+  JacobiSVD<MatrixType> svd(matrix, ComputeThinU | ComputeThinV);
+  MatrixType svd_solution = svd.solve(rhs);
+  VERIFY_IS_APPROX(cod_solution, svd_solution);
+}
+
+template <typename MatrixType, int Cols2>
+void cod_fixedsize() {
+  enum {
+    Rows = MatrixType::RowsAtCompileTime,
+    Cols = MatrixType::ColsAtCompileTime
+  };
+  typedef typename MatrixType::Scalar Scalar;
+  int rank = internal::random<int>(1, (std::min)(int(Rows), int(Cols)) - 1);
+  Matrix<Scalar, Rows, Cols> matrix;
+  createRandomPIMatrixOfRank(rank, Rows, Cols, matrix);
+  CompleteOrthogonalDecomposition<Matrix<Scalar, Rows, Cols> > cod(matrix);
+  VERIFY(rank == cod.rank());
+  VERIFY(Cols - cod.rank() == cod.dimensionOfKernel());
+  VERIFY(cod.isInjective() == (rank == Rows));
+  VERIFY(cod.isSurjective() == (rank == Cols));
+  VERIFY(cod.isInvertible() == (cod.isInjective() && cod.isSurjective()));
+
+  Matrix<Scalar, Cols, Cols2> exact_solution;
+  exact_solution.setRandom(Cols, Cols2);
+  Matrix<Scalar, Rows, Cols2> rhs = matrix * exact_solution;
+  Matrix<Scalar, Cols, Cols2> cod_solution = cod.solve(rhs);
+  VERIFY_IS_APPROX(rhs, matrix * cod_solution);
+
+  // Verify that we get the same minimum-norm solution as the SVD.
+  JacobiSVD<MatrixType> svd(matrix, ComputeFullU | ComputeFullV);
+  Matrix<Scalar, Cols, Cols2> svd_solution = svd.solve(rhs);
+  VERIFY_IS_APPROX(cod_solution, svd_solution);
+}
 
 template<typename MatrixType> void qr()
 {