rtc: rtc: rtcMat.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 .......: rtcMat.h
00012  * authors ....: Benjamin Pitzer
00013  * organization: Robert Bosch LLC
00014  * creation ...: 08/16/2006
00015  * modified ...: $Date: 2009-03-19 11:10:08 -0700 (Thu, 19 Mar 2009) $
00016  * changed by .: $Author: wg75pal $
00017  * revision ...: $Revision: 101 $
00018  */
00019 #ifndef RTC_MAT_H
00020 #define RTC_MAT_H
00021 
00022 //== INCLUDES ==================================================================
00023 #include "rtc/rtcOutputHandler.h"
00024 #include "rtc/rtcInputHandler.h"
00025 #include "rtc/rtcMath.h"
00026 #include "rtc/rtcVec.h"
00027 #include "rtc/rtcSMat.h"
00028 
00029 //== NAMESPACES ================================================================
00030 namespace rtc {
00031 
00032 // Forward declarations
00033 template <class T, int M> class Vec; // M-d vector
00034 template <class T, int M, int N> class Mat; // MxN Matrix
00035 template <class T, int M> class SMat; // MxM Matrix
00036 
00040 template <class T, int M, int N>
00041 class Mat {
00042 public:
00043   // Data
00044   T x[M*N]; 
00045 
00046   // Constructors
00047   Mat();
00048   Mat(const T* d);
00049   Mat(const T a);
00050   Mat(const Mat<T,M,N>& m);
00051 
00052   // Cast Operation
00053   template <class U> Mat(const Mat<U,M,N>& m);
00054 
00055   // Mutators
00056   void set(const T* d);
00057   void set(const T a);
00058   void set(const Mat<T,M,N>& m);
00059   Mat<T,M,N>& operator = (const T* d);
00060   Mat<T,M,N>& operator = (const T a);
00061   Mat<T,M,N>& operator = (const Mat<T,M,N>& m);
00062   void setRow(const int i, const Vec<T,N>& v);
00063   void setCol(const int j, const Vec<T,M>& v);
00064   template <int P, int Q> void setSubMat(const int r, const int c, const Mat<T,P,Q>& m);
00065 
00066   // Casting Mutators
00067   template <class U> void set(const Mat<U,M,N>& m);
00068   template <class U> Mat<T,M,N>& operator = (const Mat<U,M,N>& m);
00069 
00070   // Accessors
00071   T operator () (const int r, const int c) const;
00072   T& operator () (const int r, const int c);
00073   Vec<T,N> getRow(int r) const;
00074   Vec<T,M> getCol(int c) const;
00075   template <int P, int Q> Mat<T,P,Q> getSubMat(const int i, const int j);
00076 
00077   // Addition and subtraction: <matrix> +/- <matrix>
00078   Mat<T,M,N>& add(const Mat<T,M,N>& m);
00079   Mat<T,M,N>& subtract(const Mat<T,M,N>& m);
00080   Mat<T,M,N> operator + (const Mat<T,M,N>& m) const;
00081   void operator += (const Mat<T,M,N>& m);
00082   Mat<T,M,N> operator - (const Mat<T,M,N>& m) const;
00083   void operator -= (const Mat<T,M,N>& m);
00084   Mat<T,M,N> operator - () const;
00085 
00086   // Addition and subtraction: <matrix> +/- <scalar>
00087   Mat<T,M,N>& add(const T a);
00088   Mat<T,M,N>& subtract(const T a);
00089   Mat<T,M,N> operator + (const T a) const;
00090   void operator += (const T a);
00091   Mat<T,M,N> operator - (const T a) const;
00092   void operator -= (const T a);
00093 
00094   // Multiplication and division: <matrix> *// <scalar>
00095   Mat<T,M,N> operator * (const T a) const;
00096   void operator *= (const T a);
00097   Mat<T,M,N> operator / (const T a) const;
00098   void operator /= (const T a);
00099 
00100   // Multiplication: <matrix> * <vector>
00101   Vec<T,M> operator * (const Vec<T,N>& v) const;
00102   template <int P> Mat<T,M,P> operator * (const Mat<T,N,P>& m) const;
00103   void operator *= (const Mat<T,N,N>& m);
00104 
00105   // Equality and inequality tests
00106   Mat<bool,M,N> operator == (const Mat<T,M,N>& m) const;
00107   Mat<bool,M,N> operator != (const Mat<T,M,N>& m) const;
00108   Mat<bool,M,N> operator >= (const Mat<T,M,N>& m) const;
00109   Mat<bool,M,N> operator <= (const Mat<T,M,N>& m) const;
00110   Mat<bool,M,N> operator > (const Mat<T,M,N>& m) const;
00111   Mat<bool,M,N> operator < (const Mat<T,M,N>& m) const;
00112   int compareTo(const Mat<T,M,N>& m) const;
00113   bool equalTo(const Mat<T,M,N>& m, const T tol = T(0)) const;
00114 
00115   // Other matrix operations
00116   Mat<T,N,M> transposed() const;
00117 
00118   // Random matrix
00119   static Mat<T,M,N> uniformRand(const T a = T(0), const T b = T(1));
00120   static Mat<T,M,N> normalRand(const T mean = T(0), const T stdev = T(1));
00121   static Mat<T,M,N> multivariateGauss(const Vec<T,M>& mean, const SMat<T,M>& cov);
00122 
00123   // General elementwise operations
00124   void perform(T (*mathFun)(T));
00125   //void perform(T (*mathFun)(T,T), const T arg2);
00126   //void perform(T (*mathFun)(T,T), const Vec<T,M>& v);
00127   //Vec<T,M> performed(T (*mathFun)(T));
00128   //Vec<T,M> performed(T (*mathFun)(T,T), const T arg2);
00129   //Vec<T,M> performed(T (*mathFun)(T,T), const Vec<T,M>& v);
00130 
00131   // Reductions: Max/Min, Sum/Product
00132   //T max() const;
00133   //T min() const;
00134   Vec<T,N> sum() const;
00135   //T prod() const;
00136 
00137   // Bilinear Interpolation
00138   T interpolate(const float i, const float j) const;
00139 
00140   // statistical operations
00141   Vec<T,N> meanOfRows() const;
00142   SMat<T,N> covarianceMatrixOfRows() const;
00143 
00144   // Serialization
00145   bool write(OutputHandler& oh) const;
00146   bool read(InputHandler& ih);
00147 };
00148 
00149 // Global operators to for cases where Mat<T,M,N>
00150 // is the second argument in a binary operator
00151 template <class T, int M, int N> Mat<T,M,N> operator * (const T a, const Mat<T,M,N> &m);
00152 template <class T, int M, int N> Vec<T,N> operator * (const Vec<T,M>& v, const Mat<T,M,N> &m);
00153 
00154 // ASCII stream IO
00155 template <class T, int M, int N> std::ostream& operator<<(std::ostream& os, const Mat<T,M,N>& vec);
00156 template <class T, int M, int N> std::istream& operator>>(std::istream& is, Mat<T,M,N>& vec);
00157 
00158 
00159 //==============================================================================
00160 // Mat<T,M,N>
00161 //==============================================================================
00162 
00163 // Constructors
00164 
00167 template <class T, int M, int N>
00168 inline Mat<T,M,N>::Mat() {}
00169 
00173 template <class T, int M, int N>
00174 inline Mat<T,M,N>::Mat(const T* d) {
00175   set(d);
00176 }
00177 
00181 template <class T, int M, int N>
00182 inline Mat<T,M,N>::Mat(const T a) {
00183   set(a);
00184 }
00185 
00189 template <class T, int M, int N>
00190 inline Mat<T,M,N>::Mat(const Mat<T,M,N>& m) {
00191   set(m);
00192 }
00193 
00194 // Casting Operation
00195 
00199 template <class T, int M, int N> template <class U>
00200 inline Mat<T,M,N>::Mat(const Mat<U,M,N>& m) {
00201   set<U>(m);
00202 }
00203 
00204 // Mutators
00205 
00209 template <class T, int M, int N>
00210 inline void Mat<T,M,N>::set(const T* d) {
00211   for (int k=0;k<M*N;k++) x[k] = d[k];
00212 }
00213 
00217 template <class T, int M, int N>
00218 inline void Mat<T,M,N>::set(const T a) {
00219   if (a == T(0)) memset(x,0,M*N*sizeof(T));
00220   else for (int i=0;i<M*N;++i) x[i] = a;
00221 }
00222 
00226 template <class T, int M, int N>
00227 inline void Mat<T,M,N>::set(const Mat<T,M,N>& m) {
00228   memcpy((void*)x,(void*)m.x,M*N*sizeof(T));
00229 }
00230 
00234 template <class T, int M, int N>
00235 inline Mat<T,M,N>& Mat<T,M,N>::operator = (const T* d) {
00236   set(d);
00237   return *this;
00238 }
00239 
00243 template <class T, int M, int N>
00244 inline Mat<T,M,N>& Mat<T,M,N>::operator = (const T a) {
00245   set(a);
00246   return *this;
00247 }
00248 
00252 template <class T, int M, int N>
00253 inline Mat<T,M,N>& Mat<T,M,N>::operator = (const Mat<T,M,N>& m) {
00254   set(m);
00255   return *this;
00256 }
00257 
00263 template <class T, int M, int N>
00264 inline void Mat<T,M,N>::setRow(const int i, const Vec<T,N>& v) {
00265 #if MATMATH_CHECK_BOUNDS
00266   if (i < 0 || i > M) {
00267     std::stringstream ss;
00268     ss << "Mat<" << M << "," << N << ">::setRow(" << i;
00269     ss << ", v): index out of range\n";
00270     ss << std::flush;
00271     throw Exception(ss.str());
00272   }
00273 #endif
00274   for (int j=0,k=i*N;j<N;j++,k++) x[k] = v.x[j];
00275 }
00276 
00282 template <class T, int M, int N>
00283 inline void Mat<T,M,N>::setCol(const int j, const Vec<T,M>& v) {
00284 #if MATMATH_CHECK_BOUNDS
00285   if (j < 0 || j > N) {
00286     std::stringstream ss;
00287     ss << "Mat<" << M << "," << N << ">::setCol(" << j;
00288     ss << ", v) out of range\n";
00289     ss << std::flush;
00290     throw Exception(ss.str());
00291   }
00292 #endif
00293   for (int i=0,k=j;i<M;i++,k+=N) x[k] = v.x[i];
00294 }
00295 
00302 template <class T, int M, int N> template <int P, int Q>
00303 inline void Mat<T,M,N>::setSubMat(const int i, const int j,
00304           const Mat<T,P,Q>& m) {
00305 #if MATMATH_CHECK_BOUNDS
00306   if (i < 0 || i+P > M || j < 0 || j+Q > N) {
00307     std::stringstream ss;
00308     ss << "Mat<" << M << "," << N << ">::setSubMat(" << i;
00309     ss << ", " << j << ", m): index out of range\n";
00310     ss << std::flush;
00311     throw Exception(ss.str());
00312   }
00313 #endif
00314   for (int id=i,is=0;id<M && is<P;id++,is++)
00315     for (int jd=j,js=0;jd<N && js<Q;jd++,js++)
00316 x[id*N+jd] = m.x[is*Q+js];
00317 }
00318 
00319 // Casting Mutators
00320 
00324 template <class T, int M, int N> template <class U>
00325 inline void Mat<T,M,N>::set(const Mat<U,M,N>& m) {
00326   for (int k=0;k<M*N;k++) x[k] = T(m.x[k]);
00327 }
00328 
00332 template <class T, int M, int N> template <class U>
00333 inline Mat<T,M,N>& Mat<T,M,N>::operator = (const Mat<U,M,N>& m) {
00334   set<U>(m);
00335 }
00336 
00337 // Accessors
00338 
00345 template <class T, int M, int N>
00346 inline T Mat<T,M,N>::operator () (const int i, const int j) const {
00347 #if MATMATH_CHECK_BOUNDS
00348   if (i<0 || i>M || j<0 || j>N) {
00349     std::stringstream ss;
00350     ss << "Mat<" << M << "," << N << ">::operator (" << i;
00351     ss << ", " << j << "): index out of range\n";
00352     ss << std::flush;
00353     throw Exception(ss.str());
00354   }
00355 #endif
00356   return x[i*N + j];
00357 }
00358 
00365 template <class T, int M, int N>
00366 inline T& Mat<T,M,N>::operator () (const int i, const int j) {
00367 #if MATMATH_CHECK_BOUNDS
00368   if (i<0 || i>M || j<0 || j>N) {
00369     std::stringstream ss;
00370     ss << "&Mat<" << M << "," << N << ">::operator (" << i;
00371     ss << ", " << j << "): index out of range\n";
00372     ss << std::flush;
00373     rtc::Exception e(ss.str());
00374     throw e;
00375   }
00376 #endif
00377   return x[i*N + j];
00378 }
00379 
00384 template <class T, int M, int N>
00385 inline Vec<T,N> Mat<T,M,N>::getRow(int i) const {
00386 #if MATMATH_CHECK_BOUNDS
00387   if (i < 0 || i >= M) {
00388     std::stringstream ss;
00389     ss << "Mat<" << M << "," << N << ">::getRow(" << i;
00390     ss << "): index out of range\n";
00391     ss << std::cerr << std::flush;
00392     throw Exception(ss.str());
00393   }
00394 #endif
00395   Vec<T,N> v;
00396   for (int j=0,k=i*N;j<N;j++,k++) v.x[j] = x[k];
00397   return v;
00398 }
00399 
00404 template <class T, int M, int N>
00405 inline Vec<T,M> Mat<T,M,N>::getCol(int j) const {
00406 #if MATMATH_CHECK_BOUNDS
00407   if (j < 0 || j >= N) {
00408     std::stringstream ss;
00409     ss << "Mat<" << M << "," << N << ">::getCol(" << j;
00410     ss << "): index out of range\n";
00411     ss << std::flush;
00412     throw Exception(ss.str());
00413   }
00414 #endif
00415   Vec<T,M> v;
00416   for (int i=0,k=j;i<M;i++,k+=N) v.x[i] = x[k];
00417   return v;
00418 }
00419 
00425 template <class T, int M, int N> template <int P, int Q>
00426 inline Mat<T,P,Q> Mat<T,M,N>::getSubMat(const int i, const int j) {
00427 #if MATMATH_CHECK_BOUNDS
00428   if (i < 0 || i+P > M || j < 0 || j+Q > N) {
00429     std::stringstream ss;
00430     ss << "Mat<" << M << "," << N << ">::getSubMat(" << i;
00431     ss << ", " << j << "): index out of range\n";
00432     ss << std::flush;
00433     throw Exception(ss.str());
00434   }
00435 #endif
00436   Mat<T,P,Q> m(T(0));
00437   for (int id=i,is=0;id<M && is<P;id++,is++)
00438     for (int jd=j,js=0;jd<N && js<Q;jd++,js++)
00439 m.x[is*Q+js] = x[id*N+jd];
00440   return m;
00441 }
00442 
00443 // Addition and subtraction: <matrix> +/- <matrix>
00444 
00447 template <class T, int M, int N>
00448 inline Mat<T,M,N>& Mat<T,M,N>::add(const Mat<T,M,N>& m) {
00449   for (int k=0;k<M*N;k++) x[k] = x[k] + m.x[k];
00450   return *this;
00451 }
00452 
00455 template <class T, int M, int N>
00456 inline Mat<T,M,N>& Mat<T,M,N>::subtract(const Mat<T,M,N>& m) {
00457   for (int k=0;k<M*N;k++) x[k] = x[k] - m.x[k];
00458   return *this;
00459 }
00460 
00461 // Addition and subtraction operators: <matrix> +/- <matrix>
00462 
00465 template <class T, int M, int N>
00466 inline Mat<T,M,N> Mat<T,M,N>::operator + (const Mat<T,M,N>& m) const {
00467   Mat<T,M,N> mp;
00468   for (int k=0;k<M*N;k++) mp.x[k] = x[k] + m.x[k];
00469   return mp;
00470 }
00471 
00474 template <class T, int M, int N>
00475 inline void Mat<T,M,N>::operator += (const Mat<T,M,N>& m) {
00476   for (int k=0;k<M*N;k++) x[k] += m.x[k];
00477 }
00478 
00481 template <class T, int M, int N>
00482 inline Mat<T,M,N> Mat<T,M,N>::operator - (const Mat<T,M,N>& m) const {
00483   Mat<T,M,N> mp;
00484   for (int k=0;k<M*N;k++) mp.x[k] = x[k] - m.x[k];
00485   return mp;
00486 }
00487 
00490 template <class T, int M, int N>
00491 inline void Mat<T,M,N>::operator -= (const Mat<T,M,N>& m) {
00492   for (int k=0;k<M*N;k++) x[k] -= m.x[k];
00493 }
00494 
00497 template <class T, int M, int N>
00498 inline Mat<T,M,N> Mat<T,M,N>::operator - () const {
00499   Mat<T,M,N> mp;
00500   for (int k=0;k<M*N;k++) mp.x[k] = -x[k];
00501   return mp;
00502 }
00503 
00504 // Addition and subtraction: <matrix> +/- <scalar>
00505 
00508 template <class T, int M, int N>
00509 inline Mat<T,M,N>& Mat<T,M,N>::add(const T a) {
00510   for (int k=0;k<M*N;k++) x[k] = x[k] + a;
00511   return *this;
00512 }
00513 
00516 template <class T, int M, int N>
00517 inline Mat<T,M,N>& Mat<T,M,N>::subtract(const T a) {
00518   for (int k=0;k<M*N;k++) x[k] = x[k] - a;
00519   return *this;
00520 }
00521 
00522 // Addition and subtraction operators: <matrix> +/- <scalar
00523 
00526  template <class T, int M, int N>
00527  inline Mat<T,M,N> Mat<T,M,N>::operator + (const T a) const {
00528    Mat<T,M,N> mp;
00529    for (int k=0;k<M*N;k++) mp.x[k] = x[k] + a;
00530    return mp;
00531  }
00532 
00535  template <class T, int M, int N>
00536  inline void Mat<T,M,N>::operator += (const T a) {
00537    for (int k=0;k<M*N;k++) x[k] += a;
00538  }
00539 
00542  template <class T, int M, int N>
00543  inline Mat<T,M,N> Mat<T,M,N>::operator - (const T a) const {
00544    Mat<T,M,N> mp;
00545    for (int k=0;k<M*N;k++) mp.x[k] = x[k] - a;
00546    return mp;
00547  }
00548 
00551  template <class T, int M, int N>
00552  inline void Mat<T,M,N>::operator -= (const T a) {
00553    for (int k=0;k<M*N;k++) x[k] -= a;
00554  }
00555 
00556 // Multiplication and division: <matrix> *// <scalar>
00557 
00560 template <class T, int M, int N>
00561 inline Mat<T,M,N> Mat<T,M,N>::operator * (const T a) const {
00562   Mat<T,M,N> m;
00563   for (int k=0;k<M*N;k++) m.x[k] = x[k]*a;
00564   return m;
00565 }
00566 
00569 template <class T, int M, int N>
00570 inline void Mat<T,M,N>::operator *= (const T a) {
00571   for (int k=0;k<M*N;k++) x[k] *= a;
00572 }
00573 
00576 template <class T, int M, int N>
00577 inline Mat<T,M,N> Mat<T,M,N>::operator / (const T a) const {
00578   Mat<T,M,N> m;
00579   for (int k=0;k<M*N;k++) m.x[k] = x[k]/a;
00580   return m;
00581 }
00582 
00585 template <class T, int M, int N>
00586 inline void Mat<T,M,N>::operator /= (const T a) {
00587   for (int k=0;k<M*N;k++) x[k] /= a;
00588 }
00589 
00592 template <class T, int M, int N>
00593 Mat<T,M,N> operator * (const T a, const Mat<T,M,N> &m) {
00594   Mat<T,M,N> mp;
00595   for (int k=0;k<M*N;k++) mp.x[k] = m.x[k]*a;
00596   return mp;
00597 }
00598 
00599 // Multiplication: <matrix> * <vector>
00600 
00603 template <class T, int M, int N>
00604 inline Vec<T,M> Mat<T,M,N>::operator * (const Vec<T,N>& v) const {
00605   Vec<T,M> vp(T(0));
00606   for (int i=0,k=0;i<M;i++)
00607     for (int j=0;j<N;j++,k++)
00608       vp.x[i] += x[k]*v.x[j];
00609   return vp;
00610 }
00611 
00614 template <class T, int M, int N>
00615 Vec<T,N> operator * (const Vec<T,M>& v, const Mat<T,M,N> &m) {
00616   Vec<T,N> vp(T(0));
00617   for (int i=0,k=0;i<M;i++)
00618     for (int j=0;j<N;j++,k++)
00619       vp.x[j] += m.x[k]*v.x[i];
00620   return vp;
00621 }
00622 
00623 // Multiplication: <matrix> * <matrix>
00624 
00627 template <class T, int M, int N> template <int P>
00628 inline Mat<T,M,P> Mat<T,M,N>::operator * (const Mat<T,N,P>& m) const {
00629   Mat<T,M,P> mp(T(0));
00630   for (int i=0;i<M;i++)
00631     for (int j=0;j<P;j++)
00632       for (int k=0;k<N;k++)
00633         mp.x[i*P+j] += x[i*N+k]*m.x[k*P+j];
00634   return mp;
00635 }
00636 
00637 
00640 template <class T, int M, int N>
00641 inline void Mat<T,M,N>::operator *= (const Mat<T,N,N>& m) {
00642   (*this) = (*this)*m;
00643 }
00644 
00645 // Equality and inequality tests.
00646 
00650 template <class T, int M, int N>
00651 inline Mat<bool,M,N> Mat<T,M,N>::operator == (const Mat<T,M,N>& m) const {
00652   Mat<bool,M,N> b(false);
00653   for (int i=0;i<M*N;i++) if (x[i] == m.x[i]) b.x[i] = true;
00654   return b;
00655 }
00656 
00660 template <class T, int M, int N>
00661 inline Mat<bool,M,N> Mat<T,M,N>::operator != (const Mat<T,M,N>& m) const {
00662   Mat<bool,M,N> b(false);
00663   for (int i=0;i<M*N;i++) if (x[i] != m.x[i]) b.x[i] = true;
00664   return b;
00665 }
00666 
00670 template <class T, int M, int N>
00671 inline Mat<bool,M,N> Mat<T,M,N>::operator >= (const Mat<T,M,N>& m) const {
00672   Mat<bool,M,N> b(false);
00673   for (int i=0;i<M*N;i++) if (x[i] >= m.x[i]) b.x[i] = true;
00674   return b;
00675 }
00676 
00680 template <class T, int M, int N>
00681 inline Mat<bool,M,N> Mat<T,M,N>::operator <= (const Mat<T,M,N>& m) const {
00682   Mat<bool,M,N> b(false);
00683   for (int i=0;i<M*N;i++) if (x[i] <= m.x[i]) b.x[i] = true;
00684   return b;
00685 }
00686 
00690 template <class T, int M, int N>
00691 inline Mat<bool,M,N> Mat<T,M,N>::operator > (const Mat<T,M,N>& m) const {
00692   Mat<bool,M,N> b(false);
00693   for (int i=0;i<M*N;i++) if (x[i] > m.x[i]) b.x[i] = true;
00694   return b;
00695 }
00696 
00700 template <class T, int M, int N>
00701 inline Mat<bool,M,N> Mat<T,M,N>::operator < (const Mat<T,M,N>& m) const {
00702   Mat<bool,M,N> b(false);
00703   for (int i=0;i<M*N;i++) if (x[i] < m.x[i]) b.x[i] = true;
00704   return b;
00705 }
00706 
00711 template <class T, int M, int N>
00712 inline int Mat<T,M,N>::compareTo(const Mat<T,M,N>& m) const {
00713   int g=0, l=0;
00714   for (int i=0;i<M*N;i++) {
00715     if (x[i] < m.x[i]) l++;
00716     if (x[i] > m.x[i]) g++;
00717   }
00718   if (l==(M*N)) return -1;
00719   else if (g==(M*N)) return 1;
00720   else return 0;
00721 }
00722 
00726 template <class T, int M, int N>
00727 inline bool Mat<T,M,N>::equalTo(const Mat<T,M,N>& m, const T tol) const {
00728   bool t = true;
00729   for (int i=0;i<M*N;i++) if (fabs(x[i] - m.x[i]) > tol) t = false;
00730   return t;
00731 }
00732 
00733 // Transpose
00734 
00737 template <class T, int M, int N>
00738 inline Mat<T,N,M> Mat<T,M,N>::transposed() const {
00739   Mat<T,N,M> mp;
00740   for (int i=0;i<M;i++)
00741     for (int j=0;j<N;j++)
00742       mp.x[j*M+i] = x[i*N+j];
00743   return mp;
00744 }
00745 
00746 // Random matrices
00747 
00753 template <class T, int M, int N>
00754 inline Mat<T,M,N> Mat<T,M,N>::uniformRand(const T a, const T b) {
00755   Mat<T,M,N> m;
00756   for (int i=0;i<M*N;i++) m.x[i] = rtc_uniform_rand<T>(a,b);
00757   return m;
00758 }
00759 
00765 template <class T, int M, int N>
00766 inline Mat<T,M,N> Mat<T,M,N>::normalRand(const T mean, const T stdev) {
00767   Mat<T,M,N> m;
00768   for (int i=0;i<M*N;i++) m.x[i] = rtc_normal_rand<T>(mean,stdev);
00769   return m;
00770 }
00771 
00777 template <class T, int M, int N>
00778 inline Mat<T,M,N> Mat<T,M,N>::multivariateGauss(const Vec<T,M>& mean, const SMat<T,M>& cov) {
00779   Mat<T,M,N> m;
00780   SMat<T,M> S(cov);
00781   int n=S.choleskyDecomp();
00782   S.transpose();
00783   Mat<T,M,N> X = normalRand();
00784   for(int j=0;j<N;++j) m.setCol(j,mean);
00785   m = m + S*X;
00786   return m;
00787 }
00788 
00789 // General elementwise operations
00790 
00794 template <class T, int M, int N>
00795 inline void Mat<T,M,N>::perform(T (*mathFun)(T)) {
00796   for (int i=0;i<M*N;i++) x[i] = (*mathFun)(x[i]);
00797 }
00798 
00802 template <class T, int M, int N>
00803 inline Vec<T,N> Mat<T,M,N>::sum() const {
00804   Vec<T,N> s(T(0));
00805   for (int j=0;j<N;j++)
00806     for (int i=0;i<M;i++) s.x[j] += x[i*N+j];
00807   return s;
00808 }
00809 
00814 template <class T, int M, int N>
00815 inline T Mat<T,M,N>::interpolate(const float i, const float j) const {
00816   const int truncR = rtc_clamp(static_cast<int>(i),0,M-1);
00817   const int truncR1 = rtc_clamp(truncR+1,0,M-1);
00818   const float fractR = i - static_cast<float>(truncR);
00819   const int truncC = rtc_clamp(static_cast<int>(j),0,N-1);
00820   const int truncC1 = rtc_clamp(truncC+1,0,N-1);
00821   const float fractC = j - static_cast<float>(truncC);
00822 
00823   // do the interpolation
00824   const T syx = x[truncR*N+truncC];
00825   const T syx1 = x[truncR*N+truncC1];
00826   const T sy1x = x[truncR1*N+truncC];
00827   const T sy1x1 = x[truncR1*N+truncC1];
00828   // normal interpolation within range
00829   const T tmp1 = syx  + (syx1-syx)*T(fractC);
00830   const T tmp2 = sy1x + (sy1x1-sy1x)*T(fractC);
00831   return (tmp1 + (tmp2-tmp1)*T(fractR));
00832 }
00833 
00839 template <class T, int M, int N>
00840 inline Vec<T,N> Mat<T,M,N>::meanOfRows() const {
00841   Vec<T,N> m(T(0));
00842   for (int j=0;j<N;j++)
00843     for (int i=0;i<M;i++) m.x[j] += x[i*N+j];
00844   m/=static_cast<T>(M);
00845   return m;
00846 }
00847 
00848 /*
00849   * This is a new, faster version, written by Gu Xin:
00850   */
00851 template <class T, int M, int N>
00852 inline SMat<T,N> Mat<T,M,N>::covarianceMatrixOfRows() const {
00853   SMat<T,N> dest(T(0));
00854 
00855   //Implementation for the sum of Matrix[X];
00856   if (M<2) {  //just one row
00857    throw Exception("matrix has to have more than one row");
00858   }
00859 
00860   // mean vector
00861   Vec<T,N> mu(T(0));
00862 
00863   /*
00864   * the loop gets out the result of the Matrix[X];
00865   * Matrix[X]=sum(Matrix[X(i)]);-- i from 0 to n-1--
00866   */
00867   for(int i=0;i<M;i++){
00868    Vec<T,N> tmpV = getRow(i);
00869    mu.add(tmpV);
00870    SMat<T,N> tmp = tmpV.outer(tmpV);
00871    dest.add(tmp);          //add the Matrix[X(i)] to Matrix[sumMatrix];
00872   }
00873   //End of the Implementation for the sum of Matrix[X];
00874 
00875   //Implementation for the Matrix[meanOf];
00876   mu/=static_cast<T>(M);
00877 
00878   SMat<T,N> tmp = mu.outer(mu);
00879   tmp *= static_cast<T>(M);
00880 
00881   //get the result of the difference from Matrix[X] and Matrix[meanOf];
00882   dest.subtract(tmp);
00883   dest/=static_cast<T>(M-1);
00884   return dest;
00885 }
00886 
00887 
00888 // Serialization routines
00889 
00892 template <class T, int M, int N>
00893 inline bool Mat<T,M,N>::write(OutputHandler& oh) const {
00894   return oh.stream().write((char *)(x),M*N*sizeof(T)).good();
00895 }
00896 
00899 template <class T, int M, int N>
00900 inline bool Mat<T,M,N>::read(InputHandler& ih) {
00901   return ih.stream().read((char *)(x),M*N*sizeof(T)).good();
00902 }
00903 
00906 template <class T, int M, int N>
00907 std::ostream& operator<<(std::ostream& os, const Mat<T,M,N>& mat) {
00908 
00909   int minFieldWidth = os.precision()+2;
00910 
00911   //case with 1 row
00912   if (M == 1){
00913     os << "[" ;
00914     for (int i=0; i<N; ++i)
00915       os << std::setw(minFieldWidth) << mat.x[i] << " ";
00916 
00917     os << "]" << std::endl;
00918   }
00919   //case with 2+ rows
00920   else{
00921     //write first row
00922     os << "[" ;
00923     for (int j=0; j<N-1; ++j){
00924       os << std::setw(minFieldWidth) << mat.x[j] << " ";
00925     }
00926     os << std::setw(minFieldWidth) << mat.x[N-1] << ";" << std::endl;
00927     //write middle rows
00928     for (int i=1;i<M-1;++i){
00929       for (int j=0;j<N;++j){
00930         os << " " << std::setw(minFieldWidth) << mat.x[i*N+j];
00931         }
00932       os << ";" << std::endl;
00933       }
00934     //write last row
00935     for (int j=0; j<N; ++j){
00936       os << " " << std::setw(minFieldWidth) << mat.x[N*(M-1)+j];
00937     }
00938     os << "]" << std::endl;
00939   }
00940 
00941   return os;
00942 }
00943 
00946 template <class T, int M, int N>
00947 std::istream& operator>>(std::istream& is, Mat<T,M,N>& mat){
00948   using namespace std;
00949   vector<T> data;
00950   string matString;
00951   stringstream matStringStream;
00952   string rowString;
00953   stringstream rowStringStream;
00954 
00955   getline(is, matString, ']');
00956   int sPos = matString.find('[');
00957   if (sPos == (int)string::npos)
00958     throw Exception("format error: expecting formated matrix to start with '['");
00959 
00960   //erase the starting '['
00961   //note the ending ']' was removed by the getline function as the delim
00962   matString.erase(0,sPos+1);
00963   matStringStream.str(matString);
00964 
00965   //determine num of rows and cols
00966   int rowCount = 0, colCount = 0;
00967   int cols = -1;
00968   float tmpVal;
00969   while(getline(matStringStream, rowString, ';')){
00970     rowStringStream << rowString;
00971 
00972     colCount = 0;
00973     while(rowStringStream.good()){
00974       rowStringStream >> tmpVal;
00975       data.push_back(tmpVal);
00976       ++colCount;
00977     }
00978     rowStringStream.clear();
00979 
00980     //check that we have same num of entries in each row
00981     if (cols == -1)
00982       cols = colCount;
00983     else{
00984       if (colCount != cols){
00985         throw Exception("format error: different number of elements in rows.");
00986       }
00987     }
00988 
00989     ++rowCount;
00990   }
00991 
00992   //check that dimensions agree
00993   if (rowCount != M || colCount != N){
00994     std::stringstream ss;
00995     ss << "format error: formated text is " << rowCount << "x" << colCount;
00996     ss << " while destination matrix is " << M << "x" << N << endl;
00997     throw Exception(ss.str());
00998   }
00999 
01000   //copy extracted data
01001   for (int i=0;i<M*N;i++){
01002     mat.x[i] = data[i];
01003   }
01004 
01005   return is;
01006 }
01007 
01011 template <class T, int M, int N>
01012 bool rtc_write(OutputHandler& oh, const Mat<T,M,N>& data)
01013 {
01014   return data.write(oh);
01015 };
01016 
01020 template <class T, int M, int N>
01021 bool rtc_read(InputHandler& ih, Mat<T,M,N>& data)
01022 {
01023   return data.read(ih);
01024 };
01025 
01026 //==============================================================================
01027 } // NAMESPACE rtc
01028 //==============================================================================
01029 #endif // RTC_MAT_H defined
01030 //==============================================================================