00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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
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
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
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
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;
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
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;
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;
00278 };
00279
00280
00281
00282
00283 #ifndef TOON_NO_NAMESPACE
00284 }
00285 #endif
00286
00287
00288
00289
00290
00291
00292
00293
00294 #endif