00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
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
00065 for(int i=N-1; i>=0; i--){
00066 const int swaprow = my_IPIV[i]-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
00089 for(int i=N-1; i>=0; i--){
00090 const int swaprow = my_IPIV[i]-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;
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;
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
00230 for(int i=N-1; i>=0; i--){
00231 const int swaprow = my_IPIV[i]-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
00255 for(int i=N-1; i>=0; i--){
00256 const int swaprow = my_IPIV[i]-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;
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;
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