00001
00002 #ifndef vnl_svd_txx_
00003 #define vnl_svd_txx_
00004
00005
00006
00007 #include "vnl_svd.h"
00008
00009 #include <vcl_cassert.h>
00010 #include <vcl_cstdlib.h>
00011 #include <vcl_complex.h>
00012 #include <vcl_iostream.h>
00013 #include <vcl_algorithm.h>
00014
00015 #include <vnl/vnl_math.h>
00016 #include <vnl/vnl_fortran_copy.h>
00017 #include <vnl/algo/vnl_netlib.h>
00018
00019
00020 #define macro(p, T) \
00021 inline void vnl_linpack_svdc(vnl_netlib_svd_proto(T)) \
00022 { v3p_netlib_##p##svdc_(vnl_netlib_svd_params); }
00023 macro(s, float);
00024 macro(d, double);
00025 macro(c, vcl_complex<float>);
00026 macro(z, vcl_complex<double>);
00027 #undef macro
00028
00029
00030
00031 static bool vnl_svd_test_heavily = false;
00032 #include <vnl/vnl_matlab_print.h>
00033
00034 template <class T>
00035 vnl_svd<T>::vnl_svd(vnl_matrix<T> const& M, double zero_out_tol):
00036 m_(M.rows()),
00037 n_(M.columns()),
00038 U_(m_, n_),
00039 W_(n_),
00040 Winverse_(n_),
00041 V_(n_, n_)
00042 {
00043 assert(m_ > 0);
00044 assert(n_ > 0);
00045
00046 {
00047 long n = M.rows();
00048 long p = M.columns();
00049 long mm = vcl_min(n+1L,p);
00050
00051
00052
00053 vnl_fortran_copy<T> X(M);
00054
00055
00056 vnl_vector<T> work(n, T(0));
00057 vnl_vector<T> uspace(n*p, T(0));
00058 vnl_vector<T> vspace(p*p, T(0));
00059 vnl_vector<T> wspace(mm, T(0));
00060 vnl_vector<T> espace(p, T(0));
00061
00062
00063 long info = 0;
00064 const long job = 21;
00065 vnl_linpack_svdc((T*)X, &n, &n, &p,
00066 wspace.data_block(),
00067 espace.data_block(),
00068 uspace.data_block(), &n,
00069 vspace.data_block(), &p,
00070 work.data_block(),
00071 &job, &info);
00072
00073
00074 if (info != 0)
00075 {
00076
00077
00078
00079
00080
00081
00082
00083 M.assert_finite();
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103 vcl_cerr << __FILE__ ": suspicious return value (" << info << ") from SVDC\n"
00104 << __FILE__ ": M is " << M.rows() << 'x' << M.cols() << vcl_endl;
00105
00106 vnl_matlab_print(vcl_cerr, M, "M", vnl_matlab_print_format_long);
00107 valid_ = false;
00108 }
00109 else
00110 valid_ = true;
00111
00112
00113 {
00114 const T *d = uspace.data_block();
00115 for (int j = 0; j < p; ++j)
00116 for (int i = 0; i < n; ++i)
00117 U_(i,j) = *d++;
00118 }
00119
00120 for (int j = 0; j < mm; ++j)
00121 W_(j, j) = vcl_abs(wspace(j));
00122
00123 for (int j = mm; j < n_; ++j)
00124 W_(j, j) = 0;
00125
00126 {
00127 const T *d = vspace.data_block();
00128 for (int j = 0; j < p; ++j)
00129 for (int i = 0; i < p; ++i)
00130 V_(i,j) = *d++;
00131 }
00132 }
00133
00134 if (vnl_svd_test_heavily)
00135 {
00136
00137 typedef typename vnl_numeric_traits<T>::abs_t abs_t;
00138 abs_t recomposition_residual = vcl_abs((recompose() - M).fro_norm());
00139 abs_t n = vcl_abs(M.fro_norm());
00140 abs_t thresh = abs_t(m_) * abs_t(vnl_math::eps) * n;
00141 if (recomposition_residual > thresh)
00142 {
00143 vcl_cerr << "vnl_svd<T>::vnl_svd<T>() -- Warning, recomposition_residual = "
00144 << recomposition_residual << vcl_endl
00145 << "fro_norm(M) = " << n << vcl_endl
00146 << "eps*fro_norm(M) = " << thresh << vcl_endl
00147 << "Press return to continue\n";
00148 char x;
00149 vcl_cin.get(&x, 1, '\n');
00150 }
00151 }
00152
00153 if (zero_out_tol >= 0)
00154
00155 zero_out_absolute(double(+zero_out_tol));
00156 else
00157
00158 zero_out_relative(double(-zero_out_tol));
00159 }
00160
00161 template <class T>
00162 vcl_ostream& operator<<(vcl_ostream& s, const vnl_svd<T>& svd)
00163 {
00164 s << "vnl_svd<T>:\n"
00165
00166 << "U = [\n" << svd.U() << "]\n"
00167 << "W = " << svd.W() << '\n'
00168 << "V = [\n" << svd.V() << "]\n"
00169 << "rank = " << svd.rank() << vcl_endl;
00170 return s;
00171 }
00172
00173
00174
00175
00176
00177 template <class T>
00178 void
00179 vnl_svd<T>::zero_out_absolute(double tol)
00180 {
00181 last_tol_ = tol;
00182 rank_ = W_.rows();
00183 for (unsigned k = 0; k < W_.rows(); k++)
00184 {
00185 singval_t& weight = W_(k, k);
00186 if (vcl_abs(weight) <= tol)
00187 {
00188 Winverse_(k,k) = 0;
00189 weight = 0;
00190 --rank_;
00191 }
00192 else
00193 {
00194 Winverse_(k,k) = singval_t(1.0)/weight;
00195 }
00196 }
00197 }
00198
00199
00200 template <class T> void vnl_svd<T>::zero_out_relative(double tol)
00201 {
00202 zero_out_absolute(tol * vcl_abs(sigma_max()));
00203 }
00204
00205 static bool w=false;
00206 inline bool vnl_svn_warned() { if (w) return true; else { w=true; return false; } }
00207
00208
00209 template <class T>
00210 typename vnl_svd<T>::singval_t vnl_svd<T>::determinant_magnitude() const
00211 {
00212 if (!vnl_svn_warned() && m_ != n_)
00213 vcl_cerr << __FILE__ ": called determinant_magnitude() on SVD of non-square matrix\n"
00214 << "(This warning is displayed only once)\n";
00215 singval_t product = W_(0, 0);
00216 for (unsigned long k = 1; k < W_.columns(); k++)
00217 product *= W_(k, k);
00218
00219 return product;
00220 }
00221
00222 template <class T>
00223 typename vnl_svd<T>::singval_t vnl_svd<T>::norm() const
00224 {
00225 return vcl_abs(sigma_max());
00226 }
00227
00228
00229 template <class T>
00230 vnl_matrix<T> vnl_svd<T>::recompose(unsigned int rnk) const
00231 {
00232 if (rnk > rank_) rnk=rank_;
00233 vnl_matrix<T> Wmatr(W_.rows(),W_.columns());
00234 Wmatr.fill(T(0));
00235 for (unsigned int i=0;i<rnk;++i)
00236 Wmatr(i,i)=W_(i,i);
00237
00238 return U_*Wmatr*V_.conjugate_transpose();
00239 }
00240
00241
00242
00243 template <class T>
00244 vnl_matrix<T> vnl_svd<T>::pinverse(unsigned int rnk) const
00245 {
00246 if (rnk > rank_) rnk=rank_;
00247 vnl_matrix<T> W_inverse(Winverse_.rows(),Winverse_.columns());
00248 W_inverse.fill(T(0));
00249 for (unsigned int i=0;i<rnk;++i)
00250 W_inverse(i,i)=Winverse_(i,i);
00251
00252 return V_ * W_inverse * U_.conjugate_transpose();
00253 }
00254
00255
00256
00257 template <class T>
00258 vnl_matrix<T> vnl_svd<T>::tinverse(unsigned int rnk) const
00259 {
00260 if (rnk > rank_) rnk=rank_;
00261 vnl_matrix<T> W_inverse(Winverse_.rows(),Winverse_.columns());
00262 W_inverse.fill(T(0));
00263 for (unsigned int i=0;i<rnk;++i)
00264 W_inverse(i,i)=Winverse_(i,i);
00265
00266 return U_ * W_inverse * V_.conjugate_transpose();
00267 }
00268
00269
00270
00271 template <class T>
00272 vnl_matrix<T> vnl_svd<T>::solve(vnl_matrix<T> const& B) const
00273 {
00274 vnl_matrix<T> x;
00275 if (U_.rows() < U_.columns()) {
00276 vnl_matrix<T> yy(U_.rows(), B.columns(), T(0));
00277 yy.update(B);
00278 x = U_.conjugate_transpose() * yy;
00279 }
00280 else
00281 x = U_.conjugate_transpose() * B;
00282 for (unsigned long i = 0; i < x.rows(); ++i) {
00283 T weight = W_(i, i);
00284 if (weight != T(0))
00285 weight = T(1) / weight;
00286 for (unsigned long j = 0; j < x.columns(); ++j)
00287 x(i, j) *= weight;
00288 }
00289 x = V_ * x;
00290 return x;
00291 }
00292
00293
00294 template <class T>
00295 vnl_vector<T> vnl_svd<T>::solve(vnl_vector<T> const& y) const
00296 {
00297
00298 if (y.size() != U_.rows())
00299 {
00300 vcl_cerr << __FILE__ << ": size of rhs is incompatible with no. of rows in U_\n"
00301 << "y =" << y << '\n'
00302 << "m_=" << m_ << '\n'
00303 << "n_=" << n_ << '\n'
00304 << "U_=\n" << U_
00305 << "V_=\n" << V_
00306 << "W_=\n" << W_;
00307 }
00308
00309 vnl_vector<T> x(V_.rows());
00310 if (U_.rows() < U_.columns()) {
00311 vnl_vector<T> yy(U_.rows(), T(0));
00312 if (yy.size()<y.size()) {
00313 vcl_cerr << "yy=" << yy << vcl_endl
00314 << "y =" << y << vcl_endl;
00315
00316 }
00317 yy.update(y);
00318 x = U_.conjugate_transpose() * yy;
00319 }
00320 else
00321 x = U_.conjugate_transpose() * y;
00322
00323 for (unsigned i = 0; i < x.size(); i++) {
00324 T weight = W_(i, i), zero_(0);
00325 if (weight != zero_)
00326 x[i] /= weight;
00327 else
00328 x[i] = zero_;
00329 }
00330 return V_ * x;
00331 }
00332
00333 template <class T>
00334 void vnl_svd<T>::solve(T const *y, T *x) const
00335 {
00336 solve(vnl_vector<T>(y, m_)).copy_out(x);
00337 }
00338
00339
00340
00341 template <class T>
00342 void vnl_svd<T>::solve_preinverted(vnl_vector<T> const& y, vnl_vector<T>* x_out) const
00343 {
00344 vnl_vector<T> x;
00345 if (U_.rows() < U_.columns()) {
00346 vcl_cout << "vnl_svd<T>::solve_preinverted() -- Augmenting y\n";
00347 vnl_vector<T> yy(U_.rows(), T(0));
00348 yy.update(y);
00349 x = U_.conjugate_transpose() * yy;
00350 }
00351 else
00352 x = U_.conjugate_transpose() * y;
00353 for (unsigned i = 0; i < x.size(); i++)
00354 x[i] *= W_(i, i);
00355
00356 *x_out = V_ * x;
00357 }
00358
00359
00360
00361 template <class T>
00362 vnl_matrix <T> vnl_svd<T>::nullspace() const
00363 {
00364 int k = rank();
00365 if (k == n_)
00366 vcl_cerr << "vnl_svd<T>::nullspace() -- Matrix is full rank." << last_tol_ << vcl_endl;
00367 return nullspace(n_-k);
00368 }
00369
00370
00371
00372 template <class T>
00373 vnl_matrix <T> vnl_svd<T>::nullspace(int required_nullspace_dimension) const
00374 {
00375 return V_.extract(V_.rows(), required_nullspace_dimension, 0, n_ - required_nullspace_dimension);
00376 }
00377
00378
00379
00380 template <class T>
00381 vnl_matrix <T> vnl_svd<T>::left_nullspace() const
00382 {
00383 int k = rank();
00384 if (k == n_)
00385 vcl_cerr << "vnl_svd<T>::left_nullspace() -- Matrix is full rank." << last_tol_ << vcl_endl;
00386 return U_.extract(U_.rows(), n_-k, 0, k);
00387 }
00388
00389
00390
00391 template <class T>
00392 vnl_matrix<T> vnl_svd<T>::left_nullspace(int ) const
00393 {
00394 return left_nullspace();
00395 }
00396
00397
00398
00399
00400
00401
00402 template <class T>
00403 vnl_vector <T> vnl_svd<T>::nullvector() const
00404 {
00405 vnl_vector<T> ret(n_);
00406 for (int i = 0; i < n_; ++i)
00407 ret(i) = V_(i, n_-1);
00408 return ret;
00409 }
00410
00411
00412
00413
00414 template <class T>
00415 vnl_vector <T> vnl_svd<T>::left_nullvector() const
00416 {
00417 vnl_vector<T> ret(m_);
00418 int col = vcl_min(m_, n_) - 1;
00419 for (int i = 0; i < m_; ++i)
00420 ret(i) = U_(i, col);
00421 return ret;
00422 }
00423
00424
00425
00426 #undef VNL_SVD_INSTANTIATE
00427 #define VNL_SVD_INSTANTIATE(T) \
00428 template class vnl_svd<T >; \
00429 template vcl_ostream& operator<<(vcl_ostream &, vnl_svd<T > const &)
00430
00431 #endif // vnl_svd_txx_