Re: [eigen] GivensQR

[Andrea Arteaga <yo.eres@xxxxxxxxx>]

> yes, if you can
> write comments, it's probably useful ;) 

Voilà.

// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Andrea Arteaga <yo.eres@xxxxxxxxx>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#ifndef EIGEN_GIVENS_QR_H
#define EIGEN_GIVENS_QR_H

/** \ingroup QR_Module
  * \nonstableyet
  *
  * \class GivensQR
  *
  * \brief Givens QR decomposition of a matrix
  *
  * \param MatrixType the type of the matrix of which we are computing the QR decomposition
  *
  * This class performs a QR decomposition using Givens rotations.
  *
  */
template <typename MatrixType>
class GivensQR
{
  private:
    enum {
      Rows = MatrixType::RowsAtCompileTime,
      Cols = MatrixType::ColsAtCompileTime,
      isDynamic = (Rows == Dynamic || Cols == Dynamic) ? 1 : 0,

      //Number of Givens coefficients which are computed and stored instead of Q
      CNum = isDynamic == 1 ? Dynamic : (Rows > Cols) ? (Cols*Rows - (Cols*(Cols+1))/2) : ((Rows-1)*Rows/2)
    };

  public:
    typedef typename NumTraits<typename MatrixType::Scalar>::FloatingPoint Scalar;
    typedef typename NumTraits<Scalar>::Real RealScalar;

    typedef Matrix<Scalar, Rows, Rows> MatrixQType;
    typedef Matrix<Scalar, Rows, Cols> MatrixRType;
    typedef Matrix<Scalar, Rows, Cols> MatrixQReducedType;
    typedef Matrix<Scalar, Cols, Cols> MatrixRReducedType;

  private:
    // The whole system of coefficients is hidden to the user
    typedef Matrix<Scalar, 2, CNum> MatrixCType;

  public:
/** Constructor: initialize the decomposition, but do not perform any computation.
  * Computation is done only if strictly needed.
  *
  * \param matrix The matrix of which *this is the QR decomposition.
  *
  */
    GivensQR(const MatrixType& matrix) : m_(matrix) {
      m_QisUptodate = false;
      m_RisUptodate = false;
      m_CisUptodate = false;
      if(isDynamic) {
        rows = m_.rows();
        cols = m_.cols();
        cnum = (rows > cols) ? (cols*rows - (cols*(cols+1))/2) : ((rows*(rows-1))/2);
        m_Q.resize(rows, rows);
        m_R.resize(rows, cols);
        m_C.resize(2, cnum);
      } else {
        rows = Rows;
        cols = Cols;
        cnum = CNum;
      }

      //The class is ready to start the computation
    }

/** This method computes both Q and R matrix. After a call to compute() no more computation is needed in
  * order to return Q or R matrices.
  *
  */
    //The public compute is only a wrapper
    inline void compute() const {
      compute_QCR();
    }

/** This method return the unitary matrix (Q) of the QR decomposition.
  *
  * \note If you need both Q and R matrix, request the unitary before the upper triangular.
  */
    inline const MatrixQType& matrixQ() const {
      if (m_QisUptodate)
        return m_Q;
      compute_QCR();
      return m_Q;
    }

/** This method return the upper triangular matrix (R) of the QR decomposition.
  */
    inline const MatrixRType& matrixR() const {
      if (m_RisUptodate)
        return m_R;
      compute_CR();
      return m_R;
    };

/** This method finds a solution x to the equation Ax=b, where A is the matrix of which
  * *this is the QR decomposition, if any exists.
  *
  * \param b the right-hand-side of the equation to solve.
  *
  * \param result a pointer to the vector/matrix in which to store the solution.
  *          Resized if necessary, so that result->rows()==A.cols() and result->cols()==b.cols().
  *
  * \note b can be both a vector or a matrix
  *
  * \sa MatrixBase::solveTriangular(), kernel(), computeKernel(), inverse(), computeInverse()
*/
    template<typename OtherDerived, typename ResultType>
    bool solve(const MatrixBase<OtherDerived>&, ResultType*) const;

  private:
    //The matrix is stored as reference.
    //Problem: what if the user update the matrix?
    const MatrixType& m_;

    //Run-time values equivalent to the compile-time ones.
    int rows, cols, cnum;

    mutable MatrixQType m_Q;
    mutable MatrixQReducedType m_Qr;
    mutable bool m_QisUptodate;

    mutable MatrixRType m_R;
    mutable MatrixRReducedType m_Rr;
    mutable bool m_RisUptodate;

    mutable MatrixCType m_C;
    mutable bool m_CisUptodate;

  private:
    //Computes coefficients and R
    void compute_CR() const;

    //Computes coefficients, R and Q
    void compute_QCR() const;
};

#ifndef EIGEN_HIDE_HEAVY_CODE

template <typename MatrixType>
void GivensQR<MatrixType>::compute_QCR() const
{
  Scalar tmp;
  RealScalar r;

  // Number of cols to traverse
  const int q_cols = cols < rows ? cols : rows-1;

  // Begin: set Q to the identity and R to the matrix
  // Both matrices will be updated
  m_Q.setIdentity();
  m_R = m_.template cast<typename MatrixRType::Scalar>();

  // j = current rotation's number (or ID), which represents the column on m_C
  // in which the current coefficients are stored.
  int j = 0;
  for(int k1 = 0; k1 < q_cols; ++k1) { //Iteration over the columns
    for (int k2 = rows - 1; k2 > k1; --k2) { //Inverse iteration over the rows

      //Compute the coefficients o1 and o2, which are directly stored into m_C (i.e.: there is no copy)
      const Scalar& b = m_R.coeffRef(k2, k1);
      const Scalar& a = m_R.coeffRef(k1, k1);
      Scalar& o1 = m_C.coeffRef(0, j);
      Scalar& o2 = m_C.coeffRef(1, j++);
      r = ei_sqrt(ei_real(ei_conj(a)*a + ei_conj(b)*b));
      o1 = a/r;  // FIXME:
      o2 = b/r;  // what if r == 0 <=> (a==0 && b==0) ?

      // Update R (row-wise)
      for (int i = k1; i < cols; ++i){
        tmp = m_R.coeff(k1, i);
        m_R.coeffRef(k1, i) = ei_conj(o1)*m_R.coeff(k1, i) + ei_conj(o2)*m_R.coeff(k2, i);
        m_R.coeffRef(k2, i) = o1*m_R.coeff(k2, i) - o2*tmp;
      }

      // Update Q (column-wise)
      for (int i = 0; i < rows; ++i){
        tmp = m_Q.coeff(i, k1);
        m_Q.coeffRef(i, k1) = o1*m_Q.coeff(i, k1) + o2*m_Q.coeff(i, k2);
        m_Q.coeffRef(i, k2) = ei_conj(o1)*m_Q.coeff(i, k2) - ei_conj(o2)*tmp;
      }
    }
  }

  //Compute and store the reduced matrices
  m_Qr = m_Q.block(0, 0, rows, cols);
  m_Rr = m_R.block(0, 0, cols, cols);

  m_QisUptodate = true;
  m_RisUptodate = true;
  m_CisUptodate = true;
}


template <typename MatrixType>
void GivensQR<MatrixType>::compute_CR() const
{
  //Same as over, without update of Q.

  Scalar tmp;
  RealScalar r;
  const int q_cols = cols < rows ? cols : rows-1;

  m_R = m_.template cast<typename MatrixRType::Scalar>();

  int j = 0;
  for(int k1 = 0; k1 < q_cols; ++k1) {
    for (int k2 = rows - 1; k2 > k1; --k2) {
      const Scalar& b = m_R.coeffRef(k2, k1);
      const Scalar& a = m_R.coeffRef(k1, k1);
      Scalar& o1 = m_C.coeffRef(0, j);
      Scalar& o2 = m_C.coeffRef(1, j++);
      r = ei_sqrt(ei_real(ei_conj(a)*a + ei_conj(b)*b));
      o1 = a/r;  // FIXME:
      o2 = b/r;  // what if r == 0 <=> (a==0 && b==0) ?

      for (int i = k1; i < cols; ++i){
        tmp = m_R.coeff(k1, i);
        m_R.coeffRef(k1, i) = ei_conj(o1)*m_R.coeff(k1, i) + ei_conj(o2)*m_R.coeff(k2, i);
        m_R.coeffRef(k2, i) = o1*m_R.coeff(k2, i) - o2*tmp;
      }
    }
  }

  m_Rr = m_R.block(0, 0, cols, cols);

  m_RisUptodate = true;
  m_CisUptodate = true;
}

template<typename MatrixType>
template<typename OtherDerived, typename ResultType>
bool GivensQR<MatrixType>::solve(const MatrixBase<OtherDerived>& b_, ResultType *result) const
{
  // Left-side matrix is copied: it will be in place uptodated during the algorithm
  OtherDerived b = b_;

  Scalar tmp;
  int bcols = b.cols();
  result->resize(cols, bcols);

  if (!m_RisUptodate || !m_CisUptodate){
    //Same as compute_CR() + update of b

    RealScalar r;
    const int q_cols = cols < rows ? cols : rows-1;

    m_R = m_.template cast<typename MatrixRType::Scalar>();

    int j = 0;
    for(int k1 = 0; k1 < q_cols; ++k1) {
      for (int k2 = rows - 1; k2 > k1; --k2) {

        const Scalar& m2 = m_R.coeffRef(k2, k1);
        const Scalar& m1 = m_R.coeffRef(k1, k1);
        Scalar& o1 = m_C.coeffRef(0, j);
        Scalar& o2 = m_C.coeffRef(1, j++);
        r = ei_sqrt(ei_real(ei_conj(m1)*m1 + ei_conj(m2)*m2));
        o1 = m1/r;  // FIXME:
        o2 = m2/r;  // what if r == 0 <=> (m1==0 && m2==0) ?

        //Update R
        for (int i = k1; i < cols; ++i){
          tmp = m_R.coeff(k1, i);
          m_R.coeffRef(k1, i) = ei_conj(o1)*m_R.coeff(k1, i) + ei_conj(o2)*m_R.coeff(k2, i);
          m_R.coeffRef(k2, i) = o1*m_R.coeff(k2, i) - o2*tmp;
        }

        //Update b
        for (int i = 0; i < bcols; ++i){
          tmp = b.coeff(k1, i);
          b.coeffRef(k1, i) = ei_conj(o1)*b.coeff(k1, i) + ei_conj(o2)*b.coeff(k2, i);
          b.coeffRef(k2, i) = o1*b.coeff(k2, i) - o2*tmp;
        }
      }
    }

    //Compute reduced matrix: it will be used afterwards
    m_Rr = m_R.block(0, 0, cols, cols);

  } else {

    //More efficient iteration than over, but same system
    int k1 = 0, k2 = 1;
    for (int j = 0; j < cnum; ++j) {
      if (--k2 <= k1){
        ++k1;
        k2 = rows-1;
      }

      //Take the stored coefficients
      const Scalar& o1 = m_C.coeffRef(0, j);
      const Scalar& o2 = m_C.coeffRef(1, j);

      //Update b
      for (int i = 0; i < bcols; ++i){
        tmp = b.coeff(k1, i);
        b.coeffRef(k1, i) = ei_conj(o1)*b.coeff(k1, i) + ei_conj(o2)*b.coeff(k2, i);
        b.coeffRef(k2, i) = o1*b.coeff(k2, i) - o2*tmp;
      }

    }
  }

  //Solve the upper triangular problem in place using the reduced matrix
  m_Rr.template marked<UpperTriangular>().solveTriangularInPlace(b.block(0, 0, cols, bcols));

  //Copy the result
  *result = b.block(0, 0, cols, bcols);

  //At the moment solve() always returns true.
  return true;
}

#endif // EIGEN_HIDE_HEAVY_CODE

#endif // EIGEN_GIVENS_QR_H