matrix_n.hpp
Go to the documentation of this file.
```00001 // HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
00002 // Copyright (C) 2010 G. Grisetti, R. Kümmerle, C. Stachniss
00003 //
00004 // HOG-Man is free software: you can redistribute it and/or modify
00005 // it under the terms of the GNU Lesser General Public License as published
00006 // by the Free Software Foundation, either version 3 of the License, or
00007 // (at your option) any later version.
00008 //
00009 // HOG-Man 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
00012 // GNU Lesser General Public License for more details.
00013 //
00014 // You should have received a copy of the GNU Lesser General Public License
00015 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016
00017
00018 template <int Rows, int Cols, typename Base>
00019 _Matrix<Rows,Cols,Base>& _Matrix<Rows, Cols, Base>::operator += (const _Matrix<Rows, Cols, Base>& v){
00020   for (int i=0; i<rows(); i++)
00021     for (int j=0; j<cols(); j++)
00022       _allocator[i][j]+=v._allocator[i][j];
00023   return *this;
00024 }
00025
00026 template <int Rows, int Cols, typename Base>
00027   _Matrix<Rows,Cols,Base>& _Matrix<Rows,Cols,Base>::operator -= (const _Matrix<Rows,Cols,Base>& v){
00028   for (int i=0; i<rows(); i++)
00029     for (int j=0; j<cols(); j++)
00030       _allocator[i][j]-=v._allocator[i][j];
00031   return *this;
00032 }
00033
00034 template <int Rows, int Cols, typename Base>
00035 _Matrix<Rows,Cols,Base> _Matrix<Rows, Cols, Base>::operator - (const _Matrix<Rows, Cols, Base>& v) const {
00036   _Matrix<Rows, Cols, Base> aux(*this);
00037   aux-=v;
00038   return aux;
00039 }
00040
00041 template <int Rows, int Cols, typename Base>
00042   _Matrix<Rows,Cols,Base> _Matrix<Rows, Cols, Base>::operator + (const _Matrix<Rows, Cols, Base>& v) const {
00043   _Matrix<Rows, Cols, Base> aux(*this);
00044   aux+=v;
00045   return aux;
00046 }
00047
00048 template <int Rows, int Cols, typename Base>
00049   _Matrix<Rows,Cols,Base>& _Matrix<Rows,Cols,Base>::operator *= (Base c){
00050   for (int i=0; i<rows(); i++)
00051     for (int j=0; j<cols(); j++)
00052       _allocator[i][j]*=c;
00053   return *this;
00054 }
00055
00056 template <int Rows, int Cols, typename Base>
00057   _Matrix<Rows,Cols,Base> _Matrix<Rows,Cols,Base>::operator * (Base c) const{
00058   _Matrix<Rows,Cols,Base> aux(*this);
00059   aux*=c;
00060   return aux;
00061 }
00062
00063
00064 template <int Rows, int Cols, typename Base>
00065 _Matrix<Cols, Rows, Base> _Matrix<Rows, Cols, Base>::transpose() const{
00066   _Matrix<Cols, Rows, Base> aux(cols(),rows());
00067   for (int i=0; i<rows(); i++)
00068     for (int j=0; j<cols(); j++)
00069       aux._allocator[j][i]=_allocator[i][j];
00070   return aux;
00071 }
00072
00073 template <int Rows, int Cols, typename Base>
00074 _Matrix<Cols, Rows, Base>& _Matrix<Rows, Cols, Base>::transposeInPlace(){
00075   if (rows() == cols()) {
00076     for (int i=0; i<rows(); i++)
00077       for (int j=i+1; j<cols(); j++)
00078         swap((*this)[i][j], (*this)[j][i]);
00079   } else {
00080     _Matrix<Cols, Rows, Base> aux(cols(), rows());
00081     for (int i=0; i<rows(); i++)
00082       for (int j=0; j<cols(); j++)
00083         aux._allocator[j][i]=_allocator[i][j];
00084     *this=aux;
00085   }
00086   return *this;
00087 }
00088
00089
00090
00091 template <int Rows, int Cols, typename Base>
00092 _Matrix<Rows, Rows, Base> _Matrix<Rows, Cols, Base>::eye(Base factor, int size_){
00093   _Matrix<Rows, Rows, Base> aux(size_, size_);
00094   for (int i=0; i<aux.rows(); i++)
00095     for (int j=0; j<aux.cols(); j++)
00096       aux._allocator[i][j]=Base(0);
00097   for (int i=0; i<aux.rows(); i++){
00098     aux._allocator[i][i]=Base(factor);
00099   }
00100   return aux;
00101 }
00102
00103 template <int Rows, int Cols, typename Base>
00104 _Matrix<Rows, Rows, Base> _Matrix<Rows, Cols, Base>::diag(const _Vector<Rows, Base>& v){
00105   _Matrix<Rows, Rows, Base> aux(v.size(), v.size());
00106   for (int i=0; i<aux.rows(); i++)
00107     for (int j=0; j<aux.cols(); j++)
00108       aux._allocator[i][j]=Base(0);
00109   for (int i=0; i<aux.rows(); i++){
00110     aux._allocator[i][i]=v[i];
00111   }
00112   return aux;
00113 }
00114
00115 template <int Rows, int Cols, typename Base>
00116 _Matrix<Rows, Rows, Base> _Matrix<Rows, Cols, Base>::permutation(const _Vector<Rows, int>& p){
00117   _Matrix<Rows, Rows, Base> aux(p.size(), p.size());
00118   for (int i=0; i<aux.rows(); i++)
00119     for (int j=0; j<aux.cols(); j++)
00120       aux._allocator[i][j]=Base(0);
00121   for (int i=0; i<aux.rows(); i++){
00122     aux._allocator[p[i]][i]=p[i];
00123   }
00124   return aux;
00125 }
00126
00127 template <int Rows, int Cols, typename Base>
00128 _Matrix<Rows, Rows, Base> _Matrix<Rows, Cols, Base>::outerProduct(const _Vector<Rows, Base>& v1, const _Vector<Rows, Base>& v2){
00129   assert(v1.size()==v2.size());
00130   _Matrix<Rows, Rows, Base> aux(v1.size(), v1.size());
00131   for (int i=0; i<aux.rows(); i++)
00132     for (int j=0; j<aux.cols(); j++)
00133       aux._allocator[i][j]=v1[i]*v2[j];
00134   return aux;
00135 }
00136
00137
00138
00139 template <int Rows, int Cols, typename Base>
00140     template <int Rows1, int Cols1>
00141 _Matrix<Rows, Cols1, Base> _Matrix<Rows, Cols, Base>::operator*(const _Matrix<Rows1, Cols1, Base>& m) const{
00142   _Matrix<Cols1, Rows1, Base> tm=m.transpose(); // this makes evetyrhing cache friendly
00143   _Matrix<Rows, Cols1, Base> aux(rows(),m.cols());
00144   for (int i=0; i<aux.rows(); i++)
00145     for (int j=0; j<aux.cols(); j++){
00146       Base acc(0);
00147       for (int k=0; k<cols(); k++){
00148         acc+=_allocator[i][k]*tm[j][k];
00149       }
00150       aux._allocator[i][j]=acc;
00151     }
00152   return aux;
00153 }
00154
00155 template <int Rows, int Cols, typename Base>
00156 _Matrix<Rows, Cols, Base>& _Matrix<Rows, Cols, Base>::operator*=(const _Matrix<Rows, Cols, Base>& m) {
00157   assert(rows()==cols());
00158   *this=(*this)*m;
00159   return *this;
00160 }
00161
00162
00163
00164 template <int Rows, int Cols, typename Base>
00165   void _Matrix<Rows, Cols, Base>::swapRows(int r1, int r2){
00166   for (int i=0; i<cols(); i++){
00167     Base aux=_allocator[r1][i];
00168     _allocator[r1][i]=_allocator[r2][i];
00169     _allocator[r2][i]=aux;
00170   }
00171 }
00172
00173 template <int Rows, int Cols, typename Base>
00174   void _Matrix<Rows, Cols, Base>::sumRow(int dest, Base destFactor, int src, Base srcFactor){
00175   for (int i=0; i<cols(); i++){
00176     _allocator[dest][i]=_allocator[dest][i]*destFactor+_allocator[src][i]*srcFactor;
00177   }
00178 }
00179
00180 template <int Rows, int Cols, typename Base>
00181   void _Matrix<Rows, Cols, Base>::multRow(int dest, Base destFactor){
00182   for (int i=0; i<cols(); i++){
00183     _allocator[dest][i]*=destFactor;
00184   }
00185 }
00186
00187
00188 template <int Rows, int Cols, typename Base>
00189 _Matrix<Rows, Rows, Base> _Matrix<Rows, Cols, Base>::inverse() const {
00190   assert(rows()==cols() && "Matrix not square");
00191
00192
00193   if(rows()==2){
00194     _Matrix<Rows, Rows, Base> ret(*this);
00195     Base d=det();
00196     if (fabs(d)<=Base(0)){
00197       std::cerr << *this << std::endl;
00198       assert(0 && "Matrix not invertible");
00199     }
00200     ret[0][0]=_allocator[1][1];
00201     ret[1][1]=_allocator[0][0];
00202     ret[1][0]=-_allocator[1][0];
00203     ret[0][1]=-_allocator[0][1];
00204     ret*=(Base(1.)/d);
00205     return ret;
00206   }
00207
00208   _Matrix<Rows, Rows, Base> L(*this);
00209   _Matrix<Rows, Rows, Base> U=_Matrix<Rows, Rows, Base>::eye(Base(1),rows());
00210
00211
00212   for (int i=0;i<rows();i++) {
00213     //pivoting
00214     Base absMax=Base(0);
00215     int k=-1;
00216     for (int q=i;q<rows();q++){
00217       Base absVal=fabs(L._allocator[q][i]);
00218       if (absVal>absMax){
00219         k=q;
00220         absMax=absVal;
00221       }
00222     }
00223
00224     if (k==-1) {
00225       std::cerr << "Matrix not invertible" << std::endl;
00226       std::cerr << *this << std::endl;
00227       assert(0 && "Matrix not invertible");
00228     } else {
00229       L.swapRows(k,i);
00230       U.swapRows(k,i);
00231     }
00232
00233     Base val=L._allocator[i][i];
00234     val=Base(1)/val;
00235     L.multRow(i,val);
00236     U.multRow(i,val);
00237
00238     for (int j=0;j<rows();j++)
00239       if (j!=i) {
00240         Base tmp=-L._allocator[j][i];
00241         L.sumRow(j,Base(1),i,tmp);
00242         U.sumRow(j,Base(1),i,tmp);
00243       }
00244   }
00245   return U;
00246 }
00247
00248 template <int Rows, int Cols, typename Base>
00249 _Matrix<Rows, Rows, Base> _Matrix<Rows, Cols, Base>::cholesky() const {
00250   assert(Rows==Cols);
00251   _Matrix<Rows, Rows, Base> L=_Matrix<Rows, Rows, Base>::eye(0.,rows());
00252   Base diag[rows()];
00253   for (int i=0; i<rows(); i++)
00254     for (int j=0; j<=i; j++){
00255       if (i==j){
00256         Base aux=_allocator[j][j];
00257         for (int k=0; k<j; k++)
00258           aux-=L[i][k]*L[i][k];
00259         assert (aux>Base(0));
00260         L[j][j]=sqrt(aux);
00261         diag[j]=1./L[j][j];
00262       } else {
00263         Base aux=_allocator[i][j];
00264         for (int k=0; k<j; k++)
00265           aux-=L[i][k]*L[j][k];
00266         L[i][j]=aux*diag[j];
00267       }
00268     }
00269   return L;
00270 }
00271
00272 template <int Rows, int Cols, typename Base>
00273 Base _Matrix<Rows, Cols, Base>::det() const {
00274   assert(Rows==Cols);
00275
00276   if (rows()==2 && cols()==2){
00277     return _allocator[0][0]*_allocator[1][1]-_allocator[1][0]*_allocator[0][1];
00278   }
00279
00280
00281   _Matrix<Rows, Rows, Base> aux(*this);
00282   Base d=Base(1);
00283   for (int i=0;i<rows();i++) {
00284     int k=i;
00285     for (;k<Rows&&aux[k][i]==Base(0);k++) {}
00286     if (k>=Rows) return Base(0);
00287     Base val=aux[k][i];
00288     for (int j=0;j<rows();j++) {
00289       aux[k][j]/=val;
00290     }
00291     d=d*val;
00292     if (k!=i) {
00293       for (int j=0;j<rows();j++) {
00294         Base tmp=aux[k][j];
00295         aux[k][j]=aux[i][j];
00296         aux[i][j]=tmp;
00297       }
00298       d=-d;
00299     }
00300     for (int j=i+1;j<rows();j++){
00301       Base tmp=aux[j][i];
00302       if (!(tmp==Base(0)) ){
00303         for (int l=0;l<rows();l++) {
00304           aux[j][l]=aux[j][l]-tmp*aux[i][l];
00305         }
00306       }
00307     }
00308   }
00309   return d;
00310 }
00311
00312 template <int Rows, int Cols, typename Base>
00313 int _Matrix<Rows, Cols, Base>::nullSpace(_Matrix<Rows, Cols, Base>& null_, Base epsilon) const{
00314
00315   // reduce the matrix to triangular form
00316   _Matrix<Rows, Cols, Base> L(*this);
00317   bool freeVars[rows()];
00318   for (int i=0; i<rows(); i++)
00319     freeVars[i]=false;
00320
00321   int fidx=0;
00322   for (int i=0;i<rows();i++) {
00323     //pivoting
00324     Base absMax=epsilon;
00325     int k=-1;
00326     for (int q=i;q<rows();q++){
00327       Base absVal=fabs(L[q][fidx]);
00328       if (absVal>absMax){
00329         k=q;
00330         absMax=absVal;
00331       }
00332     }
00333
00334     // seek for free variables and do a pivoting for proper conditioning
00335     if (k==-1) {
00336       freeVars[fidx]=true;
00337       fidx++;
00338       continue;
00339     } else {
00340       L.swapRows(k,i);
00341     }
00342
00343     // clear the bounded vars of the matrix in this column
00344     L.multRow(i,Base(1)/L._allocator[i][fidx]);
00345     for (int j=0;j<rows();j++)
00346       if (j!=i)
00347         L.sumRow(j,Base(1),i,-L[j][fidx]);
00348     fidx++;
00349   }
00350
00351   while (fidx<cols()){
00352     freeVars[fidx]=true;
00353     fidx++;
00354   }
00355   null_.fill(Base(0));
00356   int fv=0;
00357   for (int i=0; i<cols(); i++){
00358     if (freeVars[i]){
00359       int bv=0;
00360       for (int j=0; j<i; j++){
00361         if (! freeVars[j]){
00362           null_[fv][j]=-L[bv][i];
00363           bv++;
00364         }
00365       }
00366       null_[fv][i]=Base(1);
00367       fv++;
00368     }
00369   }
00370   return fv;
00371 }
00372
00373 template <int R, int C, typename Base>
00374 std::ostream& operator << (std::ostream& os, const _Matrix<R, C, Base>& m){
00375   for (int j=0; j<m.rows(); j++){
00376     if (j > 0)
00377       os << std::endl;
00378     for (int i=0; i<m.cols(); i++){
00379       if (i>0)
00380         os << " ";
00381       os << (m[j][i]);
00382     }
00383   }
00384   return os;
00385 }
00386
00387 template <int Rows, int Cols, typename Base>
00388 _Vector<Rows, Base> _Matrix<Rows, Cols, Base>::operator* (const _Vector<Cols, Base>& v) const{
00389   _Vector<Rows, Base> aux;
00390   for (int i=0; i<rows(); i++){
00391     Base acc=Base(0);
00392     for (int j=0; j<cols(); j++)
00393       acc+=_allocator[i][j]*v[j];
00394     aux[i]=acc;
00395   }
00396   return aux;
00397 }
00398
00399 template <int N, typename Base>
00400 _Vector<N, Base>::operator _Matrix<N, 1, Base> () const{
00401   _Matrix<N, 1, Base> aux;
00402   for(int i=0; i<size(); i++)
00403     aux[0][i]=_allocator[i];
00404   return aux;
00405 }
00406
00407 template <int Rows, int Cols, typename Base>
00408 void _Matrix<Rows, Cols, Base>::fill(Base scalar)
00409 {
00410   for (int j=0; j<rows(); j++)
00411     for (int i=0; i<cols(); i++)
00412       (*this)[j][i] = scalar;
00413 }
00414
00415 template <int R, int C, typename Base>
00416 _Matrix<R, C, Base> operator* (Base x, const _Matrix<R, C, Base>& m)
00417 {
00418   return m * x;
00419 }
```

hogman_minimal
Author(s): Maintained by Juergen Sturm
autogenerated on Wed Dec 26 2012 15:36:48