$search
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 //==============================================================================