rtcVarSMat.h
Go to the documentation of this 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 .......: rtcVarSMat.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_VARSMAT_H
00020 #define RTC_VARSMAT_H
00021 
00022 //== INCLUDES ==================================================================
00023 #include "rtc/rtcMath.h"
00024 #include "rtc/rtcVarVec.h"
00025 #include "rtc/rtcVarMat.h"
00026 #include "rtc/rtcArray2.h"
00027 
00028 //== NAMESPACES ================================================================
00029 namespace rtc {
00030 
00031 // Forward declarations
00032 template <class T> class VarVec; // r-d vector
00033 template <class T> class VarMat; // MxN Matrix
00034 template <class T> class VarSMat; // MxM Square Matrix
00035 
00042 template <class T>
00043 class VarSMat: public VarMat<T> {
00044 public:
00045   // Data
00046   using VarMat<T>::x;
00047   using VarMat<T>::set;
00048   using VarMat<T>::rows;
00049 
00050   // Constructors
00051   VarSMat();
00052   VarSMat(int size);
00053   VarSMat(int size, const T* d);
00054   VarSMat(int size, const T diagVal);
00055   VarSMat(int size, const VarVec<T>& diagVec);
00056   VarSMat(const Array2<T>& m);
00057 
00058   // Casting Operation
00059   template <class U> VarSMat(const VarMat<U>& m);
00060 
00061   // Mutators
00062   void setSize(int size);
00063   void setIdentity();
00064   void setDiag(const T a);
00065   void setDiag(const VarVec<T>& diagVec);
00066 
00067   // Accessors
00068   VarVec<T> getDiag();
00069   int size() const;
00070 
00071   // Basic square matrix functions
00072   T trace() const;
00073   void transpose();
00074   VarSMat<T> minorMat(const int ip, const int jp) const;
00075   T det() const;
00076   VarSMat<T> inverted() const;
00077   int invert();
00078 
00079   // Decompositions
00080   void luDecomp(VarVec<int>& indx, T* d = NULL);
00081   void luSolve(const VarVec<int>& indx, VarVec<T>& b);
00082   void choleskyDecomp();
00083   void choleskyDecomp(VarSMat<T>& r);
00084   void choleskySolve(VarVec<T>& b);
00085 }; // end class VarSMat<T>
00086 
00087 // Declare a few common typdefs
00088 typedef VarSMat<bool> VarSMatb;
00089 typedef VarSMat<char> VarSMatc;
00090 typedef VarSMat<unsigned char> VarSMatuc;
00091 typedef VarSMat<int> VarSMati;
00092 typedef VarSMat<float> VarSMatf;
00093 typedef VarSMat<double> VarSMatd;
00094 
00095 //==============================================================================
00096 // VarSMat<T>
00097 //==============================================================================
00098 
00099 // Constructors
00100 
00103 template <class T>
00104 inline VarSMat<T>::VarSMat() : VarMat<T>() {}
00105 
00108 template <class T>
00109 inline VarSMat<T>::VarSMat(int size) : VarMat<T>(size,size) {}
00110 
00115 template <class T>
00116 inline VarSMat<T>::VarSMat(int size, const T* d) : VarMat<T>(size,size,d) {}
00117 
00122 template <class T>
00123 inline VarSMat<T>::VarSMat(int size, const T diagVal) : VarMat<T>(size,size) {
00124   set(T(0));
00125   setDiag(diagVal);
00126 }
00127 
00132 template <class T>
00133 inline VarSMat<T>::VarSMat(int size, const VarVec<T>& diagVec) : VarMat<T>(size,size) {
00134   set(T(0));
00135   setDiag(diagVec);
00136 }
00137 
00140 template <class T>
00141 inline VarSMat<T>::VarSMat(const Array2<T>& m) : VarMat<T>(m) {}
00142 
00143 // Casting Operation
00144 
00147 template <class T> template <class U>
00148 inline VarSMat<T>::VarSMat(const VarMat<U>& m) : VarMat<T>(m) {}
00149 
00150 // Mutators
00151 
00155 template <class T>
00156 inline void VarSMat<T>::setSize(int size) {
00157   VarMat<T>::setSize(size,size);
00158 }
00159 
00163 template <class T>
00164 inline void VarSMat<T>::setIdentity() {
00165   set(T(0));
00166   setDiag(T(1));
00167 }
00168 
00172 template <class T>
00173 inline void VarSMat<T>::setDiag(const T diagVal) {
00174   for (int i=0,k=0;i<size();i++,k+=(size()+1))
00175     x[k] = diagVal;
00176 }
00177 
00182 template <class T>
00183 inline void VarSMat<T>::setDiag(const VarVec<T>& diagVec) {
00184 #if MATMATH_CHECK_BOUNDS
00185   if (size()!=diagVec.length()) {
00186     std::stringstream ss;
00187     ss << "VarSMat<" << size() << "," << size() << ">::setDiag (";
00188     ss << "VarVec<" << diagVec.length() << ">): not a valid operation\n";
00189     ss << std::flush;
00190     throw Exception(ss.str());
00191   }
00192 #endif
00193   for (int i=0,k=0;i<size();i++,k+=(size()+1))
00194     x[k] = diagVec.x[i];
00195 }
00196 
00197 // Accessors
00198 
00202 template <class T>
00203 inline VarVec<T> VarSMat<T>::getDiag() {
00204   VarVec<T> v(size());
00205   for (int i=0,k=0;i<size();i++,k+=(size()+1)) v.x[i] = x[k];
00206   return v;
00207 }
00208 
00212 template <class T>
00213 inline int VarSMat<T>::size() const {
00214   return rows();
00215 }
00216 
00217 // Some basic square matrix functions
00218 
00221 template <class T>
00222 inline T VarSMat<T>::trace() const {
00223   T sum = T(0);
00224   for (int i=0,k=0;i<size();i++,k+=(size()+1)) sum += x[k];
00225   return sum;
00226 }
00227 
00230 template <class T>
00231 inline void VarSMat<T>::transpose() {
00232   for (int i=0;i<size();i++) for (int j=i+1;j<size();j++) rtc_swap(x[i*size()+j],x[j*size()+i]);
00233 }
00234 
00237 template <class T>
00238 inline VarSMat<T> VarSMat<T>::minorMat(const int ip, const int jp) const {
00239 #if MATMATH_CHECK_BOUNDS
00240   if (ip<0 || ip>size() || jp<0 || jp>size()) {
00241     std::stringstream ss;
00242     ss << "VarSMat<" << size() << ">::minorMat(" << ip;
00243     ss << ", " << jp << "): index out of range\n";
00244     ss << std::flush;
00245     throw Exception(ss.str());
00246   }
00247 #endif
00248   VarSMat<T> m(size()-1);
00249   for (int i=0,k=0;i<size();++i) if (i != ip) {
00250     for (int j=0,l=0;j<size();++j) if (j != jp) {
00251       m.x[k*(size()-1)+l] = x[i*size()+j];
00252       ++l;
00253     }
00254     ++k;
00255   }
00256   return m;
00257 }
00258 
00263 template <class T>
00264 inline T VarSMat<T>::det() const {
00265   VarSMat<T> tmp(*this);
00266   VarVec<int> indx(size()); T d;
00267   tmp.luDecomp(indx,&d);
00268   return d*tmp.getDiag().prod();
00269 }
00270 
00274 template <class T>
00275 inline VarSMat<T> VarSMat<T>::inverted() const {
00276   VarSMat<T> tmp(*this);
00277   VarVec<int> indx(size()); T d;
00278   try {
00279     tmp.luDecomp(indx,&d);
00280   }
00281   catch (Exception e) {
00282     throw Exception( "VarSMat::invert(): error, can't take inverse of singular matrix.");
00283   }
00284   VarSMat<T> inv(size());
00285   for (int i=0;i<size();i++) {
00286     VarVec<T> col(size(),T(0));
00287     col(i) = T(1);
00288     tmp.luSolve(indx,col);
00289     inv.setCol(i,col);
00290   }
00291   return inv;
00292 }
00293 
00297 template <class T>
00298 inline int VarSMat<T>::invert() {
00299   VarSMat<T> tmp(*this);
00300   VarVec<int> indx(size()); T d;
00301   try {
00302     tmp.luDecomp(indx,&d);
00303   }
00304   catch (Exception e) {
00305     throw Exception( "VarSMat::inverted(): error, can't take inverse of singular matrix.");
00306   }
00307   for (int i=0;i<size();i++) {
00308     VarVec<T> col(size(),T(0));
00309     col(i) = T(1);
00310     tmp.luSolve(indx,col);
00311     this->setCol(i,col);
00312   }
00313   return 0;
00314 }
00315 
00316 // Decomposition and Solve Routines (compliments of Numerical Recipes in C)
00317 
00325 template <class T>
00326 inline void VarSMat<T>::luDecomp(VarVec<int>& indx, T* d)
00327 {
00328   int r = size();
00329   if(r!=indx.length()) indx.setSize(r);
00330   int i,imax=0,j,k;
00331   T big,dum,sum,temp;
00332   VarVec<T> vv(r);
00333 
00334   if (d)
00335     *d=1.0;
00336   for (i=0; i<r; i++) {
00337     big=0.0;
00338     for (j=0; j<r; j++)
00339       if ((temp=fabs(x[i*r+j])) > big)
00340         big=temp;
00341     if (big == 0.0) {
00342       std::stringstream ss;
00343       ss << "VarSMat<" << r << "," << r << ">::luDecomp(VarVec<" << r << ">& indx, T* d)";
00344       ss << ": matrix is singular.";
00345       throw Exception(ss.str());
00346     }
00347     vv(i)=T(1)/big;
00348   }
00349   for (j=0; j<r; j++) {
00350     for (i=0; i<j; i++) {
00351       sum=x[i*r+j];
00352       for (k=0; k<i; k++)
00353         sum -= x[i*r+k]*x[k*r+j];
00354       x[i*r+j]=sum;
00355     }
00356     big=0.0;
00357     for (i=j; i<r; i++) {
00358       sum=x[i*r+j];
00359       for (k=0; k<j; k++)
00360         sum -= x[i*r+k]*x[k*r+j];
00361       x[i*r+j]=sum;
00362       if ((dum=vv(i)*fabs(sum)) >= big) {
00363         big=dum;
00364         imax=i;
00365       }
00366     }
00367     if (j != imax) {
00368       for (k=0; k<r; k++) {
00369         dum=x[imax*r+k];
00370         x[imax*r+k]=x[j*r+k];
00371         x[j*r+k]=dum;
00372       }
00373       if (d)
00374         *d = -(*d);
00375       vv(imax)=vv(j);
00376     }
00377     indx.x[j]=imax;
00378     if (x[j*r+j] == 0.0)
00379       x[j*r+j] = T(1e-20);
00380     if (j != r-1) {
00381       dum=T(1)/(x[j*r+j]);
00382       for (i=j+1; i<r; i++)
00383         x[i*r+j] *= dum;
00384     }
00385   }
00386 }
00387 
00394 template <class T>
00395 inline void VarSMat<T>::luSolve(const VarVec<int>& indx, VarVec<T>& b)
00396 {
00397 #if MATMATH_CHECK_BOUNDS
00398   if (size()!=indx.length() || size()!=b.length()) {
00399     std::stringstream ss;
00400     ss << "VarSMat<" << size() << "," << size() << ">::luSolve (";
00401     ss << "VarVec<" << indx.length() << ">, VarVec<" << b.length() << ">): not a valid operation\n";
00402     throw Exception(ss.str());
00403   }
00404 #endif
00405   int i,ii=-1,ip,j;
00406   T sum;
00407   for (i=0;i<size();i++) {
00408     ip=indx.x[i]; sum=b.x[ip]; b.x[ip]=b.x[i];
00409     if (ii>=0) for (j=ii;j<i;j++) sum -= x[i*size()+j]*b.x[j];
00410     else if (sum) ii=i;
00411     b.x[i]=sum;
00412   }
00413   for (i=(size()-1);i>=0;i--) {
00414     sum=b.x[i];
00415     for (j=i+1;j<size();j++) sum -= x[i*size()+j]*b.x[j];
00416     b.x[i]=sum/x[i*size()+i];
00417   }
00418 }
00419 
00425 template <class T>
00426 inline void VarSMat<T>::choleskyDecomp()
00427 {
00428   int r = size();
00429   T sum;
00430   T p[r];
00431   for (int i=0; i<r; i++)
00432     for (int j=i, k; j<r; j++) {
00433       for (sum=x[i*r+j], k=i-1; k>=0; k--)
00434         sum -= x[i*r+k]*x[j*r+k];
00435       if (i == j) {
00436         if (sum <= T(0)) {
00437           std::stringstream ss;
00438           ss << "VarSMat<" << r << "," << r << ">::choleskyDecomp()";
00439           ss << ": matrix is not positive definite.";
00440           throw Exception(ss.str());
00441         }
00442         p[i] = sqrt(double(sum));
00443       } else
00444         x[j*r+i] = sum/p[i];
00445     }
00446   for (int i=0; i<r; i++)
00447     x[i*r+i] = p[i];
00448 }
00449 
00453 template <class T>
00454 inline void VarSMat<T>::choleskyDecomp(VarSMat<T>& _r)
00455 {
00456   _r.set(*this);
00457   _r.choleskyDecomp();
00458   for (int i=0;i<size()-1;i++)
00459     for (int j=i+1;j<size();j++)
00460       _r.x[i*size()+j]=T(0);
00461   _r.transpose();
00462 }
00463 
00469 template <class T>
00470 inline void VarSMat<T>::choleskySolve(VarVec<T>& b) {
00471 #if MATMATH_CHECK_BOUNDS
00472   if (size()!=b.length()) {
00473     std::stringstream ss;
00474     ss << "VarSMat<" << size() << "," << size() << ">::choleskySolve (";
00475     ss << "VarVec<" << b.length() << ">): not a valid operation\n";
00476     throw Exception(ss.str());
00477   }
00478 #endif
00479   T sum;
00480   for (int i=0,k;i<size();i++) {
00481     for (sum=b.x[i],k=i-1;k>=0;k--) sum -= x[i*size()+k]*b.x[k];
00482     b.x[i] = sum/x[i*size()+i];
00483   }
00484   for (int i=size()-1,k;i>=0;i--) {
00485     for (sum=b.x[i],k=i+1;k<size();k++) sum -= x[k*size()+i]*b.x[k];
00486     b.x[i] = sum/x[i*size()+i];
00487   }
00488 }
00489 
00490 //==============================================================================
00491 } // NAMESPACE rtc
00492 //==============================================================================
00493 #endif // RTC_VARSMAT_H defined
00494 //==============================================================================


rtc
Author(s): Benjamin Pitzer
autogenerated on Thu Jan 2 2014 11:04:54