examples/calibrate3d/TooN/LU.h

00001 /*                       
00002          Copyright (C) 2005 Tom Drummond
00003 
00004      This library is free software; you can redistribute it and/or
00005      modify it under the terms of the GNU Lesser General Public
00006      License as published by the Free Software Foundation; either
00007      version 2.1 of the License, or (at your option) any later version.
00008 
00009      This library is distributed in the hope that it will be useful,
00010      but WITHOUT ANY WARRANTY; without even the implied warranty of
00011      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00012      Lesser General Public License for more details.
00013 
00014      You should have received a copy of the GNU Lesser General Public
00015      License along with this library; if not, write to the Free Software
00016      Foundation, Inc.
00017      51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
00018 */
00019 #ifndef __LU_H
00020 #define __LU_H
00021 
00022 #include <iostream>
00023 
00024 #include <TooN/lapack.h>
00025 
00026 #include <TooN/TooN.h>
00027 
00028 #ifndef TOON_NO_NAMESPACE
00029 namespace TooN {
00030 #endif 
00031 
00032 template <int Size=-1>
00033 class LU {
00034   public:
00035 
00036   template<class Accessor>
00037   LU(const FixedMatrix<Size,Size,Accessor>& m){
00038     compute(m);
00039   }
00040 
00041   template<class Accessor>
00042   void compute(const FixedMatrix<Size,Size,Accessor>& m){
00043     my_lu=m;
00044     int lda = Size;
00045     int M = Size;
00046     int N = Size;
00047     dgetrf_(&M,&N,my_lu.get_data_ptr(),&lda,my_IPIV,&my_info);
00048     if(my_info < 0){
00049       std::cerr << "error in LU, INFO was " << my_info << std::endl;
00050     }
00051   }
00052 
00053   template <int NRHS, class Accessor>
00054   Matrix<Size,NRHS,RowMajor> backsub(const FixedMatrix<Size,NRHS,Accessor>& rhs){
00055     Matrix<Size,NRHS,RowMajor> result(rhs);
00056     int M=NRHS;
00057     int N=Size;
00058     double alpha=1;
00059     int lda=Size;
00060     int ldb=NRHS;
00061     dtrsm_("R","U","N","N",&M,&N,&alpha,my_lu.get_data_ptr(),&lda,result.get_data_ptr(),&ldb);
00062     dtrsm_("R","L","N","U",&M,&N,&alpha,my_lu.get_data_ptr(),&lda,result.get_data_ptr(),&ldb);
00063 
00064     // now do the row swapping (lapack dlaswp.f only shuffles fortran rows = Rowmajor cols)
00065     for(int i=N-1; i>=0; i--){
00066       const int swaprow = my_IPIV[i]-1; // fortran arrays start at 1
00067       for(int j=0; j<NRHS; j++){
00068         double temp = result[i][j];
00069         result[i][j] = result[swaprow][j];
00070         result[swaprow][j] = temp;
00071       }
00072     }
00073     return result;
00074   }
00075 
00076   template <class Accessor>
00077   Matrix<-1,-1,RowMajor> backsub(const DynamicMatrix<Accessor>& rhs){
00078     Matrix<-1,-1,RowMajor> result(rhs);
00079     assert(result.num_rows == my_lu.num_rows());
00080     int M=result.num_cols();
00081     int N=result.num_rows();
00082     double alpha=1;
00083     int lda=result.num_rows();
00084     int ldb=result.num_cols();
00085     dtrsm_("R","U","N","N",&M,&N,&alpha,my_lu.get_data_ptr(),&lda,result.get_data_ptr(),&ldb);
00086     dtrsm_("R","L","N","U",&M,&N,&alpha,my_lu.get_data_ptr(),&lda,result.get_data_ptr(),&ldb);
00087 
00088     // now do the row swapping (lapack dlaswp.f only shuffles fortran rows = Rowmajor cols)
00089     for(int i=N-1; i>=0; i--){
00090       const int swaprow = my_IPIV[i]-1; // fortran arrays start at 1
00091       for(int j=0; j<result.num_cols(); j++){
00092         double temp = result(i,j);
00093         result(i,j) = result(swaprow,j);
00094         result(swaprow,j) = temp;
00095       }
00096     }
00097     return result;
00098   }
00099 
00100 
00101   template <class Accessor>
00102   Vector<Size> backsub(const FixedVector<Size,Accessor>& rhs){
00103     Vector<Size> result(rhs);
00104     int M=1;
00105     int N=Size;
00106     double alpha=1;
00107     int lda=Size;
00108     int ldb=1;
00109     dtrsm_("R","U","N","N",&M,&N,&alpha,my_lu.get_data_ptr(),&lda,result.get_data_ptr(),&ldb);
00110     dtrsm_("R","L","N","U",&M,&N,&alpha,my_lu.get_data_ptr(),&lda,result.get_data_ptr(),&ldb);
00111     for(int i=N-1; i>=0; i--){
00112       const int swaprow = my_IPIV[i]-1; // fortran arrays start at 1
00113       double temp = result[i];
00114       result[i]=result[swaprow];
00115       result[swaprow]=temp;
00116     }
00117     return result;
00118   }
00119 
00120   template <class Accessor>
00121   Vector<> backsub(const DynamicVector<Accessor>& rhs){
00122     assert(rhs.size()==Size);
00123     Vector<> result(rhs);
00124     int M=1;
00125     int N=Size;
00126     double alpha=1;
00127     int lda=Size;
00128     int ldb=1;
00129     dtrsm_("R","U","N","N",&M,&N,&alpha,my_lu.get_data_ptr(),&lda,result.get_data_ptr(),&ldb);
00130     dtrsm_("R","L","N","U",&M,&N,&alpha,my_lu.get_data_ptr(),&lda,result.get_data_ptr(),&ldb);
00131     for(int i=N-1; i>=0; i--){
00132       const int swaprow = my_IPIV[i]-1; // fortran arrays start at 1
00133       double temp = result[i];
00134       result[i]=result[swaprow];
00135       result[swaprow]=temp;
00136     }
00137     return result;
00138   }
00139 
00140 
00141   Matrix<Size,Size,RowMajor> get_inverse(){
00142     Matrix<Size,Size,RowMajor> Inverse = my_lu;
00143     int N = Size;
00144     int lda=Size;
00145     int lwork=-1;
00146     double size;
00147     dgetri_(&N, Inverse.get_data_ptr(), &lda, my_IPIV, &size, &lwork, &my_info);
00148     lwork=int(size);
00149     double* WORK = new double[lwork];
00150     dgetri_(&N, Inverse.get_data_ptr(), &lda, my_IPIV, WORK, &lwork, &my_info);
00151     delete [] WORK;
00152     return Inverse;
00153   }
00154 
00155   Matrix<Size,Size,RowMajor>& get_lu(){return my_lu;}
00156   const Matrix<Size,Size,RowMajor>& get_lu()const {return my_lu;}
00157 
00158   inline int get_sign() const {
00159     int result=1;
00160     for(int i=0; i<Size-1; i++){
00161       if(my_IPIV[i] > i+1){
00162         result=-result;
00163       }
00164     }
00165     return result;
00166   }
00167 
00168   inline double determinant() const {
00169     double result = get_sign();
00170     for (int i=0; i<Size; i++){
00171       result*=my_lu(i,i);
00172     }
00173     return result;
00174   }
00175 
00176   int get_info() const { return my_info; }
00177  private:
00178   Matrix<Size,Size,RowMajor> my_lu;
00179   int my_info;
00180   int my_IPIV[Size];
00181 };
00182   
00183 
00184 template <>
00185 class LU<> {
00186   public:
00187 
00188   LU(int size):my_lu(size,size){
00189     my_IPIV = new int[size];
00190   }
00191 
00192   template<class Accessor>
00193   LU(const DynamicMatrix<Accessor>& m) :
00194     my_lu(m.num_rows(),m.num_cols())
00195   {
00196     my_IPIV = new int[m.num_rows()];
00197     assert(m.num_rows() == m.num_cols());
00198     compute(m);
00199   }
00200 
00201   ~LU(){delete[] my_IPIV;}
00202 
00203 
00204   template<class Accessor>
00205   void compute(const DynamicMatrix<Accessor>& m){
00206     my_lu=m;
00207     int lda = my_lu.num_cols();
00208     int M = lda;
00209     int N = lda;
00210     dgetrf_(&M,&N,my_lu.get_data_ptr(),&lda,my_IPIV,&my_info);
00211     if(my_info < 0){
00212       std::cerr << "error in LU, INFO was " << my_info << std::endl;
00213     }
00214   }
00215 
00216 
00217   template <int Rows, int Cols, class Accessor>
00218   Matrix<Rows,Cols,RowMajor> backsub(const FixedMatrix<Rows,Cols,Accessor>& rhs){
00219     assert(my_lu.num_rows() == rhs.num_rows());
00220     Matrix<Rows,Cols,RowMajor> result(rhs);
00221     int M=result.num_cols();
00222     int N=result.num_rows();
00223     double alpha=1;
00224     int lda=my_lu.num_rows();
00225     int ldb=result.num_cols();
00226     dtrsm_("R","U","N","N",&M,&N,&alpha,my_lu.get_data_ptr(),&lda,result.get_data_ptr(),&ldb);
00227     dtrsm_("R","L","N","U",&M,&N,&alpha,my_lu.get_data_ptr(),&lda,result.get_data_ptr(),&ldb);
00228 
00229     // now do the row swapping (lapack dlaswp.f only shuffles fortran rows = Rowmajor cols)
00230     for(int i=N-1; i>=0; i--){
00231       const int swaprow = my_IPIV[i]-1; // fortran arrays start at 1
00232       for(int j=0; j<result.num_cols(); j++){
00233         double temp = result(i,j);
00234         result(i,j) = result(swaprow,j);
00235         result(swaprow,j) = temp;
00236       }
00237     }
00238     return result;
00239   }
00240 
00241 
00242   template <class Accessor>
00243   Matrix<-1,-1,RowMajor> backsub(const DynamicMatrix<Accessor>& rhs){
00244     assert(my_lu.num_rows() == rhs.num_rows());
00245     Matrix<-1,-1,RowMajor> result(rhs);
00246     int M=result.num_cols();
00247     int N=result.num_rows();
00248     double alpha=1;
00249     int lda=my_lu.num_rows();
00250     int ldb=result.num_cols();
00251     dtrsm_("R","U","N","N",&M,&N,&alpha,my_lu.get_data_ptr(),&lda,result.get_data_ptr(),&ldb);
00252     dtrsm_("R","L","N","U",&M,&N,&alpha,my_lu.get_data_ptr(),&lda,result.get_data_ptr(),&ldb);
00253 
00254     // now do the row swapping (lapack dlaswp.f only shuffles fortran rows = Rowmajor cols)
00255     for(int i=N-1; i>=0; i--){
00256       const int swaprow = my_IPIV[i]-1; // fortran arrays start at 1
00257       for(int j=0; j<result.num_cols(); j++){
00258         double temp = result(i,j);
00259         result(i,j) = result(swaprow,j);
00260         result(swaprow,j) = temp;
00261       }
00262     }
00263     return result;
00264   }
00265 
00266   template <int Size, class Accessor>
00267   Vector<Size> backsub(const FixedVector<Size,Accessor>& rhs){
00268     assert(rhs.size() == my_lu.num_rows());
00269     Vector<Size> result(rhs);
00270     int M=1;
00271     int N=result.size();
00272     double alpha=1;
00273     int lda=result.size();
00274     int ldb=1;
00275     dtrsm_("R","U","N","N",&M,&N,&alpha,my_lu.get_data_ptr(),&lda,result.get_data_ptr(),&ldb);
00276     dtrsm_("R","L","N","U",&M,&N,&alpha,my_lu.get_data_ptr(),&lda,result.get_data_ptr(),&ldb);
00277     for(int i=N-1; i>=0; i--){
00278       const int swaprow = my_IPIV[i]-1; // fortran arrays start at 1
00279       double temp = result[i];
00280       result[i]=result[swaprow];
00281       result[swaprow]=temp;
00282     }
00283     return result;
00284   }
00285 
00286   
00287   template <class Accessor>
00288   Vector<> backsub(const DynamicVector<Accessor>& rhs){
00289     assert(rhs.size() == my_lu.num_rows());
00290     Vector<> result(rhs);
00291     int M=1;
00292     int N=result.size();
00293     double alpha=1;
00294     int lda=result.size();
00295     int ldb=1;
00296     dtrsm_("R","U","N","N",&M,&N,&alpha,my_lu.get_data_ptr(),&lda,result.get_data_ptr(),&ldb);
00297     dtrsm_("R","L","N","U",&M,&N,&alpha,my_lu.get_data_ptr(),&lda,result.get_data_ptr(),&ldb);
00298     for(int i=N-1; i>=0; i--){
00299       const int swaprow = my_IPIV[i]-1; // fortran arrays start at 1
00300       double temp = result[i];
00301       result[i]=result[swaprow];
00302       result[swaprow]=temp;
00303     }
00304     return result;
00305   }
00306 
00307   Matrix<-1,-1,RowMajor> get_inverse(){
00308     Matrix<-1,-1,RowMajor> Inverse(my_lu);
00309     int N = my_lu.num_rows();
00310     int lda=my_lu.num_rows();
00311     int lwork=-1;
00312     double size;
00313     dgetri_(&N, Inverse.get_data_ptr(), &lda, my_IPIV, &size, &lwork, &my_info);
00314     lwork=int(size);
00315     double* WORK = new double[lwork];
00316     dgetri_(&N, Inverse.get_data_ptr(), &lda, my_IPIV, WORK, &lwork, &my_info);
00317     delete [] WORK;
00318     return Inverse;
00319   }
00320 
00321   Matrix<-1,-1,RowMajor>& get_lu(){return my_lu;}
00322   const Matrix<-1,-1,RowMajor>& get_lu()const {return my_lu;}
00323 
00324   inline int get_sign() const {
00325     int result=1;
00326     for(int i=0; i<my_lu.num_rows()-1; i++){
00327       if(my_IPIV[i] > i+1){
00328         result=-result;
00329       }
00330     }
00331     return result;
00332   }
00333 
00334   inline double determinant() const {
00335     double result = get_sign();
00336     for (int i=0; i<my_lu.num_rows(); i++){
00337       result*=my_lu(i,i);
00338     }
00339     return result;
00340   }
00341 
00342   int get_info() const { return my_info; }
00343 
00344 
00345  private:
00346   Matrix<-1,-1,RowMajor> my_lu;
00347   int my_info;
00348   int* my_IPIV;
00349 };
00350 
00351 
00352 
00353 #ifndef TOON_NO_NAMESPACE
00354 }
00355 #endif 
00356 
00357 
00358 
00359 
00360 
00361 #endif

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