00001
00002 #ifndef vnl_qr_txx_
00003 #define vnl_qr_txx_
00004
00005
00006
00007
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>
00015 #include <vnl/vnl_matlab_print.h>
00016 #include <vnl/vnl_complex_traits.h>
00017 #include <vnl/algo/vnl_netlib.h>
00018
00019
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
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;
00051 jpvt_.fill(0);
00052
00053 vnl_vector<T> work(M.rows());
00054 vnl_linpack_qrdc(qrdc_out_.data_block(),
00055 &r, &r, &c,
00056 qraux_.data_block(),
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
00070
00071
00072
00073
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
00087 template <class T>
00088 vnl_matrix<T> const& vnl_qr<T>::Q() const
00089 {
00090 int m = qrdc_out_.columns();
00091 int n = qrdc_out_.rows();
00092
00093 bool verbose = false;
00094
00095 if (!Q_) {
00096 Q_ = new vnl_matrix<T>(m,m);
00097
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
00112
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
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
00128
00129
00130 if (sq > abs_t(0)) {
00131 abs_t scale = abs_t(2)/sq;
00132
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
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
00152 template <class T>
00153 vnl_matrix<T> const& vnl_qr<T>::R() const
00154 {
00155 if (!R_) {
00156 int m = qrdc_out_.columns();
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
00179
00180
00181
00182
00183
00184
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
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,
00204 (T*)0,
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
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
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,
00233 Qt_B.data_block(),
00234 (T*)0,
00235 (T*)0,
00236 (T*)0,
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
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);
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
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);
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());
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);
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