make HouseholderQR uses the Householder module

6 files changed, 89 insertions, 92 deletions

diff --git a/Eigen/QR b/Eigen/QR
index ecebd32e4..151c0b31b 100644
--- a/Eigen/QR
+++ b/Eigen/QR
@@ -7,6 +7,7 @@
 
 #include "Cholesky"
 #include "Jacobi"
+#include "Householder"
 
 // Note that EIGEN_HIDE_HEAVY_CODE has to be defined per module
 #if (defined EIGEN_EXTERN_INSTANTIATIONS) && (EIGEN_EXTERN_INSTANTIATIONS>=2)
diff --git a/Eigen/src/Core/MatrixBase.h b/Eigen/src/Core/MatrixBase.h
index 380ac89c0..9e92c043f 100644
--- a/Eigen/src/Core/MatrixBase.h
+++ b/Eigen/src/Core/MatrixBase.h
@@ -786,15 +786,18 @@ template<typename Derived> class MatrixBase
 
 ////////// Householder module ///////////
 
+    void makeHouseholderInPlace(RealScalar *tau, Scalar *beta);
     template<typename EssentialPart>
     void makeHouseholder(EssentialPart *essential,
-                         RealScalar *beta) const;
+                         RealScalar *tau, Scalar *beta) const;
     template<typename EssentialPart>
     void applyHouseholderOnTheLeft(const EssentialPart& essential,
-                                   const RealScalar& beta);
+                                   const RealScalar& tau,
+                                   Scalar* workspace);
     template<typename EssentialPart>
     void applyHouseholderOnTheRight(const EssentialPart& essential,
-                                    const RealScalar& beta);
+                                    const RealScalar& tau,
+                                    Scalar* workspace);
 
 ///////// Jacobi module /////////
 
diff --git a/Eigen/src/Core/NoAlias.h b/Eigen/src/Core/NoAlias.h
index a45493ddc..66d8d834d 100644
--- a/Eigen/src/Core/NoAlias.h
+++ b/Eigen/src/Core/NoAlias.h
@@ -48,26 +48,26 @@ class NoAlias
     /** Behaves like MatrixBase::lazyAssign(other)
       * \sa MatrixBase::lazyAssign() */
     template<typename OtherDerived>
-    ExpressionType& operator=(const MatrixBase<OtherDerived>& other)
+    EIGEN_STRONG_INLINE ExpressionType& operator=(const MatrixBase<OtherDerived>& other)
     { return m_expression.lazyAssign(other.derived()); }
 
     /** \sa MatrixBase::operator+= */
     template<typename OtherDerived>
-    ExpressionType& operator+=(const MatrixBase<OtherDerived>& other)
+    EIGEN_STRONG_INLINE ExpressionType& operator+=(const MatrixBase<OtherDerived>& other)
     { return m_expression.lazyAssign(m_expression + other.derived()); }
 
     /** \sa MatrixBase::operator-= */
     template<typename OtherDerived>
-    ExpressionType& operator-=(const MatrixBase<OtherDerived>& other)
+    EIGEN_STRONG_INLINE ExpressionType& operator-=(const MatrixBase<OtherDerived>& other)
     { return m_expression.lazyAssign(m_expression - other.derived()); }
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
     template<typename ProductDerived, typename Lhs, typename Rhs>
-    ExpressionType& operator+=(const ProductBase<ProductDerived, Lhs,Rhs>& other)
+    EIGEN_STRONG_INLINE ExpressionType& operator+=(const ProductBase<ProductDerived, Lhs,Rhs>& other)
     { other.derived().addTo(m_expression); return m_expression; }
 
     template<typename ProductDerived, typename Lhs, typename Rhs>
-    ExpressionType& operator-=(const ProductBase<ProductDerived, Lhs,Rhs>& other)
+    EIGEN_STRONG_INLINE ExpressionType& operator-=(const ProductBase<ProductDerived, Lhs,Rhs>& other)
     { other.derived().subTo(m_expression); return m_expression; }
 #endif
 
diff --git a/Eigen/src/Core/Product.h b/Eigen/src/Core/Product.h
index fba4241b1..e1e106b80 100644
--- a/Eigen/src/Core/Product.h
+++ b/Eigen/src/Core/Product.h
@@ -187,7 +187,7 @@ class GeneralProduct<Lhs, Rhs, OuterProduct>
 
 template<> struct ei_outer_product_selector<ColMajor> {
   template<typename ProductType, typename Dest>
-  static void run(const ProductType& prod, Dest& dest, typename ProductType::Scalar alpha) {
+  EIGEN_DONT_INLINE static void run(const ProductType& prod, Dest& dest, typename ProductType::Scalar alpha) {
     // FIXME make sure lhs is sequentially stored
     const int cols = dest.cols();
     for (int j=0; j<cols; ++j)
@@ -197,7 +197,7 @@ template<> struct ei_outer_product_selector<ColMajor> {
 
 template<> struct ei_outer_product_selector<RowMajor> {
   template<typename ProductType, typename Dest>
-  static void run(const ProductType& prod, Dest& dest, typename ProductType::Scalar alpha) {
+  EIGEN_DONT_INLINE static void run(const ProductType& prod, Dest& dest, typename ProductType::Scalar alpha) {
     // FIXME make sure rhs is sequentially stored
     const int rows = dest.rows();
     for (int i=0; i<rows; ++i)
diff --git a/Eigen/src/Householder/Householder.h b/Eigen/src/Householder/Householder.h
index 42635cb18..a7bbe96ce 100644
--- a/Eigen/src/Householder/Householder.h
+++ b/Eigen/src/Householder/Householder.h
@@ -42,10 +42,33 @@ void makeTrivialHouseholder(
 }
 
 template<typename Derived>
+void MatrixBase<Derived>::makeHouseholderInPlace(RealScalar *tau, Scalar *beta)
+{
+  VectorBlock<Derived, ei_decrement_size<SizeAtCompileTime>::ret> essentialPart(derived(), 1, size()-1);
+  makeHouseholder(&essentialPart, tau, beta);
+}
+
+/** Computes the elementary reflector H such that:
+  * \f$ H *this = [ beta 0 ... 0]^T \f$
+  * where the transformation H is:
+  * \f$ H = I - tau v v^*\f$
+  * and the vector v is:
+  * \f$ v^T = [1 essential^T] \f$
+  * 
+  * On output:
+  * \param essential the essential part of the vector \c v
+  * \param tau the scaling factor of the householder transformation
+  * \param beta the result of H * \c *this
+  * 
+  * \sa MatrixBase::makeHouseholderInPlace(), MatrixBase::applyHouseholderOnTheLeft(),
+  *     MatrixBase::applyHouseholderOnTheRight()
+  */
+template<typename Derived>
 template<typename EssentialPart>
 void MatrixBase<Derived>::makeHouseholder(
   EssentialPart *essential,
-  RealScalar *beta) const
+  RealScalar *tau,
+  Scalar *beta) const
 {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(EssentialPart)
   RealScalar _squaredNorm = squaredNorm();
@@ -62,35 +85,38 @@ void MatrixBase<Derived>::makeHouseholder(
   VectorBlock<Derived, EssentialPart::SizeAtCompileTime> tail(derived(), 1, size()-1);
   *essential = tail / c0;
   const RealScalar c0abs2 = ei_abs2(c0);
-  *beta = RealScalar(2) * c0abs2 / (c0abs2 + _squaredNorm - ei_abs2(coeff(0)));
+  *tau = RealScalar(2) * c0abs2 / (c0abs2 + _squaredNorm - ei_abs2(coeff(0)));
+  *beta = coeff(0) - c0;
 }
 
 template<typename Derived>
 template<typename EssentialPart>
 void MatrixBase<Derived>::applyHouseholderOnTheLeft(
   const EssentialPart& essential,
-  const RealScalar& beta)
+  const RealScalar& tau,
+  Scalar* workspace)
 {
-  Matrix<Scalar, 1, ColsAtCompileTime, PlainMatrixType::Options, 1, MaxColsAtCompileTime> tmp(cols());
+  Map<Matrix<Scalar, 1, ColsAtCompileTime, PlainMatrixType::Options, 1, MaxColsAtCompileTime> > tmp(workspace,cols());
   Block<Derived, EssentialPart::SizeAtCompileTime, Derived::ColsAtCompileTime> bottom(derived(), 1, 0, rows()-1, cols());
-  tmp = (essential.adjoint() * bottom).lazy();
+  tmp.noalias() = essential.adjoint() * bottom;
   tmp += row(0);
-  row(0) -= beta * tmp;
-  bottom -= beta * essential * tmp;
+  row(0) -= tau * tmp;
+  bottom.noalias() -= tau * essential * tmp;
 }
 
 template<typename Derived>
 template<typename EssentialPart>
 void MatrixBase<Derived>::applyHouseholderOnTheRight(
   const EssentialPart& essential,
-  const RealScalar& beta)
+  const RealScalar& tau,
+  Scalar* workspace)
 {
-  Matrix<Scalar, RowsAtCompileTime, 1, PlainMatrixType::Options, MaxRowsAtCompileTime, 1> tmp(rows());
+  Map<Matrix<Scalar, RowsAtCompileTime, 1, PlainMatrixType::Options, MaxRowsAtCompileTime, 1> > tmp(workspace,rows());
   Block<Derived, Derived::RowsAtCompileTime, EssentialPart::SizeAtCompileTime> right(derived(), 0, 1, rows(), cols()-1);
-  tmp = (right * essential.conjugate()).lazy();
+  tmp.noalias() = right * essential.conjugate();
   tmp += col(0);
-  col(0) -= beta * tmp;
-  right -= beta * tmp * essential.transpose();
+  col(0) -= tau * tmp;
+  right.noalias() -= tau * tmp * essential.transpose();
 }
 
 #endif // EIGEN_HOUSEHOLDER_H
diff --git a/Eigen/src/QR/QR.h b/Eigen/src/QR/QR.h
index d37a019ff..0c6142129 100644
--- a/Eigen/src/QR/QR.h
+++ b/Eigen/src/QR/QR.h
@@ -45,11 +45,15 @@ template<typename MatrixType> class HouseholderQR
 {
   public:
 
+    enum {
+      MinSizeAtCompileTime = EIGEN_ENUM_MIN(MatrixType::ColsAtCompileTime,MatrixType::RowsAtCompileTime)
+    };
+    
     typedef typename MatrixType::Scalar Scalar;
     typedef typename MatrixType::RealScalar RealScalar;
     typedef Block<MatrixType, MatrixType::ColsAtCompileTime, MatrixType::ColsAtCompileTime> MatrixRBlockType;
     typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, MatrixType::ColsAtCompileTime> MatrixTypeR;
-    typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, 1> VectorType;
+    typedef Matrix<RealScalar, MinSizeAtCompileTime, 1> HCoeffsType;
 
     /**
     * \brief Default Constructor.
@@ -61,7 +65,7 @@ template<typename MatrixType> class HouseholderQR
 
     HouseholderQR(const MatrixType& matrix)
       : m_qr(matrix.rows(), matrix.cols()),
-        m_hCoeffs(matrix.cols()),
+        m_hCoeffs(std::min(matrix.rows(),matrix.cols())),
         m_isInitialized(false)
     {
       compute(matrix);
@@ -105,7 +109,7 @@ template<typename MatrixType> class HouseholderQR
 
   protected:
     MatrixType m_qr;
-    VectorType m_hCoeffs;
+    HCoeffsType m_hCoeffs;
     bool m_isInitialized;
 };
 
@@ -114,65 +118,25 @@ template<typename MatrixType> class HouseholderQR
 template<typename MatrixType>
 HouseholderQR<MatrixType>& HouseholderQR<MatrixType>::compute(const MatrixType& matrix)
 {
-  m_qr = matrix;
-  m_hCoeffs.resize(matrix.cols());
-
   int rows = matrix.rows();
   int cols = matrix.cols();
-  RealScalar eps2 = precision<RealScalar>()*precision<RealScalar>();
+  int size = std::min(rows,cols);
+  
+  m_qr = matrix;
+  m_hCoeffs.resize(size);
+
   Matrix<Scalar,1,MatrixType::ColsAtCompileTime> temp(cols);
 
-  for (int k = 0; k < cols; ++k)
+  for (int k = 0; k < size; ++k)
   {
-    int remainingSize = rows-k;
-
-    RealScalar beta;
-    Scalar v0 = m_qr.col(k).coeff(k);
-
-    if (remainingSize==1)
-    {
-      if (NumTraits<Scalar>::IsComplex)
-      {
-        // Householder transformation on the remaining single scalar
-        beta = ei_abs(v0);
-        if (ei_real(v0)>0)
-          beta = -beta;
-        m_qr.coeffRef(k,k) = beta;
-        m_hCoeffs.coeffRef(k) = (beta - v0) / beta;
-      }
-      else
-      {
-        m_hCoeffs.coeffRef(k) = 0;
-      }
-    }
-    else if ((beta=m_qr.col(k).end(remainingSize-1).squaredNorm())>eps2)
-    // FIXME what about ei_imag(v0) ??
-    {
-      // form k-th Householder vector
-      beta = ei_sqrt(ei_abs2(v0)+beta);
-      if (ei_real(v0)>=0.)
-        beta = -beta;
-      m_qr.col(k).end(remainingSize-1) /= v0-beta;
-      m_qr.coeffRef(k,k) = beta;
-      Scalar h = m_hCoeffs.coeffRef(k) = (beta - v0) / beta;
-
-      // apply the Householder transformation (I - h v v') to remaining columns, i.e.,
-      // R <- (I - h v v') * R   where v = [1,m_qr(k+1,k), m_qr(k+2,k), ...]
-      int remainingCols = cols - k -1;
-      if (remainingCols>0)
-      {
-        m_qr.coeffRef(k,k) = Scalar(1);
-        temp.end(remainingCols) = (m_qr.col(k).end(remainingSize).adjoint()
-                                * m_qr.corner(BottomRight, remainingSize, remainingCols)).lazy();
-        m_qr.corner(BottomRight, remainingSize, remainingCols) -= (ei_conj(h) * m_qr.col(k).end(remainingSize)
-                                                                              * temp.end(remainingCols)).lazy();
-        m_qr.coeffRef(k,k) = beta;
-      }
-    }
-    else
-    {
-      m_hCoeffs.coeffRef(k) = 0;
-    }
+    int remainingRows = rows - k;
+    int remainingCols = cols - k -1;
+
+    m_qr.col(k).end(remainingRows).makeHouseholderInPlace(&m_hCoeffs.coeffRef(k), &m_qr.coeffRef(k,k));
+    
+    if (remainingRows>1 && remainingCols>0)
+      m_qr.corner(BottomRight, remainingRows, remainingCols)
+          .applyHouseholderOnTheLeft(m_qr.col(k).end(remainingRows-1), m_hCoeffs.coeffRef(k), &temp.coeffRef(k+1));
   }
   m_isInitialized = true;
   return *this;
@@ -187,12 +151,20 @@ void HouseholderQR<MatrixType>::solve(
 {
   ei_assert(m_isInitialized && "HouseholderQR is not initialized.");
   const int rows = m_qr.rows();
+  const int cols = b.cols();
   ei_assert(b.rows() == rows);
-  result->resize(rows, b.cols());
+  result->resize(rows, cols);
 
-  // TODO(keir): There is almost certainly a faster way to multiply by
-  // Q^T without explicitly forming matrixQ(). Investigate.
-  *result = matrixQ().transpose()*b;
+  *result = b;
+  
+  Matrix<Scalar,1,MatrixType::ColsAtCompileTime> temp(cols);
+  for (int k = 0; k < cols; ++k)
+  {
+    int remainingSize = rows-k;
+
+    result->corner(BottomRight, remainingSize, cols)
+           .applyHouseholderOnTheLeft(m_qr.col(k).end(remainingSize-1), ei_real(m_hCoeffs.coeff(k)), &temp.coeffRef(0));
+  }
 
   const int rank = std::min(result->rows(), result->cols());
   m_qr.corner(TopLeft, rank, rank)
@@ -214,15 +186,10 @@ MatrixType HouseholderQR<MatrixType>::matrixQ() const
   Matrix<Scalar,1,MatrixType::ColsAtCompileTime> temp(cols);
   for (int k = cols-1; k >= 0; k--)
   {
-    // to make easier the computation of the transformation, let's temporarily
-    // overwrite m_qr(k,k) such that the end of m_qr.col(k) is exactly our Householder vector.
-    Scalar beta = m_qr.coeff(k,k);
-    m_qr.const_cast_derived().coeffRef(k,k) = 1;
-    int endLength = rows-k;
-
-    temp.end(cols-k) = (m_qr.col(k).end(endLength).adjoint() * res.corner(BottomRight,endLength, cols-k)).lazy();
-    res.corner(BottomRight,endLength, cols-k) -= ((m_hCoeffs.coeff(k) * m_qr.col(k).end(endLength)) * temp.end(cols-k)).lazy();
-    m_qr.const_cast_derived().coeffRef(k,k) = beta;
+    int remainingSize = rows-k;
+
+    res.corner(BottomRight, remainingSize, cols-k)
+       .applyHouseholderOnTheLeft(m_qr.col(k).end(remainingSize-1), ei_real(m_hCoeffs.coeff(k)), &temp.coeffRef(k));
   }
   return res;
 }