rtcVarMat.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 .......: rtcVarMat.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_VARMAT_H
00020 #define RTC_VARMAT_H
00021 
00022 //== INCLUDES ==================================================================
00023 #include "rtc/rtcMath.h"
00024 #include "rtc/rtcVec2.h"
00025 #include "rtc/rtcVarVec.h"
00026 #include "rtc/rtcVarSMat.h"
00027 #include "rtc/rtcArray2.h"
00028 
00029 //== NAMESPACES ================================================================
00030 namespace rtc {
00031 
00033 template <class T> class VarVec;  // M-d vector
00034 template <class T> class VarMat;  // MxN Matrix
00035 template <class T> class VarSMat; // MxM Matrix
00036 
00059 template <class T>
00060 class VarMat : public Array2<T> {
00061 public:
00062   // Constructors
00063   VarMat();
00064   VarMat(int rows, int cols);
00065   VarMat(int rows, int cols, const T* d);
00066   VarMat(int rows, int cols, const T a);
00067   VarMat(const Array<T,2>& a);
00068 
00069   // Cast Operation
00070   template <class U> VarMat(const VarMat<U>& m);
00071   template <class U, int M, int N> VarMat(const Mat<U,M,N>& m);
00072 
00073   // Mutators
00074   void setRow(int i, const VarVec<T>& v);
00075   void setCol(int j, const VarVec<T>& v);
00076   void setSubMat(int r, int c, const VarMat<T>& m);
00077 
00078   // Casting Mutators
00079   template <class U> void set(const VarMat<U>& m);
00080   template <class U, int M, int N> void set(const Mat<U,M,N>& m);
00081   template <class U> VarMat<T>& operator = (const VarMat<U>& m);
00082   template <class U, int M, int N> VarMat<T>& operator = (const Mat<U,M,N>& m);
00083 
00084   // Accessors
00085   int length() const;
00086   VarVec<T> getRow(int r) const;
00087   VarVec<T> getCol(int c) const;
00088   VarMat<T> getSubMat(int i, int j, int sub_rows, int sub_cols);
00089 
00090   // Addition and subtraction: <matrix> +/- <matrix>
00091   VarMat<T>& add(const VarMat<T>& m);
00092   void addSubMat(int r, int c, const VarMat<T>& m);
00093   VarMat<T>& subtract(const VarMat<T>& m);
00094   void subtractSubMat(int r, int c, const VarMat<T>& m);
00095   VarMat<T> operator + (const VarMat<T>& m) const;
00096   void operator += (const VarMat<T>& m);
00097   VarMat<T> operator - (const VarMat<T>& m) const;
00098   void operator -= (const VarMat<T>& m);
00099   VarMat<T> operator - () const;
00100 
00101   // Addition and subtraction: <matrix> +/- <scalar>
00102   VarMat<T>& add(const T a);
00103   VarMat<T>& subtract(const T a);
00104   VarMat<T> operator + (const T a) const;
00105   void operator += (const T a);
00106   VarMat<T> operator - (const T a) const;
00107   void operator -= (const T a);
00108 
00109   // Multiplication and division: <matrix> *// <scalar>
00110   VarMat<T> operator * (const T a) const;
00111   void operator *= (const T a);
00112   VarMat<T> operator / (const T a) const;
00113   void operator /= (const T a);
00114 
00115   // Multiplication: <matrix> * <vector>
00116   VarVec<T> operator * (const VarVec<T>& v) const;
00117   VarMat<T> operator * (const VarMat<T>& m) const;
00118   void operator *= (const VarMat<T>& m);
00119 
00120   // Equality and inequality tests
00121   VarMat<bool> operator == (const VarMat<T>& m) const;
00122   VarMat<bool> operator != (const VarMat<T>& m) const;
00123   VarMat<bool> operator >= (const VarMat<T>& m) const;
00124   VarMat<bool> operator <= (const VarMat<T>& m) const;
00125   VarMat<bool> operator > (const VarMat<T>& m) const;
00126   VarMat<bool> operator < (const VarMat<T>& m) const;
00127   int compareTo(const VarMat<T>& m) const;
00128   bool equalTo(const VarMat<T>& m, const T tol = T(0)) const;
00129 
00130   // Other matrix operations
00131   VarMat<T> transposed() const;
00132 
00133   // Random matrix
00134   VarMat<T> uniformRand(const T a = T(0), const T b = T(1));
00135   VarMat<T> normalRand(const T mean = T(0), const T stdev = T(1));
00136 //    static VarMat<T> multivariateGauss(const Vec<T,M>& mean, const SMat<T,M>& cov);
00137 
00138   // General elementwise operations
00139   void perform(T (*mathFun)(T));
00140 
00141   // Reductions: Max/Min, Sum/Product
00142   //T max() const;
00143   //T min() const;
00144   VarVec<T> sum() const;
00145   //T prod() const;
00146 
00147   // Bilinear Interpolation
00148   T interpolate(const float i, const float j) const;
00149 
00150   // statistical operations
00151   VarVec<T> meanOfRows() const;
00152   VarSMat<T> covarianceMatrixOfRows() const;
00153 
00154   // inherit member data and functions of parent
00155   using Array<T,2>::x;
00156   using Array<T,2>::len;
00157   using Array<T,2>::set;
00158   using Array<T,2>::mul;
00159   using Array2<T>::setSize;
00160   using Array2<T>::rows;
00161   using Array2<T>::columns;
00162 };
00163 
00164 // Declare a few common typdefs
00165 typedef VarMat<bool> VarMatb;
00166 typedef VarMat<char> VarMatc;
00167 typedef VarMat<unsigned char> VarMatuc;
00168 typedef VarMat<int> VarMati;
00169 typedef VarMat<float> VarMatf;
00170 typedef VarMat<double> VarMatd;
00171 
00172 // Global operators to for cases where VarMat<T>
00173 // is the second argument in a binary operator
00174 template <class T> VarMat<T> operator * (const T a, const VarMat<T> &m);
00175 //  template <class T> Vec<T,N> operator * (const Vec<T,M>& v, const VarMat<T> &m);
00176 
00177 // ASCII stream IO
00178 template <class T> std::ostream& operator<<(std::ostream& os, const VarMat<T>& m);
00179 template <class T> std::istream& operator>>(std::istream& is, VarMat<T>& m);
00180 
00181 //==============================================================================
00182 // VarMat<T>
00183 //==============================================================================
00184 
00185 // Constructors
00186 
00190 template <class T>
00191 inline VarMat<T>::VarMat() : Array2<T>() {}
00192 
00197 template <class T>
00198 inline VarMat<T>::VarMat(int rows, int cols) : Array2<T>(rows,cols) {
00199 }
00200 
00206 template <class T>
00207 inline VarMat<T>::VarMat(int rows, int cols, const T* d) : Array2<T>(rows,cols,d) {
00208 }
00209 
00215 template <class T>
00216 inline VarMat<T>::VarMat(int rows, int cols, const T a) : Array2<T>(rows,cols,a) {
00217 }
00218 
00222 template <class T>
00223 inline VarMat<T>::VarMat(const Array<T,2>& a) : Array2<T>(a) {
00224 }
00225 
00226 // Casting Operation
00227 
00231 template <class T> template <class U>
00232 inline VarMat<T>::VarMat(const VarMat<U>& m) : Array2<T>() {
00233   setSize(m.rows(),m.columns());
00234   set<U>(m);
00235 }
00236 
00237 // Mutators
00238 
00242 template <class T> template <class U, int M, int N>
00243 inline VarMat<T>::VarMat(const Mat<U,M,N>& m) : Array2<T>() {
00244   setSize(M,N);
00245   set<U,M,N>(m);
00246 }
00247 
00248 // Mutators
00249 
00255 template <class T>
00256 inline void VarMat<T>::setRow(int i, const VarVec<T>& v) {
00257 #if MATMATH_CHECK_BOUNDS
00258   if (columns()!=v.size() || i < 0 || i > rows() ) {
00259     std::stringstream ss;
00260     ss << "VarMat<" << rows() << "," << columns() << ">::setRow(" << i << ", ";
00261     ss << "VarVec<" << v.size() << ">): not a valid operation\n";
00262     throw Exception(ss.str());
00263   }
00264 #endif
00265   for (int j=0,k=i*columns();j<columns();j++,k++) x[k] = v.x[j];
00266 }
00267 
00273 template <class T>
00274 inline void VarMat<T>::setCol(int j, const VarVec<T>& v) {
00275 #if MATMATH_CHECK_BOUNDS
00276   if (rows()!=v.size() || j < 0 || j > rows() ) {
00277     std::stringstream ss;
00278     ss << "VarMat<" << rows() << "," << columns() << ">::setCol(" << j << ", ";
00279     ss << "VarVec<" << v.size() << ">): not a valid operation\n";
00280     throw Exception(ss.str());
00281   }
00282 #endif
00283   for (int i=0,k=j;i<rows();i++,k+=columns()) x[k] = v.x[i];
00284 }
00285 
00292 template <class T>
00293 inline void VarMat<T>::setSubMat(int i, int j, const VarMat<T>& m) {
00294 #if MATMATH_CHECK_BOUNDS
00295   if (i < 0 || i+m.rows() > rows() || j < 0 || j+m.columns() > columns()) {
00296     std::stringstream ss;
00297     ss << "VarMat<" << rows() << "," << columns() << ">::setSubMat(" << i << ", " << j << ", ";
00298     ss << "VarMat<" << m.rows() << "," << m.columns() << ">): not a valid operation\n";
00299     throw Exception(ss.str());
00300   }
00301 #endif
00302   for (int id=i,is=0;id<rows() && is<m.rows();id++,is++)
00303     for (int jd=j,js=0;jd<columns() && js<m.columns();jd++,js++)
00304       x[id*columns()+jd] = m.x[is*m.columns()+js];
00305 }
00306 
00307 // Casting Mutators
00308 
00312 template <class T> template <class U>
00313 inline void VarMat<T>::set(const VarMat<U>& m) {
00314   if(rows()!=m.rows()||columns()!=m.columns()) setSize(m.rows(),m.columns());
00315   for (int k=0;k<len;k++) x[k] = T(m.x[k]);
00316 }
00317 
00321 template <class T> template <class U, int M, int N>
00322 inline void VarMat<T>::set(const Mat<U,M,N>& m) {
00323   if(rows()!=M||columns()!=N) setSize(M,N);
00324   for (int k=0;k<len;k++) x[k] = T(m.x[k]);
00325 }
00326 
00330 template <class T> template <class U>
00331 inline VarMat<T>& VarMat<T>::operator = (const VarMat<U>& m) {
00332   set<U>(m);
00333 }
00334 
00338 template <class T> template <class U, int M, int N>
00339 inline VarMat<T>& VarMat<T>::operator = (const Mat<U,M,N>& m) {
00340   set<U,M,N>(m);
00341 }
00342 
00343 // Accessors
00344 
00348 template <class T>
00349 inline int VarMat<T>::length() const {
00350   return len;
00351 }
00352 
00357 template <class T>
00358 inline VarVec<T> VarMat<T>::getRow(int i) const {
00359 #if MATMATH_CHECK_BOUNDS
00360   if (i < 0 || i >= rows()) {
00361     std::stringstream ss;
00362     ss << "VarMat<" << rows() << "," << columns() << ">::getRow(" << i;
00363     ss << "): index out of range\n";
00364     throw Exception(ss.str());
00365   }
00366 #endif
00367   VarVec<T> v(columns());
00368   for (int j=0,k=i*columns();j<columns();j++,k++) v.x[j] = x[k];
00369   return v;
00370 }
00371 
00377 template <class T>
00378 inline VarVec<T> VarMat<T>::getCol(int j) const {
00379 #if MATMATH_CHECK_BOUNDS
00380   if (j < 0 || j >= columns()) {
00381     std::stringstream ss;
00382     ss << "Mat<" << rows() << "," << columns() << ">::getCol(" << j;
00383     ss << "): index out of range\n";
00384     throw Exception(ss.str());
00385   }
00386 #endif
00387   VarVec<T> v(rows());
00388   for (int i=0,k=j;i<rows();i++,k+=columns()) v.x[i] = x[k];
00389   return v;
00390 }
00391 
00400 template <class T>
00401 inline VarMat<T> VarMat<T>::getSubMat(int i, int j, int sub_rows, int sub_cols) {
00402 #if MATMATH_CHECK_BOUNDS
00403   if (i < 0 || i+sub_rows > rows() || j < 0 || j+sub_cols > columns()) {
00404     std::stringstream ss;
00405     ss << "VarMat<" << rows() << "," << columns() << ">::getSubMat(" << i;
00406     ss << ", " << j << "): index out of range\n";
00407     throw Exception(ss.str());
00408   }
00409 #endif
00410   VarMat<T> m(sub_rows,sub_cols,T(0));
00411   for (int id=i,is=0;id<rows() && is<sub_rows;id++,is++)
00412     for (int jd=j,js=0;jd<columns() && js<sub_cols;jd++,js++)
00413       m.x[is*sub_cols+js] = x[id*columns()+jd];
00414   return m;
00415 }
00416 
00417 // Addition and subtraction: <matrix> +/- <matrix>
00418 
00421 template <class T>
00422 inline VarMat<T>& VarMat<T>::add(const VarMat<T>& m) {
00423 #if MATMATH_CHECK_BOUNDS
00424   if (rows()!=m.rows() || columns()!=m.columns()) {
00425     std::stringstream ss;
00426     ss << "VarMat<" << rows() << "," << columns() << ">::add (" <<
00427     ss << "VarMat<" << m.rows() << "," << m.columns() << "): not a valid operation\n";
00428     throw Exception(ss.str());
00429   }
00430 #endif
00431   for (int k=0;k<len;k++) x[k] = x[k] + m.x[k];
00432   return *this;
00433 }
00434 
00441 template <class T>
00442 inline void VarMat<T>::addSubMat(int i, int j, const VarMat<T>& m) {
00443 #if MATMATH_CHECK_BOUNDS
00444   if (i < 0 || i+m.rows() > rows() || j < 0 || j+m.columns() > columns()) {
00445     std::stringstream ss;
00446     ss << "VarMat<" << rows() << "," << columns() << ">::setSubMat(" << i << ", " << j << ", ";
00447     ss << "VarVec<" << m.rows() << "," << m.columns() << ">): not a valid operation\n";
00448     throw Exception(ss.str());
00449   }
00450 #endif
00451   for (int id=i,is=0;id<rows() && is<m.rows();id++,is++)
00452     for (int jd=j,js=0;jd<columns() && js<m.columns();jd++,js++)
00453       x[id*columns()+jd] += m.x[is*m.columns()+js];
00454 }
00455 
00458 template <class T>
00459 inline VarMat<T>& VarMat<T>::subtract(const VarMat<T>& m) {
00460 #if MATMATH_CHECK_BOUNDS
00461   if (rows()!=m.rows() || columns()!=m.columns()) {
00462     std::stringstream ss;
00463     ss << "VarMat<" << rows() << "," << columns() << ">::subtract (" <<
00464     ss << "VarMat<" << m.rows() << "," << m.columns() << "): not a valid operation\n";
00465     throw Exception(ss.str());
00466   }
00467 #endif
00468   for (int k=0;k<len;k++) x[k] = x[k] - m.x[k];
00469   return *this;
00470 }
00471 
00478 template <class T>
00479 inline void VarMat<T>::subtractSubMat(int i, int j, const VarMat<T>& m) {
00480 #if MATMATH_CHECK_BOUNDS
00481   if (i < 0 || i+m.rows() > rows() || j < 0 || j+m.columns() > columns()) {
00482     std::stringstream ss;
00483     ss << "VarMat<" << rows() << "," << columns() << ">::setSubMat(" << i << ", " << j << ", ";
00484     ss << "VarVec<" << m.rows() << "," << m.columns() << ">): not a valid operation\n";
00485     throw Exception(ss.str());
00486   }
00487 #endif
00488   for (int id=i,is=0;id<rows() && is<m.rows();id++,is++)
00489     for (int jd=j,js=0;jd<columns() && js<m.columns();jd++,js++)
00490       x[id*columns()+jd] -= m.x[is*m.columns()+js];
00491 }
00492 
00493 // Addition and subtraction operators: <matrix> +/- <matrix>
00494 
00497 template <class T>
00498 inline VarMat<T> VarMat<T>::operator + (const VarMat<T>& m) const {
00499 #if MATMATH_CHECK_BOUNDS
00500   if (rows()!=m.rows() || columns()!=m.columns()) {
00501     std::stringstream ss;
00502     ss << "VarMat<" << rows() << "," << columns() << ">::operator + (" <<
00503     ss << "VarMat<" << m.rows() << "," << m.columns() << "): not a valid operation\n";
00504     throw Exception(ss.str());
00505   }
00506 #endif
00507   VarMat<T> mp(rows(),columns());
00508   for (int k=0;k<len;k++) mp.x[k] = x[k] + m.x[k];
00509   return mp;
00510 }
00511 
00514 template <class T>
00515 inline void VarMat<T>::operator += (const VarMat<T>& m) {
00516 #if MATMATH_CHECK_BOUNDS
00517   if (rows()!=m.rows() || columns()!=m.columns()) {
00518     std::stringstream ss;
00519     ss << "VarMat<" << rows() << "," << columns() << ">::operator += (" <<
00520     ss << "VarMat<" << m.rows() << "," << m.columns() << "): not a valid operation\n";
00521     throw Exception(ss.str());
00522   }
00523 #endif
00524   for (int k=0;k<len;k++) x[k] += m.x[k];
00525 }
00526 
00529 template <class T>
00530 inline VarMat<T> VarMat<T>::operator - (const VarMat<T>& m) const {
00531 #if MATMATH_CHECK_BOUNDS
00532   if (rows()!=m.rows() || columns()!=m.columns()) {
00533     std::stringstream ss;
00534     ss << "VarMat<" << rows() << "," << columns() << ">::operator - (" <<
00535     ss << "VarMat<" << m.rows() << "," << m.columns() << "): not a valid operation\n";
00536     throw Exception(ss.str());
00537   }
00538 #endif
00539   VarMat<T> mp(rows(),columns());
00540   for (int k=0;k<len;k++) mp.x[k] = x[k] - m.x[k];
00541   return mp;
00542 }
00543 
00546 template <class T>
00547 inline void VarMat<T>::operator -= (const VarMat<T>& m) {
00548 #if MATMATH_CHECK_BOUNDS
00549   if (rows()!=m.rows() || columns()!=m.columns()) {
00550     std::stringstream ss;
00551     ss << "VarMat<" << rows() << "," << columns() << ">::operator -= (" <<
00552     ss << "VarMat<" << m.rows() << "," << m.columns() << "): not a valid operation\n";
00553     throw Exception(ss.str());
00554   }
00555 #endif
00556   for (int k=0;k<len;k++) x[k] -= m.x[k];
00557 }
00558 
00561 template <class T>
00562 inline VarMat<T> VarMat<T>::operator - () const {
00563   VarMat<T> mp(rows(),columns());
00564   for (int k=0;k<len;k++) mp.x[k] = -x[k];
00565   return mp;
00566 }
00567 
00568 // Addition and subtraction: <matrix> +/- <scalar>
00569 
00572 template <class T>
00573 inline VarMat<T>& VarMat<T>::add(const T a) {
00574   for (int k=0;k<len;k++) x[k] = x[k] + a;
00575   return *this;
00576 }
00577 
00580 template <class T>
00581 inline VarMat<T>& VarMat<T>::subtract(const T a) {
00582   for (int k=0;k<len;k++) x[k] = x[k] - a;
00583   return *this;
00584 }
00585 
00586 // Addition and subtraction operators: <matrix> +/- <scalar
00587 
00590  template <class T>
00591  inline VarMat<T> VarMat<T>::operator + (const T a) const {
00592    VarMat<T> mp(rows(),columns());
00593    for (int k=0;k<len;k++) mp.x[k] = x[k] + a;
00594    return mp;
00595  }
00596 
00599  template <class T>
00600  inline void VarMat<T>::operator += (const T a) {
00601    for (int k=0;k<len;k++) x[k] += a;
00602  }
00603 
00606  template <class T>
00607  inline VarMat<T> VarMat<T>::operator - (const T a) const {
00608    VarMat<T> mp(rows(),columns());
00609    for (int k=0;k<len;k++) mp.x[k] = x[k] - a;
00610    return mp;
00611  }
00612 
00615  template <class T>
00616  inline void VarMat<T>::operator -= (const T a) {
00617    for (int k=0;k<len;k++) x[k] -= a;
00618  }
00619 
00620 // Multiplication and division: <matrix> *// <scalar>
00621 
00624 template <class T>
00625 inline VarMat<T> VarMat<T>::operator * (const T a) const {
00626   VarMat<T> m(rows(),columns());
00627   for (int k=0;k<len;k++) m.x[k] = x[k]*a;
00628   return m;
00629 }
00630 
00633 template <class T>
00634 inline void VarMat<T>::operator *= (const T a) {
00635   for (int k=0;k<len;k++) x[k] *= a;
00636 }
00637 
00640 template <class T>
00641 inline VarMat<T> VarMat<T>::operator / (const T a) const {
00642   VarMat<T> m(rows(),columns());
00643   for (int k=0;k<len;k++) m.x[k] = x[k]/a;
00644   return m;
00645 }
00646 
00649 template <class T>
00650 inline void VarMat<T>::operator /= (const T a) {
00651   for (int k=0;k<len;k++) x[k] /= a;
00652 }
00653 
00656 template <class T>
00657 VarMat<T> operator * (const T a, const VarMat<T> &m) {
00658   VarMat<T> mp(m.rows(),m.columns());
00659   for (int k=0;k<m.length();k++) mp.x[k] = m.x[k]*a;
00660   return mp;
00661 }
00662 
00663 // Multiplication: <matrix> * <vector>
00664 
00667 template <class T>
00668 inline VarVec<T> VarMat<T>::operator * (const VarVec<T>& v) const {
00669 #if MATMATH_CHECK_BOUNDS
00670   if (columns()!=v.length()) {
00671     std::stringstream ss;
00672     ss << "VarMat<" << rows() << "," << columns() << ">::operator * (" <<
00673     ss << "VarVec<" << v.length() << "): not a valid operation\n";
00674     throw Exception(ss.str());
00675   }
00676 #endif
00677   VarVec<T> vp(rows(),T(0));
00678   for (int i=0,k=0;i<rows();i++)
00679     for (int j=0;j<columns();j++,k++)
00680       vp.x[i] += x[k]*v.x[j];
00681   return vp;
00682 }
00683 
00686 template <class T>
00687 VarVec<T> operator * (const VarVec<T>& v, const VarMat<T> &m) {
00688 #if MATMATH_CHECK_BOUNDS
00689   if (m.rows()!=v.length()) {
00690     std::stringstream ss;
00691     ss << "VarMat<" << m.rows() << "," << m.columns() << ">::operator * (";
00692     ss << "VarVec<" << v.length() << "): not a valid operation\n";
00693     throw Exception(ss.str());
00694   }
00695 #endif
00696   VarVec<T> vp(m.columns(),T(0));
00697   for (int i=0,k=0;i<m.rows();i++)
00698     for (int j=0;j<m.columns();j++,k++)
00699       vp.x[j] += m.x[k]*v.x[i];
00700   return vp;
00701 }
00702 
00703 // Multiplication: <matrix> * <matrix>
00704 
00707 template <class T>
00708 inline VarMat<T> VarMat<T>::operator * (const VarMat<T>& m) const {
00709 #if MATMATH_CHECK_BOUNDS
00710   if (columns()!=m.rows()) {
00711     std::stringstream ss;
00712     ss << "VarMat<" << rows() << "," << columns() << ">::operator * (" <<
00713     ss << "VarMat<" << m.rows() << "," << m.columns() << "): not a valid operation\n";
00714     throw Exception(ss.str());
00715   }
00716 #endif
00717   VarMat<T> mp(rows(),m.columns(),T(0));
00718   for (int i=0;i<rows();i++)
00719     for (int j=0;j<m.columns();j++)
00720       for (int k=0;k<columns();k++)
00721         mp.x[i*m.columns()+j] += x[i*columns()+k]*m.x[k*m.columns()+j];
00722   return mp;
00723 }
00724 
00727 template <class T>
00728 inline void VarMat<T>::operator *= (const VarMat<T>& m) {
00729 #if MATMATH_CHECK_BOUNDS
00730   if (columns()!=m.rows() || columns()!=m.columns()) {
00731     std::stringstream ss;
00732     ss << "VarMat<" << rows() << "," << columns() << ">::operator *= (" <<
00733     ss << "VarMat<" << m.rows() << "," << m.columns() << "): not a valid operation\n";
00734     throw Exception(ss.str());
00735   }
00736 #endif
00737   (*this) = (*this)*m;
00738 }
00739 
00740 // Equality and inequality tests.
00741 
00745 template <class T>
00746 inline VarMat<bool> VarMat<T>::operator == (const VarMat<T>& m) const {
00747 #if MATMATH_CHECK_BOUNDS
00748   if (rows()!=m.rows() || columns()!=m.columns()) {
00749     std::stringstream ss;
00750     ss << "VarMat<" << rows() << "," << columns() << ">::operator == (" <<
00751     ss << "VarMat<" << m.rows() << "," << m.columns() << "): not a valid operation\n";
00752     throw Exception(ss.str());
00753   }
00754 #endif
00755   VarMat<bool> b(rows(),columns(),false);
00756   for (int i=0;i<len;i++) if (x[i] == m.x[i]) b.x[i] = true;
00757   return b;
00758 }
00759 
00763 template <class T>
00764 inline VarMat<bool> VarMat<T>::operator != (const VarMat<T>& m) const {
00765 #if MATMATH_CHECK_BOUNDS
00766   if (rows()!=m.rows() || columns()!=m.columns()) {
00767     std::stringstream ss;
00768     ss << "VarMat<" << rows() << "," << columns() << ">::operator != (" <<
00769     ss << "VarMat<" << m.rows() << "," << m.columns() << "): not a valid operation\n";
00770     throw Exception(ss.str());
00771   }
00772 #endif
00773   VarMat<bool> b(rows(),columns(),false);
00774   for (int i=0;i<len;i++) if (x[i] != m.x[i]) b.x[i] = true;
00775   return b;
00776 }
00777 
00781 template <class T>
00782 inline VarMat<bool> VarMat<T>::operator >= (const VarMat<T>& m) const {
00783 #if MATMATH_CHECK_BOUNDS
00784   if (rows()!=m.rows() || columns()!=m.columns()) {
00785     std::stringstream ss;
00786     ss << "VarMat<" << rows() << "," << columns() << ">::operator >= (" <<
00787     ss << "VarMat<" << m.rows() << "," << m.columns() << "): not a valid operation\n";
00788     throw Exception(ss.str());
00789   }
00790 #endif
00791   VarMat<bool> b(rows(),columns(),false);
00792   for (int i=0;i<len;i++) if (x[i] >= m.x[i]) b.x[i] = true;
00793   return b;
00794 }
00795 
00799 template <class T>
00800 inline VarMat<bool> VarMat<T>::operator <= (const VarMat<T>& m) const {
00801 #if MATMATH_CHECK_BOUNDS
00802   if (rows()!=m.rows() || columns()!=m.columns()) {
00803     std::stringstream ss;
00804     ss << "VarMat<" << rows() << "," << columns() << ">::operator <= (" <<
00805     ss << "VarMat<" << m.rows() << "," << m.columns() << "): not a valid operation\n";
00806     throw Exception(ss.str());
00807   }
00808 #endif
00809   VarMat<bool> b(rows(),columns(),false);
00810   for (int i=0;i<len;i++) if (x[i] <= m.x[i]) b.x[i] = true;
00811   return b;
00812 }
00813 
00817 template <class T>
00818 inline VarMat<bool> VarMat<T>::operator > (const VarMat<T>& m) const {
00819 #if MATMATH_CHECK_BOUNDS
00820   if (rows()!=m.rows() || columns()!=m.columns()) {
00821     std::stringstream ss;
00822     ss << "VarMat<" << rows() << "," << columns() << ">::operator > (" <<
00823     ss << "VarMat<" << m.rows() << "," << m.columns() << ">): not a valid operation\n";
00824     throw Exception(ss.str());
00825   }
00826 #endif
00827   VarMat<bool> b(rows(),columns(),false);
00828   for (int i=0;i<len;i++) if (x[i] > m.x[i]) b.x[i] = true;
00829   return b;
00830 }
00831 
00835 template <class T>
00836 inline VarMat<bool> VarMat<T>::operator < (const VarMat<T>& m) const {
00837 #if MATMATH_CHECK_BOUNDS
00838   if (rows()!=m.rows() || columns()!=m.columns()) {
00839     std::stringstream ss;
00840     ss << "VarMat<" << rows() << "," << columns() << ">::operator < (" <<
00841     ss << "VarMat<" << m.rows() << "," << m.columns() << ">): not a valid operation\n";
00842     throw Exception(ss.str());
00843   }
00844 #endif
00845   VarMat<bool> b(rows(),columns(),false);
00846   for (int i=0;i<len;i++) if (x[i] < m.x[i]) b.x[i] = true;
00847   return b;
00848 }
00849 
00854 template <class T>
00855 inline int VarMat<T>::compareTo(const VarMat<T>& m) const {
00856 #if MATMATH_CHECK_BOUNDS
00857   if (rows()!=m.rows() || columns()!=m.columns()) {
00858     std::stringstream ss;
00859     ss << "VarMat<" << rows() << "," << columns() << ">::compareTo(" <<
00860     ss << "VarMat<" << m.rows() << "," << m.columns() << ">): not a valid operation\n";
00861     throw Exception(ss.str());
00862   }
00863 #endif
00864   int g=0, l=0;
00865   for (int i=0;i<len;i++) {
00866     if (x[i] < m.x[i]) l++;
00867     if (x[i] > m.x[i]) g++;
00868   }
00869   if (l==(len)) return -1;
00870   else if (g==(len)) return 1;
00871   else return 0;
00872 }
00873 
00877 template <class T>
00878 inline bool VarMat<T>::equalTo(const VarMat<T>& m, const T tol) const {
00879 #if MATMATH_CHECK_BOUNDS
00880   if (rows()!=m.rows() || columns()!=m.columns()) {
00881     std::stringstream ss;
00882     ss << "VarMat<" << rows() << "," << columns() << ">::equalTo(" <<
00883     ss << "VarMat<" << m.rows() << "," << m.columns() << ">): not a valid operation\n";
00884     throw Exception(ss.str());
00885   }
00886 #endif
00887   bool t = true;
00888   for (int i=0;i<len;i++) if (fabs(x[i] - m.x[i]) > tol) t = false;
00889   return t;
00890 }
00891 
00892 // Transpose
00893 
00896 template <class T>
00897 inline VarMat<T> VarMat<T>::transposed() const {
00898   VarMat<T> mp(columns(),rows());
00899   for (int i=0;i<rows();i++)
00900     for (int j=0;j<columns();j++)
00901       mp.x[j*rows()+i] = x[i*columns()+j];
00902   return mp;
00903 }
00904 
00905 // Random matrices
00906 
00912 template <class T>
00913 inline VarMat<T> VarMat<T>::uniformRand(const T a, const T b) {
00914   VarMat<T> m(rows(),columns());
00915   for (int i=0;i<len;i++) m.x[i] = rtc_uniform_rand<T>(a,b);
00916   return m;
00917 }
00918 
00924 template <class T>
00925 inline VarMat<T> VarMat<T>::normalRand(const T mean, const T stdev) {
00926   VarMat<T> m(rows(),columns());
00927   for (int i=0;i<len;i++) m.x[i] = rtc_normal_rand<T>(mean,stdev);
00928   return m;
00929 }
00930 
00931 //  /** Create matrix of samples from a multivariate gaussian distribution.
00932 //   * @param mean mean of normal distribution
00933 //   * @param stdev standard deviation of normal distribution
00934 //   * @return matrix of normal samples
00935 //   */
00936 //  template <class T>
00937 //  inline VarMat<T> VarMat<T>::multivariateGauss(const Vec<T,M>& mean, const SMat<T,M>& cov) {
00938 //    VarMat<T> m;
00939 //    SMat<T,M> S(cov);
00940 //    int n=S.choleskyDecomp();
00941 //    assert(n==0);
00942 //    S.transpose();
00943 //    VarMat<T> X = normalRand();
00944 //    for(int j=0;j<N;++j) m.setCol(j,mean);
00945 //    m = m + S*X;
00946 //    return m;
00947 //  }
00948 //
00949 // Serialization routines
00950 
00953 template <class T>
00954 std::ostream& operator<<(std::ostream& os, const VarMat<T>& mat) {
00955 
00956   int minFieldWidth = os.precision()+2;
00957 
00958   //case with 1 row
00959   if (mat.rows() == 1){
00960     os << "[" ;
00961     for (int i=0; i<mat.columns(); ++i)
00962       os << std::setw(minFieldWidth) << mat(0,i) << " ";
00963 
00964     os << "]" << std::endl;
00965   }
00966   //case with 2+ rows
00967   else{
00968     //write first row
00969     os << "[" ;
00970     for (int j=0; j<mat.columns()-1; ++j){
00971       os << std::setw(minFieldWidth) << mat(0,j) << " ";
00972     }
00973     os << std::setw(minFieldWidth) << mat(0,mat.columns()-1) << ";" << std::endl;
00974     //write middle rows
00975     for (int i=1;i<mat.rows()-1;++i){
00976       for (int j=0;j<mat.columns();++j){
00977         os << " " << std::setw(minFieldWidth) << mat(i,j);
00978         }
00979       os << ";" << std::endl;
00980       }
00981     //write last row
00982     for (int j=0; j<mat.columns(); ++j){
00983       os << " " << std::setw(minFieldWidth) << mat(mat.rows()-1,j);
00984     }
00985     os << "]" << std::endl;
00986   }
00987 
00988   return os;
00989 }
00990 
00993 template <class T>
00994 std::istream& operator>>(std::istream& is, VarMat<T>& mat){
00995   using namespace std;
00996   vector<T> data;
00997   string sizeString;
00998   string matString;
00999   stringstream matStringStream;
01000   string rowString;
01001   stringstream rowStringStream;
01002   int sPos;
01003 
01004   getline(is, matString, ']');
01005   sPos = matString.find('[');
01006   if (sPos == (int)string::npos)
01007     throw Exception("format error: expecting formated matrix to start with '['");
01008 
01009   //erase the starting '['
01010   //note the ending ']' was removed by the getline function as the delim
01011   matString.erase(0,sPos+1);
01012   matStringStream.str(matString);
01013 
01014   //determine num of rows and cols
01015   int rowCount = 0, colCount = 0;
01016   int cols = -1;
01017   float tmpVal;
01018   while(getline(matStringStream, rowString, ';')){
01019     rowStringStream << rowString;
01020 
01021     colCount = 0;
01022     while(rowStringStream.good()){
01023       rowStringStream >> tmpVal;
01024       data.push_back(tmpVal);
01025       ++colCount;
01026     }
01027     rowStringStream.clear();
01028 
01029     //check that we have same num of entries in each row
01030     if (cols == -1)
01031       cols = colCount;
01032     else{
01033       if (colCount != cols){
01034         throw Exception("format error: different number of elements in rows.");
01035       }
01036     }
01037 
01038     ++rowCount;
01039   }
01040 
01041   //check that dimensions agree
01042   if (rowCount != mat.rows() || colCount != mat.columns())
01043     mat.setSize(rowCount,colCount);
01044 
01045   //copy extracted data
01046   for (int i=0;i<mat.len;i++){
01047     mat.x[i] = data[i];
01048   }
01049 
01050   return is;
01051 }
01052 
01053 // General elementwise operations
01054 
01058 template <class T>
01059 inline void VarMat<T>::perform(T (*mathFun)(T)) {
01060   for (int i=0;i<len;i++) x[i] = (*mathFun)(x[i]);
01061 }
01062 
01066 template <class T>
01067 inline VarVec<T> VarMat<T>::sum() const {
01068   VarVec<T> s(columns(),T(0));
01069   for (int j=0;j<columns();j++)
01070     for (int i=0;i<rows();i++) s.x[j] += x[i*columns()+j];
01071   return s;
01072 }
01073 
01078 template <class T>
01079 inline T VarMat<T>::interpolate(const float i, const float j) const {
01080   int r = rows();
01081   int c = columns();
01082   int truncR = rtc_clamp(static_cast<int>(i),0,r-1);
01083   int truncR1 = rtc_clamp(truncR+1,0,r-1);
01084   const float fractR = i - static_cast<float>(truncR);
01085   int truncC = rtc_clamp(static_cast<int>(j),0,c-1);
01086   int truncC1 = rtc_clamp(truncC+1,0,c-1);
01087   const float fractC = j - static_cast<float>(truncC);
01088 
01089   // do the interpolation
01090   const T syx = x[truncR*c+truncC];
01091   const T syx1 = x[truncR*c+truncC1];
01092   const T sy1x = x[truncR1*c+truncC];
01093   const T sy1x1 = x[truncR1*c+truncC1];
01094   // normal interpolation within range
01095   const T tmp1 = syx  + (syx1-syx)*T(fractC);
01096   const T tmp2 = sy1x + (sy1x1-sy1x)*T(fractC);
01097   return (tmp1 + (tmp2-tmp1)*T(fractR));
01098 }
01099 
01105 template <class T>
01106 inline VarVec<T> VarMat<T>::meanOfRows() const {
01107   VarVec<T> m(columns(),T(0));
01108   for (int j=0;j<columns();j++)
01109     for (int i=0;i<rows();i++)
01110       m.x[j] += x[i*columns()+j];
01111   m/=static_cast<T>(rows());
01112   return m;
01113 }
01114 
01115 /*
01116   * This is a new, faster version, written by Gu Xin:
01117   */
01118 template <class T>
01119 inline VarSMat<T> VarMat<T>::covarianceMatrixOfRows() const {
01120   VarSMat<T> dest(columns(),T(0));
01121 
01122   //Implementation for the sum of Matrix[X];
01123   if (rows()<2) { //just one row
01124     std::stringstream ss;
01125     ss << "VarMat<" << rows() << "," << columns() << ">::covarianceMatrixOfRows(): " <<
01126     ss << "matrix has to have more than one row\n";
01127     throw Exception(ss.str());
01128   }
01129 
01130   // mean vector
01131   VarVec<T> mu(columns(),T(0));
01132 
01133   /*
01134    * the loop gets out the result of the Matrix[X];
01135    * Matrix[X]=sum(Matrix[X(i)]);-- i from 0 to n-1--
01136    */
01137   for(int i=0;i<rows();i++) {
01138     VarVec<T> tmpV = getRow(i);
01139     mu.add(tmpV);
01140     VarSMat<T> tmp = tmpV.outer(tmpV);
01141     dest.add(tmp); //add the Matrix[X(i)] to Matrix[sumMatrix];
01142   }
01143   //End of the Implementation for the sum of Matrix[X];
01144 
01145   //Implementation for the Matrix[meanOf];
01146   mu/=static_cast<T>(rows());
01147 
01148   VarSMat<T> tmp = mu.outer(mu);
01149   tmp *= static_cast<T>(rows());
01150 
01151   //get the result of the difference from Matrix[X] and Matrix[meanOf];
01152   dest.subtract(tmp);
01153   dest/=static_cast<T>(rows()-1);
01154   return dest;
01155 }
01156 
01157 //==============================================================================
01158 } // NAMESPACE rtc
01159 //==============================================================================
01160 #endif // RTC_VARMAT_H defined
01161 //==============================================================================
01162 


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