void QRDecomposition<T>::backSub(const VectorT& b, VectorT& x) const
{
if(x.n == 0) x.resize(QR.n);
Assert(QR.m == b.n);
Assert(QR.n == x.n);
/* compute rhs = Q^T b */
VectorT rhs;
QtMul(b,rhs);
if(QR.m == QR.n) {
/* Solve R x = rhs, storing x in-place */
UBackSubstitute(QR,rhs,x);
//UBackSubstitute(QR,x,x);
}
else if(QR.m > QR.n) {
//solve the top part of R x = rhs
MatrixT R1; R1.setRef(QR,0,0,1,1,QR.n,QR.n);
VectorT rhs1; rhs1.setRef(rhs,0,1,QR.n);
UBackSubstitute(R1,rhs1,x);
}
else {
cerr<<"What do we do with m < n?"<<endl;
MatrixPrinter mprint(QR); mprint.mode = MatrixPrinter::AsciiShade;
cerr<<mprint<<endl;
//solve the left part of R x = rhs
MatrixT R1; R1.setRef(QR,0,0,1,1,QR.m,QR.m);
VectorT x1; x1.setRef(x,0,1,QR.m);
UBackSubstitute(R1,rhs,x1);
getchar();
}
}
inline bool UBackSubstitute(const MatrixTemplate<T>& a, const MatrixTemplate<T>& b, MatrixTemplate<T>& x)
{
if(x.isEmpty()) x.resize(a.n,b.n);
else Assert(x.m == a.n && x.n == b.n);
for(int i=0;i<x.n;i++) {
VectorTemplate<T> xi,bi;
x.getColRef(i,xi);
b.getColRef(i,bi);
if(!UBackSubstitute(a,bi,xi)) return false;
}
return true;
}
void QRDecomposition<T>::leastSquares(const VectorT& b, VectorT& x, VectorT& residual) const
{
if(x.n == 0) x.resize(QR.n);
Assert(QR.m >= QR.n);
Assert(QR.m == b.n);
Assert(QR.n == x.n);
Assert(QR.m == residual.n);
MatrixT R;
R.setRef(QR,0,0,1,1,QR.n,QR.n);
VectorT c;
c.setRef(residual,0,1,QR.n);
/* compute rhs = Q^T b */
QtMul(b,residual);
/* Solve R x = rhs */
UBackSubstitute(R,c,x);
/* Compute residual = b - A x = Q (Q^T b - R x) */
c.setZero();
QMul(residual,residual);
}
void NRQRDecomposition<T>::backSub(const VectorT& x,VectorT& b) const
{
VectorT y;
QBackSub(x,y);
UBackSubstitute(QR,y,b);
}
void QRDecomposition<T>::RBackSub(const VectorT& b,VectorT& x) const
{
UBackSubstitute(QR,b,x);
}
