core/vnl/algo/vnl_qr.txx
Go to the documentation of this file.
00001 // This is core/vnl/algo/vnl_qr.txx
00002 #ifndef vnl_qr_txx_
00003 #define vnl_qr_txx_
00004 //:
00005 // \file
00006 // \author Andrew W. Fitzgibbon, Oxford RRG
00007 // \date   08 Dec 1996
00008 
00009 #include "vnl_qr.h"
00010 #include <vcl_cassert.h>
00011 #include <vcl_iostream.h>
00012 #include <vcl_complex.h>
00013 #include <vnl/vnl_math.h>
00014 #include <vnl/vnl_complex.h>  // vnl_math_squared_magnitude()
00015 #include <vnl/vnl_matlab_print.h>
00016 #include <vnl/vnl_complex_traits.h>
00017 #include <vnl/algo/vnl_netlib.h> // dqrdc_(), dqrsl_()
00018 
00019 // use C++ overloading to call the right linpack routine from the template code:
00020 #ifndef DOXYGEN_SHOULD_SKIP_THIS
00021 #define macro(p, T) \
00022 inline void vnl_linpack_qrdc(vnl_netlib_qrdc_proto(T)) \
00023 { v3p_netlib_##p##qrdc_(vnl_netlib_qrdc_params); } \
00024 inline void vnl_linpack_qrsl(vnl_netlib_qrsl_proto(T)) \
00025 { v3p_netlib_##p##qrsl_(vnl_netlib_qrsl_params); }
00026 macro(s, float);
00027 macro(d, double);
00028 macro(c, vcl_complex<float>);
00029 macro(z, vcl_complex<double>);
00030 #undef macro
00031 #endif
00032 
00033 template <class T>
00034 vnl_qr<T>::vnl_qr(vnl_matrix<T> const& M):
00035   qrdc_out_(M.columns(), M.rows()),
00036   qraux_(M.columns()),
00037   jpvt_(M.rows()),
00038   Q_(0),
00039   R_(0)
00040 {
00041   assert(! M.empty());
00042 
00043   // Fill transposed O/P matrix
00044   long c = M.columns();
00045   long r = M.rows();
00046   for (int i = 0; i < r; ++i)
00047     for (int j = 0; j < c; ++j)
00048       qrdc_out_(j,i) = M(i,j);
00049 
00050   long do_pivot = 0; // Enable[!=0]/disable[==0] pivoting.
00051   jpvt_.fill(0); // Allow all columns to be pivoted if pivoting is enabled.
00052 
00053   vnl_vector<T> work(M.rows());
00054   vnl_linpack_qrdc(qrdc_out_.data_block(), // On output, UT is R, below diag is mangled Q
00055                    &r, &r, &c,
00056                    qraux_.data_block(), // Further information required to demangle Q
00057                    jpvt_.data_block(),
00058                    work.data_block(),
00059                    &do_pivot);
00060 }
00061 
00062 template <class T>
00063 vnl_qr<T>::~vnl_qr()
00064 {
00065   delete Q_;
00066   delete R_;
00067 }
00068 
00069 //: Return the determinant of M.  This is computed from M = Q R as follows:
00070 // |M| = |Q| |R|
00071 // |R| is the product of the diagonal elements.
00072 // |Q| is (-1)^n as it is a product of Householder reflections.
00073 // So det = -prod(-r_ii).
00074 template <class T>
00075 T vnl_qr<T>::determinant() const
00076 {
00077   int m = vnl_math_min((int)qrdc_out_.columns(), (int)qrdc_out_.rows());
00078   T det = qrdc_out_(0,0);
00079 
00080   for (int i = 1; i < m; ++i)
00081     det *= -qrdc_out_(i,i);
00082 
00083   return det;
00084 }
00085 
00086 //: Unpack and return unitary part Q.
00087 template <class T>
00088 vnl_matrix<T> const& vnl_qr<T>::Q() const
00089 {
00090   int m = qrdc_out_.columns(); // column-major storage
00091   int n = qrdc_out_.rows();
00092 
00093   bool verbose = false;
00094 
00095   if (!Q_) {
00096     Q_ = new vnl_matrix<T>(m,m);
00097     // extract Q.
00098     if (verbose) {
00099       vcl_cerr << __FILE__ ": vnl_qr<T>::Q()\n"
00100                << " m,n = " << m << ", " << n << '\n'
00101                << " qr0 = [" << qrdc_out_ << "];\n"
00102                << " aux = [" << qraux_ << "];\n";
00103     }
00104 
00105     Q_->set_identity();
00106     vnl_matrix<T>& matrQ = *Q_;
00107 
00108     vnl_vector<T> v(m, T(0));
00109     vnl_vector<T> w(m, T(0));
00110 
00111     // Golub and vanLoan, p199.  backward accumulation of householder matrices
00112     // Householder vector k is [zeros(1,k-1) qraux_[k] qrdc_out_[k,:]]
00113     typedef typename vnl_numeric_traits<T>::abs_t abs_t;
00114     for (int k = n-1; k >= 0; --k) {
00115       if (k >= m) continue;
00116       // Make housevec v, and accumulate norm at the same time.
00117       v[k] = qraux_[k];
00118       abs_t sq = vnl_math_squared_magnitude(v[k]);
00119       for (int j = k+1; j < m; ++j) {
00120         v[j] = qrdc_out_(k,j);
00121         sq += vnl_math_squared_magnitude(v[j]);
00122       }
00123       if (verbose) vnl_matlab_print(vcl_cerr, v, "v");
00124 #ifndef DOXYGEN_SHOULD_SKIP_THIS
00125 # define c vnl_complex_traits<T>::conjugate
00126 #endif
00127       // Premultiply emerging Q by house(v), noting that v[0..k-1] == 0.
00128       // Q_new = (1 - (2/v'*v) v v')Q
00129       // or Q -= (2/v'*v) v (v'Q)
00130       if (sq > abs_t(0)) {
00131         abs_t scale = abs_t(2)/sq;
00132         // w = (2/v'*v) v' Q
00133         for (int i = k; i < m; ++i) {
00134           w[i] = T(0);
00135           for (int j = k; j < m; ++j)
00136             w[i] += scale * c(v[j]) * matrQ(j, i);
00137         }
00138         if (verbose) vnl_matlab_print(vcl_cerr, w, "w");
00139 
00140         // Q -= v w
00141         for (int i = k; i < m; ++i)
00142           for (int j = k; j < m; ++j)
00143             matrQ(i,j) -= (v[i]) * (w[j]);
00144       }
00145 #undef c
00146     }
00147   }
00148   return *Q_;
00149 }
00150 
00151 //: Unpack and return R.
00152 template <class T>
00153 vnl_matrix<T> const& vnl_qr<T>::R() const
00154 {
00155   if (!R_) {
00156     int m = qrdc_out_.columns(); // column-major storage
00157     int n = qrdc_out_.rows();
00158     R_ = new vnl_matrix<T>(m,n);
00159     vnl_matrix<T> & Rmatr = *R_;
00160 
00161     for (int i = 0; i < m; ++i)
00162       for (int j = 0; j < n; ++j)
00163         if (i > j)
00164           Rmatr(i, j) = T(0);
00165         else
00166           Rmatr(i, j) = qrdc_out_(j,i);
00167   }
00168 
00169   return *R_;
00170 }
00171 
00172 template <class T>
00173 vnl_matrix<T> vnl_qr<T>::recompose() const
00174 {
00175   return Q() * R();
00176 }
00177 
00178 // JOB: ABCDE decimal
00179 // A     B     C     D              E
00180 // ---   ---   ---   ---            ---
00181 // Qb    Q'b   x     norm(A*x - b)  A*x
00182 
00183 
00184 //: Solve equation M x = b for x using the computed decomposition.
00185 template <class T>
00186 vnl_vector<T> vnl_qr<T>::solve(const vnl_vector<T>& b) const
00187 {
00188   long n = qrdc_out_.columns();
00189   long p = qrdc_out_.rows();
00190   const T* b_data = b.data_block();
00191   vnl_vector<T> Qt_B(n);
00192   vnl_vector<T> x(p);
00193 
00194   // see comment above
00195   long JOB = 100;
00196 
00197   long info = 0;
00198   vnl_linpack_qrsl(qrdc_out_.data_block(),
00199                    &n, &n, &p,
00200                    qraux_.data_block(),
00201                    b_data, (T*)0, Qt_B.data_block(),
00202                    x.data_block(),
00203                    (T*)0/*residual*/,
00204                    (T*)0/*Ax*/,
00205                    &JOB,
00206                    &info);
00207 
00208   if (info > 0)
00209     vcl_cerr << __FILE__ ": vnl_qr<T>::solve() : matrix is rank-deficient by "
00210              << info << '\n';
00211 
00212   return x;
00213 }
00214 
00215 //: Return residual vector d of M x = b -> d = Q'b
00216 template <class T>
00217 vnl_vector<T> vnl_qr<T>::QtB(const vnl_vector<T>& b) const
00218 {
00219   long n = qrdc_out_.columns();
00220   long p = qrdc_out_.rows();
00221   const T* b_data = b.data_block();
00222   vnl_vector<T> Qt_B(n);
00223 
00224   // see comment above
00225   long JOB = 1000;
00226 
00227   long info = 0;
00228   vnl_linpack_qrsl(qrdc_out_.data_block(),
00229                    &n, &n, &p,
00230                    qraux_.data_block(),
00231                    b_data,
00232                    (T*)0,               // A: Qb
00233                    Qt_B.data_block(),   // B: Q'b
00234                    (T*)0,               // C: x
00235                    (T*)0,               // D: residual
00236                    (T*)0,               // E: Ax
00237                    &JOB,
00238                    &info);
00239 
00240   if (info > 0)
00241     vcl_cerr << __FILE__ ": vnl_qr<T>::QtB() -- matrix is rank-deficient by "
00242              << info << '\n';
00243 
00244   return Qt_B;
00245 }
00246 
00247 template <class T>
00248 vnl_matrix<T> vnl_qr<T>::inverse() const
00249 {
00250   unsigned int r = qrdc_out_.columns();
00251   assert(r > 0 && r == qrdc_out_.rows());
00252   vnl_matrix<T> inv(r,r);
00253 
00254   // Use solve() to compute the inverse matrix, using (00..010..00) as rhs
00255   vnl_vector<T> rhs(r,T(0));
00256   for (unsigned int i=0; i<r; ++i)
00257   {
00258     rhs(i) = T(1);
00259     vnl_vector<T> col = this->solve(rhs); // returns i-th column of inverse
00260     inv.set_column(i,col);
00261     rhs(i) = T(0);
00262   }
00263   return inv;
00264 }
00265 
00266 template <class T>
00267 vnl_matrix<T> vnl_qr<T>::tinverse() const
00268 {
00269   unsigned int r = qrdc_out_.columns();
00270   assert(r > 0 && r == qrdc_out_.rows());
00271   vnl_matrix<T> tinv(r,r);
00272 
00273   // Use solve() to compute the inverse matrix, using (00..010..00) as rhs
00274   vnl_vector<T> rhs(r,T(0));
00275   for (unsigned int i=0; i<r; ++i)
00276   {
00277     rhs(i) = T(1);
00278     vnl_vector<T> col = this->solve(rhs); // returns i-th column of inverse
00279     tinv.set_row(i,col);
00280     rhs(i) = T(0);
00281   }
00282   return tinv;
00283 }
00284 
00285 template <class T>
00286 vnl_matrix<T> vnl_qr<T>::solve(vnl_matrix<T> const& rhs) const
00287 {
00288   assert(rhs.rows() == qrdc_out_.columns()); // column-major storage
00289   int c = qrdc_out_.rows();
00290   int n = rhs.columns();
00291   vnl_matrix<T> result(c,n);
00292 
00293   for (int i=0; i<n; ++i)
00294   {
00295     vnl_vector<T> b = rhs.get_column(i);
00296     vnl_vector<T> col = this->solve(b); // returns i-th column of result
00297     result.set_column(i,col);
00298   }
00299   return result;
00300 }
00301 
00302 //------------------------------------------------------------------------------
00303 
00304 #define VNL_QR_INSTANTIATE(T) \
00305  template class vnl_qr<T >; \
00306  VCL_INSTANTIATE_INLINE(T vnl_qr_determinant(vnl_matrix<T > const&))
00307 
00308 #endif