rtc: rtc: rtcSMat.h Source File

00001 /*
00002  * Copyright (C) 2009
00003  * Robert Bosch LLC
00004  * Research and Technology Center North America
00005  * Palo Alto, California
00006  *
00007  * All rights reserved.
00008  *
00009  *------------------------------------------------------------------------------
00010  * project ....: Autonomous Technologies
00011  * file .......: rtcSMat.h
00012  * authors ....: Benjamin Pitzer
00013  * organization: Robert Bosch LLC
00014  * creation ...: 08/16/2006
00015  * modified ...: $Date: 2009-01-21 18:19:16 -0800 (Wed, 21 Jan 2009) $
00016  * changed by .: $Author: benjaminpitzer $
00017  * revision ...: $Revision: 14 $
00018  */
00019 #ifndef RTC_SMAT_H
00020 #define RTC_SMAT_H
00021 
00022 //== INCLUDES ==================================================================
00023 #include "rtc/rtcMath.h"
00024 #include "rtc/rtcVec.h"
00025 #include "rtc/rtcMat.h"
00026 
00027 //== NAMESPACES ================================================================
00028 namespace rtc {
00029 
00030 // Forward declarations
00031 template <class T, int M> class Vec; // M-d vector
00032 template <class T, int M, int N> class Mat; // MxN Matrix
00033 template <class T, int M> class SMat; // MxM Square Matrix
00034 
00041 template <class T, int M>
00042 class SMat: public Mat<T,M,M> {
00043 public:
00044   // Data
00045   using Mat<T,M,M>::x;
00046   using Mat<T,M,M>::set;
00047 
00048   // Constructors
00049   SMat();
00050   SMat(const T* d);
00051   SMat(const T diagVal);
00052   SMat(const Vec<T,M>& diagVec);
00053   SMat(const Mat<T,M,M>& m);
00054 
00055   // Casting Operation
00056   template <class U> SMat(const Mat<U,M,M>& m);
00057 
00058   // Mutators
00059   void setIdentity();
00060   void setDiag(const T a);
00061   void setDiag(const Vec<T,M>& diagVec);
00062   SMat<T,M>& leftMultiply(const SMat<T,M>& m);
00063 
00064   // Accessors
00065   Vec<T,M> getDiag();
00066 
00067   // Basic square matrix functions
00068   T trace() const;
00069   void transpose();
00070   SMat<T,M-1> minorMat(const int ip, const int jp) const;
00071   T det() const;
00072   SMat<T,M> inverted() const;
00073   int invert();
00074 
00075   // Decompositions
00076   int luDecomp(Vec<int,M>& indx, T* d = NULL);
00077   void luSolve(const Vec<int,M>& indx, Vec<T,M>& b);
00078   int choleskyDecomp();
00079   int choleskyDecomp(SMat<T,M>& r);
00080   void choleskySolve(Vec<T,M>& b);
00081 };
00082 
00083 //==============================================================================
00084 // SMat<T,M>
00085 //==============================================================================
00086 
00087 // Constructors
00088 
00091 template <class T, int M>
00092 inline SMat<T,M>::SMat() {}
00093 
00097 template <class T, int M>
00098 inline SMat<T,M>::SMat(const T* d) : Mat<T,M,M>(d) {}
00099 
00103 template <class T, int M>
00104 inline SMat<T,M>::SMat(const T diagVal) {
00105   set(T(0));
00106   setDiag(diagVal);
00107 }
00108 
00112 template <class T, int M>
00113 inline SMat<T,M>::SMat(const Vec<T,M>& diagVec) {
00114   set(T(0));
00115   setDiag(diagVec);
00116 }
00117 
00120 template <class T, int M>
00121 inline SMat<T,M>::SMat(const Mat<T,M,M>& m) : Mat<T,M,M>(m) {}
00122 
00123 // Casting Operation
00124 
00127 template <class T, int M> template <class U>
00128 inline SMat<T,M>::SMat(const Mat<U,M,M>& m) : Mat<T,M,M>(m) {}
00129 
00130 // Mutators
00131 
00135 template <class T, int M>
00136 inline void SMat<T,M>::setIdentity() {
00137   set(T(0));
00138   setDiag(T(1));
00139 }
00140 
00144 template <class T, int M>
00145 inline void SMat<T,M>::setDiag(const T diagVal) {
00146   for (int i=0,k=0;i<M;i++,k+=(M+1))
00147     x[k] = diagVal;
00148 }
00149 
00154 template <class T, int M>
00155 inline void SMat<T,M>::setDiag(const Vec<T,M>& diagVec) {
00156   for (int i=0,k=0;i<M;i++,k+=(M+1))
00157     x[k] = diagVec.x[i];
00158 }
00159 
00162 template <class T, int M>
00163 inline SMat<T,M>& SMat<T,M>::leftMultiply(const SMat<T,M>& m) {
00164   (*this) = m*(*this);
00165   return *this;
00166 }
00167 
00168 // Accessors
00169 
00173 template <class T, int M>
00174 inline Vec<T,M> SMat<T,M>::getDiag() {
00175   Vec<T,M> v;
00176   for (int i=0,k=0;i<M;i++,k+=(M+1)) v.x[i] = x[k];
00177   return v;
00178 }
00179 
00180 // Some basic square matrix functions
00181 
00184 template <class T, int M>
00185 inline T SMat<T,M>::trace() const {
00186   T sum = T(0);
00187   for (int i=0,k=0;i<M;i++,k+=(M+1)) sum += x[k];
00188   return sum;
00189 }
00190 
00193 template <class T, int M>
00194 inline void SMat<T,M>::transpose() {
00195   for (int i=0;i<M;i++) for (int j=i+1;j<M;j++) rtc_swap(x[i*M+j],x[j*M+i]);
00196 }
00197 
00200 template <class T, int M>
00201 inline SMat<T,M-1> SMat<T,M>::minorMat(const int ip, const int jp) const {
00202 #if MATMATH_CHECK_BOUNDS
00203   if (ip<0 || ip>M || jp<0 || jp>M) {
00204     std::stringstream ss;
00205     ss << "SMat<" << M << ">::minorMat(" << ip;
00206     ss << ", " << jp << "): index out of range\n";
00207     ss << std::flush;
00208     throw Exception(ss.str());
00209   }
00210 #endif
00211   SMat<T,M-1> m;
00212   for (int i=0,k=0;i<M;++i) if (i != ip) {
00213     for (int j=0,l=0;j<M;++j) if (j != jp) {
00214 m.x[k*(M-1)+l] = x[i*M+j];
00215 ++l;
00216     }
00217     ++k;
00218   }
00219   return m;
00220 }
00221 
00226 template <class T, int M>
00227 inline T SMat<T,M>::det() const {
00228   SMat<T,M> tmp(*this);
00229   Vec<int,M> indx; T d;
00230   tmp.luDecomp(indx,&d);
00231   return d*tmp.getDiag().prod();
00232 }
00233 
00237 template <class T, int M>
00238 inline SMat<T,M> SMat<T,M>::inverted() const {
00239   SMat<T,M> tmp(*this);
00240   Vec<int,M> indx; T d;
00241   if (tmp.luDecomp(indx,&d)) {
00242     throw Exception("SMat::inverted(): error, can't take the inverse of a singular matrix.");
00243   }
00244   SMat<T,M> inv;
00245   for (int i=0;i<M;i++) {
00246     Vec<T,M> col(T(0));
00247     col(i) = T(1);
00248     tmp.luSolve(indx,col);
00249     inv.setCol(i,col);
00250   }
00251   return inv;
00252 }
00253 
00257 template <class T, int M>
00258 inline int SMat<T,M>::invert() {
00259   SMat<T,M> tmp(*this);
00260   Vec<int,M> indx; T d;
00261   if (tmp.luDecomp(indx,&d))
00262     throw Exception("SMat::invert(): error, can't take the inverse of a singular matrix.");
00263 
00264   for (int i=0;i<M;i++) {
00265     Vec<T,M> col(T(0));
00266     col(i) = T(1);
00267     tmp.luSolve(indx,col);
00268     this->setCol(i,col);
00269   }
00270   return 0;
00271 }
00272 
00273 // Decomposition and Solve Routines (compliments of Numerical Recipes in C)
00274 
00282 template <class T, int M>
00283 inline int SMat<T,M>::luDecomp(Vec<int,M>& indx, T* d)
00284 {
00285   int i,imax=0,j,k;
00286   T big,dum,sum,temp;
00287   Vec<T,M> vv;
00288 
00289   if (d) *d=1.0;
00290   for (i=0;i<M;i++) {
00291     big=0.0;
00292     for (j=0;j<M;j++) if ((temp=fabs(x[i*M+j])) > big) big=temp;
00293     if (big == 0.0) return 1;
00294     vv(i)=T(1)/big;
00295   }
00296   for (j=0;j<M;j++) {
00297     for (i=0;i<j;i++) {
00298       sum=x[i*M+j];
00299       for (k=0;k<i;k++) sum -= x[i*M+k]*x[k*M+j];
00300       x[i*M+j]=sum;
00301     }
00302     big=0.0;
00303     for (i=j;i<M;i++) {
00304       sum=x[i*M+j];
00305       for (k=0;k<j;k++) sum -= x[i*M+k]*x[k*M+j];
00306       x[i*M+j]=sum;
00307       if ((dum=vv(i)*fabs(sum)) >= big) {
00308         big=dum;
00309         imax=i;
00310       }
00311     }
00312     if (j != imax) {
00313       for (k=0;k<M;k++) {
00314         dum=x[imax*M+k];
00315         x[imax*M+k]=x[j*M+k];
00316         x[j*M+k]=dum;
00317       }
00318       if (d) *d = -(*d);
00319       vv(imax)=vv(j);
00320     }
00321     indx.x[j]=imax;
00322     if (x[j*M+j] == 0.0) x[j*M+j] = T(1e-20);
00323     if (j != M-1) {
00324       dum=T(1)/(x[j*M+j]);
00325       for (i=j+1;i<M;i++) x[i*M+j] *= dum;
00326     }
00327   }
00328   return 0;
00329 }
00330 
00337 template <class T, int M>
00338 inline void SMat<T,M>::luSolve(const Vec<int,M>& indx, Vec<T,M>& b)
00339 {
00340   int i,ii=-1,ip,j;
00341   float sum;
00342   for (i=0;i<M;i++) {
00343     ip=indx.x[i]; sum=b.x[ip]; b.x[ip]=b.x[i];
00344     if (ii>=0) for (j=ii;j<i;j++) sum -= x[i*M+j]*b.x[j];
00345     else if (sum) ii=i;
00346     b.x[i]=sum;
00347   }
00348   for (i=(M-1);i>=0;i--) {
00349     sum=b.x[i];
00350     for (j=i+1;j<M;j++) sum -= x[i*M+j]*b.x[j];
00351     b.x[i]=sum/x[i*M+i];
00352   }
00353 }
00354 
00360 template <class T, int M>
00361 inline int SMat<T,M>::choleskyDecomp()
00362 {
00363   T sum; T p[M];
00364   for (int i=0;i<M;i++) for (int j=i,k;j<M;j++) {
00365     for (sum=x[i*M+j],k=i-1;k>=0;k--) sum -= x[i*M+k]*x[j*M+k];
00366     if (i == j) {
00367       if (sum <= T(0)) return 1;
00368       p[i] = sqrt(double(sum));
00369     } else x[j*M+i] = sum/p[i];
00370   }
00371   for (int i=0;i<M;i++) x[i*M+i] = p[i];
00372   return 0;
00373 }
00374 
00378 template <class T, int M>
00379 inline int SMat<T,M>::choleskyDecomp(SMat<T,M>& r)
00380 {
00381   r.set(*this);
00382   r.choleskyDecomp();
00383   for (int i=0;i<M-1;i++)
00384     for (int j=i+1;j<M;j++)
00385       r.x[i*M+j]=T(0);
00386   r.transpose();
00387   return 0;
00388 }
00389 
00395 template <class T, int M>
00396 inline void SMat<T,M>::choleskySolve(Vec<T,M>& b)
00397 {
00398   T sum;
00399   for (int i=0,k;i<M;i++) {
00400     for (sum=b.x[i],k=i-1;k>=0;k--) sum -= x[i*M+k]*b.x[k];
00401     b.x[i] = sum/x[i*M+i];
00402   }
00403   for (int i=M-1,k;i>=0;i--) {
00404     for (sum=b.x[i],k=i+1;k<M;k++) sum -= x[k*M+i]*b.x[k];
00405     b.x[i] = sum/x[i*M+i];
00406   }
00407 }
00408 
00409 //==============================================================================
00410 } // namespace rtc
00411 //==============================================================================
00412 #endif // RTC_SMAT_H defined
00413 //==============================================================================