Tools/Math/LA_Qr_decomp.cpp

Go to the documentation of this file.

//------------------------------------------------------------------------------
#include <assert.h>
#include <math.h>
#include "LA_Qr_decomp.h"
//------------------------------------------------------------------------------
namespace LA
{
//------------------------------------------------------------------------------
QR::QR(const Matrix& A)
{
  QR_ = A;
  m = A.rows();
  n = A.columns();
  Rdiag.create(n);
  int i=0, j=0, k=0;

  // Main loop.
  for (k = 0; k < n; k++)
  {
    // Compute 2-norm of k-th column without under/overflow.
    double nrm = 0;
    for (i = k; i < m; i++)
    {
      nrm = _hypot(nrm,QR_[i][k]);
    }

    if (nrm != 0.0)
    {
      // Form k-th Householder vector.
      if (QR_[k][k] < 0)
      {
        nrm = -nrm;
      }
      for (i = k; i < m; i++)
      {
        QR_[i][k] /= nrm;
      }
      QR_[k][k] += 1.0;

      // Apply transformation to remaining columns.
      for (j = k+1; j < n; j++)
      {
        double s = 0.0; 
        for (i = k; i < m; i++)
        {
          s += QR_[i][k]*QR_[i][j];
        }
        s = -s/QR_[k][k];
        for (i = k; i < m; i++)
        {
          QR_[i][j] += s*QR_[i][k];
        }
      }
    }
    Rdiag[k] = -nrm;
  }
}
//------------------------------------------------------------------------------
bool QR::isFullRank() const
{
  for (int j = 0; j < n; j++) 
  {
    if (Rdiag[j] == 0)
      return false;
  }
  return true;
}
//------------------------------------------------------------------------------
Matrix QR::getHouseholder() const
{
  Matrix H(m,n);

  for (int i = 0; i < m; i++) 
  {
    for (int j = 0; j < n; j++) 
    {
      if (i >= j)
      {
        H[i][j] = QR_[i][j];
      }
      else
      {
        H[i][j] = 0.0;
      }
    }
  }
  return H;
}
//------------------------------------------------------------------------------
Matrix QR::getR() const
{
  Matrix R(n,n);
  for (int i = 0; i < n; i++)
  {
    for (int j = 0; j < n; j++)
    {
      if (i < j)
      {
        R[i][j] = QR_[i][j];
      }
      else if (i == j)
      {
        R[i][j] = Rdiag[i];
      }
      else
      {
        R[i][j] = 0.0;
      }
    }
  } 
  return R;
}
//------------------------------------------------------------------------------
Matrix QR::getQ() const
{
  int i=0, j=0, k=0;

  Matrix Q(m,n);
  for (k = n-1; k >= 0; k--)
  {
    for (i = 0; i < m; i++)
    {
      Q[i][k] = 0.0;
    }
    Q[k][k] = 1.0;
    for (j = k; j < n; j++)
    {
      if (QR_[k][k] != 0)
      {
        double s = 0.0;
        for (i = k; i < m; i++)
        {
          s += QR_[i][k]*Q[i][j];
        }
        s = -s / QR_[k][k];
        for (i = k; i < m; i++)
        {
          Q[i][j] += s*QR_[i][k];
        }
      }
    }
  }
  return Q;
}
//------------------------------------------------------------------------------
Vector QR::solve(const Vector& b) const
{
  assert(b.dim() == m); /* arrays must be conformant */
  
  if (!isFullRank())  /* matrix is rank deficient */
  {
    throw Exception(Exception::SingularMatrix);
  }

  Vector x = b;
  int i=0, j=0, k=0;

  // Compute Y = transpose(Q)*b
  for (k = 0; k < n; k++) 
  {
    double s = 0.0; 
    for (i = k; i < m; i++) 
    {
      s += QR_[i][k]*x[i];
    }
    s = -s/QR_[k][k];
    for (i = k; i < m; i++) 
    {
      x[i] += s*QR_[i][k];
    }
  }
  // Solve R*X = Y;
  for (k = n-1; k >= 0; k--) 
  {
    x[k] /= Rdiag[k];
    for (i = 0; i < k; i++)
    {
      x[i] -= x[k]*QR_[i][k];
    }
  }


  /* return n x nx portion of X */
  Vector x_(n);
  for (i=0; i<n; i++)
    x_[i] = x[i];

  return x_;
}
//------------------------------------------------------------------------------
}
//------------------------------------------------------------------------------

---