examples/homography/TooN/SVD.h

00001 
00002 /*                       
00003          Copyright (C) 2005 Tom Drummond
00004 
00005      This library is free software; you can redistribute it and/or
00006      modify it under the terms of the GNU Lesser General Public
00007      License as published by the Free Software Foundation; either
00008      version 2.1 of the License, or (at your option) any later version.
00009 
00010      This library is distributed in the hope that it will be useful,
00011      but WITHOUT ANY WARRANTY; without even the implied warranty of
00012      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00013      Lesser General Public License for more details.
00014 
00015      You should have received a copy of the GNU Lesser General Public
00016      License along with this library; if not, write to the Free Software
00017      Foundation, Inc.
00018      51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
00019 */
00020 #ifndef __SVD_H
00021 #define __SVD_H
00022 
00023 #include <iostream>
00024 
00025 #include <TooN/lapack.h>
00026 
00027 #include <TooN/TooN.h>
00028 
00029 #ifndef TOON_NO_NAMESPACE
00030 namespace TooN {
00031 #endif 
00032 
00033 enum{Horizontal,Vertical};
00034 
00035 template <int Rows, int Cols, int HV>
00036 class HV_SVD;
00037 
00038 // (for Rows > Cols)
00039 template <int Rows, int Cols>
00040 class HV_SVD <Rows,Cols,Vertical> {
00041  public:
00042   static const int Inter = (Rows>Cols?Cols:Rows);
00043 
00044   HV_SVD(){}
00045 
00046   template<class Accessor>
00047   HV_SVD(const FixedMatrix<Rows,Cols,Accessor>& m){
00048     compute(m);
00049   }
00050 
00051   template<class Accessor>
00052   void compute(const FixedMatrix<Rows,Cols,Accessor>& m){
00053     my_U=m;
00054     int width=Cols;
00055     int height=Rows;
00056     int lwork=-1;
00057     int info;
00058     double size;
00059     // find out FORTRAN space requirements
00060     dgesvd_(const_cast<char*>("S"),const_cast<char*>("O"),&width,&height,my_U.get_data_ptr(),&width,my_diagonal.get_data_ptr(),my_VT.get_data_ptr(),&width,my_U.get_data_ptr(),&width,&size,&lwork,&info);
00061     lwork = int(size);
00062     double * WORK = new double[lwork];
00063     dgesvd_(const_cast<char*>("S"),const_cast<char*>("O"),&width,&height,my_U.get_data_ptr(),&width,my_diagonal.get_data_ptr(),my_VT.get_data_ptr(),&width,my_U.get_data_ptr(),&width,WORK,&lwork,&info);
00064     delete [] WORK;
00065     if(info!=0){
00066       std::cerr << "error - info was " << info << std::endl;
00067     }
00068   }
00069 
00070   Matrix<Rows,Inter,RowMajor>& get_U(){return my_U;}
00071   Vector<Inter>& get_diagonal(){return my_diagonal;}
00072   Matrix<Inter,Cols,RowMajor>& get_VT(){return my_VT;}
00073 
00074  protected:
00075   Matrix<Rows,Inter,RowMajor> my_U;
00076   Vector<Inter> my_diagonal;
00077   Matrix<Inter,Cols,RowMajor> my_VT;
00078 };
00079 
00080 
00081 // (for Rows < Cols)
00082 template <int Rows, int Cols>
00083 class HV_SVD <Rows,Cols,Horizontal> {
00084 public:
00085   static const int Inter = (Rows>Cols?Cols:Rows);
00086 
00087   HV_SVD(){}
00088 
00089   template<class Accessor>
00090   HV_SVD(const FixedMatrix<Rows,Cols,Accessor>& m){
00091     compute(m);
00092   }
00093 
00094   template<class Accessor>
00095   void compute(const FixedMatrix<Rows,Cols,Accessor>& m){
00096     my_VT=m;
00097     int width=Cols;
00098     int height=Rows;
00099     int lwork=-1;
00100     int info;
00101     double size;
00102     // find out FORTRAN space requirements
00103     dgesvd_(const_cast<char*>("O"),const_cast<char*>("S"),&width,&height,my_VT.get_data_ptr(),&width,my_diagonal.get_data_ptr(),my_VT.get_data_ptr(),&width,my_U.get_data_ptr(),&height,&size,&lwork,&info);
00104     lwork = int(size);
00105     double * WORK = new double[lwork];
00106     dgesvd_(const_cast<char*>("O"),const_cast<char*>("S"),&width,&height,my_VT.get_data_ptr(),&width,my_diagonal.get_data_ptr(),my_VT.get_data_ptr(),&width,my_U.get_data_ptr(),&height,WORK,&lwork,&info);
00107     delete [] WORK;
00108     if(info!=0){
00109       std::cerr << "error - info was " << info << std::endl;
00110     }
00111   }
00112 
00113   Matrix<Rows,Inter,RowMajor>& get_U(){return my_U;}
00114   Vector<Inter>& get_diagonal(){return my_diagonal;}
00115   Matrix<Inter,Cols,RowMajor>& get_VT(){return my_VT;}
00116 
00117  protected:
00118   Matrix<Rows,Inter> my_U;
00119   Vector<Inter> my_diagonal;
00120   Matrix<Inter,Cols> my_VT;
00121 };
00122 
00123 
00124 static const double condition_no=1e9; // GK HACK TO GLOBAL
00125 
00126 template <int Rows=-1, int Cols=Rows>
00127 class SVD : public HV_SVD< Rows, Cols, (Rows<Cols?Horizontal:Vertical) > {
00128   public:
00129   SVD(){}
00130 
00131 
00132 
00133   template<class Accessor>
00134   SVD(const FixedMatrix<Rows,Cols,Accessor>& m): HV_SVD< Rows, Cols, (Rows<Cols?Horizontal:Vertical) > (m) {}
00135 
00136   template <int RHS, class Accessor>
00137     Matrix<Cols,RHS> backsub(const FixedMatrix<Rows,RHS,Accessor>& rhs, const double condition=condition_no){
00138     get_inv_diag(condition);
00139     return (this->my_VT.T() * diagmult(this->inv_diag, (this->my_U.T() * rhs)));
00140   }
00141 
00142   template<class Accessor>
00143     Matrix<> backsub(const DynamicMatrix<Accessor>& rhs, const double condition=condition_no){
00144     get_inv_diag(condition);
00145     return (this->my_VT.T() * diagmult(this->inv_diag, (this->my_U.T() * rhs)));
00146   }
00147 
00148   template <class Accessor>
00149     Vector<Cols> backsub(const FixedVector<Rows,Accessor>& v, const double condition=condition_no){
00150     get_inv_diag(condition);
00151     return (this->my_VT.T() * diagmult(this->inv_diag, (this->my_U.T() * v)));
00152   }
00153 
00154   template <class Accessor>
00155     Vector<> backsub(const DynamicVector<Accessor>& v, const double condition=condition_no){
00156     get_inv_diag(condition);
00157     return (this->my_VT.T() * diagmult(this->inv_diag, (this->my_U.T() * v)));
00158   }
00159  
00160 
00161   Matrix<Cols,Rows> get_pinv(const double condition = condition_no){
00162     get_inv_diag(condition);
00163     return diagmult(this->my_VT.T(),this->inv_diag) * this->my_U.T();
00164   }
00165 
00166 
00167   void get_inv_diag(const double condition){
00168     for(int i=0; i<HV_SVD< Rows, Cols, (Rows<Cols?Horizontal:Vertical) >::Inter; i++){
00169       if(this->my_diagonal[i] * condition <= this->my_diagonal[0]){
00170         inv_diag[i]=0;
00171       } else {
00172         inv_diag[i]=1.0/this->my_diagonal[i];
00173       }
00174     }
00175   }
00176 
00177  private:
00178 
00179   Vector<HV_SVD< Rows, Cols, (Rows<Cols?Horizontal:Vertical) >::Inter> inv_diag;
00180 
00181 };
00182 
00183 
00184 template<>
00185 class SVD<-1> {
00186 public:
00187   template <class Accessor>
00188   SVD(const MatrixBase<Accessor>& m) : my_orig(m),
00189                                        my_height(m.num_rows()),
00190                                        my_width(m.num_cols()),
00191                                        my_min_dim(my_height<my_width?my_height:my_width),
00192                                        my_diagonal(my_min_dim),
00193                                        my_square(my_min_dim,my_min_dim) {
00194     int lwork=-1;
00195     int info;
00196     double size;
00197 
00198     // compute the storage requirements
00199     if(is_vertical()){
00200       dgesvd_(const_cast<char*>("S"),const_cast<char*>("O"),&my_width,&my_height,my_orig.get_data_ptr(),&my_width,my_diagonal.get_data_ptr(),my_square.get_data_ptr(),&my_width,my_orig.get_data_ptr(),&my_width,&size,&lwork,&info);
00201     } else {
00202       dgesvd_(const_cast<char*>("O"),const_cast<char*>("S"),&my_width,&my_height,my_orig.get_data_ptr(),&my_width,my_diagonal.get_data_ptr(),my_orig.get_data_ptr(),&my_width,my_square.get_data_ptr(),&my_height,&size,&lwork,&info);
00203     }
00204     lwork = int(size);
00205     double * WORK = new double[lwork];
00206     if(is_vertical()){
00207       dgesvd_(const_cast<char*>("S"),const_cast<char*>("O"),&my_width,&my_height,my_orig.get_data_ptr(),&my_width,my_diagonal.get_data_ptr(),my_square.get_data_ptr(),&my_width,my_orig.get_data_ptr(),&my_width,WORK,&lwork,&info);
00208     } else {
00209       dgesvd_(const_cast<char*>("O"),const_cast<char*>("S"),&my_width,&my_height,my_orig.get_data_ptr(),&my_width,my_diagonal.get_data_ptr(),my_orig.get_data_ptr(),&my_width,my_square.get_data_ptr(),&my_height,WORK,&lwork,&info);
00210     }
00211     delete [] WORK;
00212     if(info!=0){
00213       std::cerr << "error - info was " << info << std::endl;
00214     }
00215   }
00216 
00217   bool is_vertical(){return (my_orig.num_rows() >= my_orig.num_cols());}
00218 
00219   Matrix<-1,-1,RowMajor>& get_U(){if(is_vertical()){return my_orig;} else {return my_square;}}
00220   Vector<-1>& get_diagonal(){return my_diagonal;}
00221   Matrix<-1,-1,RowMajor>& get_VT(){if(is_vertical()){return my_square;} else {return my_orig;}}
00222 
00223 
00224   template <class Accessor>
00225     Matrix<> backsub(const DynamicMatrix<Accessor>& rhs, const double condition=condition_no){
00226     Vector<> inv_diag(my_min_dim);
00227     get_inv_diag(inv_diag,condition);
00228     return (get_VT().T() * diagmult(inv_diag, (get_U().T() * rhs)));
00229   }
00230 
00231   template <int R, int C, class Accessor>
00232     Matrix<R,C> backsub(const FixedMatrix<R, C, Accessor>& rhs, const double condition=condition_no){
00233     Vector<> inv_diag(my_min_dim);
00234     get_inv_diag(inv_diag,condition);
00235     return (get_VT().T() * diagmult(inv_diag, (get_U().T() * rhs)));
00236   }
00237 
00238   template <class Accessor>
00239     Vector<> backsub(const DynamicVector<Accessor>& v, const double condition=condition_no){
00240     Vector<> inv_diag(my_min_dim);
00241     get_inv_diag(inv_diag,condition);
00242     return (get_VT().T() * diagmult(inv_diag, (get_U().T() * v)));
00243   }
00244 
00245   template <int Size, class Accessor>
00246     Vector<Size> backsub(const FixedVector<Size, Accessor>& v, const double condition=condition_no){
00247     Vector<> inv_diag(my_min_dim);
00248     get_inv_diag(inv_diag,condition);
00249     return (get_VT().T() * diagmult(inv_diag, (get_U().T() * v)));
00250   }
00251 
00252   Matrix<> get_pinv(const double condition = condition_no){
00253     Vector<> inv_diag(my_min_dim);
00254     get_inv_diag(inv_diag,condition);
00255     return diagmult(get_VT().T(),inv_diag) * get_U().T();
00256   }
00257 
00258 
00259   void get_inv_diag(Vector<>& inv_diag, const double condition){
00260     for(int i=0; i<my_min_dim; i++){
00261       if(my_diagonal[i] * condition <= my_diagonal[0]){
00262         inv_diag[i]=0;
00263       } else {
00264         inv_diag[i]=1.0/my_diagonal[i];
00265       }
00266     }
00267   }
00268 
00269 
00270 
00271 private:
00272   Matrix<-1,-1,RowMajor> my_orig;  // matrix with the original shape
00273   int my_height;
00274   int my_width;
00275   int my_min_dim;
00276   Vector<-1> my_diagonal;
00277   Matrix<-1,-1,RowMajor> my_square;   // square matrix (U or V' depending on the shape of my_orig)
00278 };
00279 
00280 
00281 
00282 
00283 #ifndef TOON_NO_NAMESPACE
00284 }
00285 #endif 
00286 
00287 
00288 
00289 
00290 
00291 
00292 
00293 
00294 #endif

Generated on Fri Feb 22 18:26:54 2008 for QVision by  doxygen 1.5.3