examples/homography/TooN/helpers.h

00001 
00002 /*                       
00003          Copyright (C) 2005 Tom Drummond
00004 
00005      This library is free software; you can redistribute it and/or
00006      modify it under the terms of the GNU Lesser General Public
00007      License as published by the Free Software Foundation; either
00008      version 2.1 of the License, or (at your option) any later version.
00009 
00010      This library is distributed in the hope that it will be useful,
00011      but WITHOUT ANY WARRANTY; without even the implied warranty of
00012      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00013      Lesser General Public License for more details.
00014 
00015      You should have received a copy of the GNU Lesser General Public
00016      License along with this library; if not, write to the Free Software
00017      Foundation, Inc.
00018      51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
00019 */
00020 #ifndef __NUMHELPERS_H
00021 #define __NUMHELPERS_H
00022 
00023 #include <TooN/TooN.h>
00024 
00025 #ifndef TOON_NO_NAMESPACE
00026 namespace TooN {
00027 #endif 
00028 
00030 
00031 // normalizations (note US spelling)
00032 template <class Accessor> inline void  normalize(VectorBase<Accessor>& v);
00033 template <class Accessor> inline void  normalize_last(VectorBase<Accessor>& v);
00034 template <class Accessor> inline void  normalize_but_last(VectorBase<Accessor>& v);
00035 
00036 // Project
00037 template <int Size, class Accessor> Vector<Size-1>  project(const FixedVector<Size,Accessor>& v);
00038 template  <class Accessor> Vector<>                 project(const DynamicVector<Accessor>& v);
00039 
00040 // Unproject
00041 template <int Size, class Accessor> Vector<Size+1>  unproject(const FixedVector<Size,Accessor>& v);
00042 template  <class Accessor> Vector<>                 unproject(const DynamicVector<Accessor>& v);
00043 
00044 
00045 // as_vector
00046 template<int Size> inline FixedVector<Size,FixedVAccessor<Size,Stack<Size> > >&  as_vector(double* data);
00047 template<int Size> inline const FixedVector<Size,FixedVAccessor<Size,Stack<Size> > >&  as_vector(const double* data);
00048 
00049 // set a matrix to (a multiple of) the Identity
00050 template <class Accessor> void  Identity(MatrixBase<Accessor>& M, const double factor=1);
00051  template <int N> Matrix<N> Identity(const double factor=1) {
00052      Matrix<N> M;
00053      Identity(M,factor);
00054      return M;
00055  }
00056 
00057 // symmetrize a matrix
00058 template <class Accessor> void Symmetrize(MatrixBase<Accessor>& m);
00059 
00060 // transpose a matrix
00061 template <class Accessor> void Transpose(MatrixBase<Accessor>& m);
00062 
00063 // set a matrix to Zero
00064 template <class Accessor> void Zero(MatrixBase<Accessor>&m);
00065 
00066 // set a vector to zero
00067 template <class Accessor> inline void Zero(VectorBase<Accessor>& v);
00068 
00069 // Fill a matrix with a certain value
00070 template <class Accessor> void Fill(MatrixBase<Accessor>&m, double value);
00071 
00072 // Fill a vector with a certain value
00073 template <class Accessor> inline void Fill(VectorBase<Accessor>& v, double value);
00074 
00075  template <class T, int N> struct ZeroBlock {
00076      static const T data[N];
00077  };
00078 
00079  template <class T, int N> const T ZeroBlock<T,N>::data[N] = {0}; 
00080  
00081  template <int M, int N> inline const Matrix<M,N>& zeros() { return *reinterpret_cast<const Matrix<M,N>*>(ZeroBlock<double,M*N>::data); }
00082  template <int N> inline const Vector<N>& zeros() { return *reinterpret_cast<const Vector<N>*>(ZeroBlock<double,N>::data); }
00084 
00085 
00086 
00087 // normalizations (note US spelling)
00088 template <class Accessor>
00089 inline void normalize(VectorBase<Accessor>& v){
00090     double sumSq = v[0]*v[0];
00091     for (int i=1; i<v.size(); i++)
00092         sumSq += v[i]*v[i];
00093     double factor = 1.0/sqrt(sumSq);
00094     for (int i=0; i<v.size(); i++)
00095         v[i] *= factor;
00096 }
00097 
00098 template <class Accessor>
00099 inline void normalize_last(VectorBase<Accessor>& v){
00100   const double scalefac = 1/v[v.size()-1];
00101   for(int i=0; i<v.size(); i++){
00102     v[i]*=scalefac;
00103   }
00104 }
00105 
00106 template <class Accessor>
00107 inline void normalize_but_last(VectorBase<Accessor>& v){
00108   double sumsq=0;
00109   for(int i=0; i<v.size()-1; i++){
00110     sumsq += v[i] * v[i];
00111   }
00112   const double scalefac = 1/sqrt(sumsq);
00113   for(int i=0; i<v.size(); i++){
00114     v[i]*=scalefac;
00115   }
00116 }
00117 
00118 
00119 // Project
00120 template <int Size, class Accessor>
00121 struct FixedVProject {
00122   inline static void eval(Vector<Size-1>& ret, const FixedVector<Size,Accessor>& v){
00123     const double scalefac = 1/v[Size-1];
00124     for(int i=0; i<Size-1; i++){
00125       ret[i]=v[i]*scalefac;
00126     }
00127   }
00128 };
00129 
00130   template <int Size, class Accessor> inline 
00131 Vector<Size-1> project(const FixedVector<Size,Accessor>& v){
00132   return Vector<Size-1>(v,Operator<FixedVProject<Size,Accessor> >());
00133 }
00134 
00135 template <class Accessor>
00136 struct DynamicVProject : public VSizer{
00137   inline static void eval(Vector<>& ret, const DynamicVector<Accessor>& v){
00138     const int size = v.size();
00139     set_size(ret,size-1);
00140     const double scalefac = 1/v[size-1];
00141     for(int i=0; i<size-1; i++){
00142       ret[i]=v[i]*scalefac;
00143     }
00144   }
00145 };
00146 
00147 template  <class Accessor>
00148 Vector<> project(const DynamicVector<Accessor>& v){
00149   return Vector<>(v,Operator<DynamicVProject<Accessor> >());
00150 }
00151 
00152 // Unproject
00153 template <int Size, class Accessor>
00154 struct FixedVUnproject {
00155   inline static void eval(Vector<Size+1>& ret, const FixedVector<Size,Accessor>& v){
00156     ret.template slice<0,Size>() = v;
00157     ret[Size]=1;
00158   }
00159 };
00160 
00161 template <int Size, class Accessor>
00162 Vector<Size+1> unproject(const FixedVector<Size,Accessor>& v){
00163   return Vector<Size+1>(v,Operator<FixedVUnproject<Size,Accessor> >());
00164 }
00165 
00166 template <class Accessor>
00167 struct DynamicVUnproject : public VSizer{
00168   inline static void eval(Vector<>& ret, const DynamicVector<Accessor>& v){
00169     const int size = v.size();
00170     set_size(ret,size+1);
00171     v.copy_into(ret.get_data_ptr());
00172     ret[size]=1;
00173   }
00174 };
00175 
00176 template  <class Accessor>
00177 Vector<> unproject(const DynamicVector<Accessor>& v){
00178   return Vector<>(v,Operator<DynamicVUnproject<Accessor> >());
00179 }
00180 
00181 
00182 // as_vector<Size>(double*) to convert a pointer to
00183 // an array of data into a Vector<Size>
00184 template<int Size>
00185 inline FixedVector<Size,FixedVAccessor<Size,Stack<Size> > >&  as_vector(double* data){
00186   return reinterpret_cast<FixedVector<Size,FixedVAccessor<Size,Stack<Size> > >&>(*data);
00187 }
00188 
00189 template<int Size>
00190 inline const FixedVector<Size,FixedVAccessor<Size,Stack<Size> > >&  as_vector(const double* data){
00191   return reinterpret_cast<const FixedVector<Size,FixedVAccessor<Size,Stack<Size> > >&>(*data);
00192 }
00193 
00194 
00195 // set a matrix to (a multiple of) the Identity
00196 template <class Accessor>
00197   void Identity(MatrixBase<Accessor>& M, const double factor){
00198   assert(M.num_rows() == M.num_cols());
00199   for(int i=0; i<M.num_rows(); i++){
00200     for(int j=0; j<M.num_cols(); j++){
00201       M(i,j)=0;
00202     }
00203     M(i,i)=factor;
00204   }
00205 };
00206 
00207 
00208 // symmetrize a matrix
00209 template <class Accessor>
00210 void Symmetrize(MatrixBase<Accessor>& m){
00211   assert(m.num_rows()==m.num_cols());
00212   for(int r=0; r<m.num_rows()-1; r++){
00213     for(int c=r; c<m.num_rows(); c++){
00214         m(c,r) = m(r,c);// = 0.5*(m(r,c)+m(c,r));
00215     }
00216   }
00217 }
00218 
00219 // Transpose a matrix
00220 template<class Accessor>
00221 void Transpose(MatrixBase<Accessor>& m){
00222   assert(m.num_rows() == m.num_cols());
00223   for(int r=0; r<m.num_rows()-1; r++){
00224     for(int c=r; c<m.num_rows(); c++){
00225       double temp = m(r,c);
00226       m(r,c) = m(c,r);
00227       m(c,r) = temp;
00228     }
00229   }
00230 }
00231 
00232 // set a Matrix to zero
00233 template <class Accessor> void Zero(MatrixBase<Accessor>&m){
00234   for(int r=0; r<m.num_rows(); r++){
00235     for(int c=0; c<m.num_cols(); c++){
00236       m(r,c) = 0;
00237     }
00238   }
00239 }
00240 
00241 // set a vector to zero
00242 template <class Accessor> inline void Zero(VectorBase<Accessor>& v){
00243   for(int i=0; i<v.size(); i++){
00244     v[i]=0;
00245   }
00246 }
00247 
00248 // Fill a matrix with a certain value
00249 template <class Accessor> void Fill(MatrixBase<Accessor>&m, double value){
00250   for(int r=0; r<m.num_rows(); r++){
00251     for(int c=0; c<m.num_cols(); c++){
00252       m(r,c) = value;
00253     }
00254   }
00255 }
00256 
00257 // Fill a vector with a certain value
00258 template <class Accessor> inline void Fill(VectorBase<Accessor>& v, double value){
00259   for(int i=0; i<v.size(); i++){
00260     v[i]=value;
00261   }
00262 }
00263 
00264  namespace util {
00265      template <class F, int R, int N, class A1, class A2, class A3> inline void transformCovariance(const FixedMatrix<R,N,A1>& A, const FixedMatrix<N,N,A2>& B, FixedMatrix<R,R,A3>& M)
00266      {
00267          for (int i=0; i<R; ++i) {
00268              const Vector<N> ABi = B * A[i];
00269              F::eval(M[i][i],ABi * A[i]);
00270              for (int j=i+1; j<R; ++j) {
00271                  const double v = ABi * A[j];
00272                  F::eval(M[i][j], v);
00273                  F::eval(M[j][i], v);
00274              }
00275          }
00276      }
00277 
00278      template <class F, int R, int N, class A1, class A2, class A3> inline void transformCovarianceUpper(const FixedMatrix<R,N,A1>& A, const FixedMatrix<N,N,A2>& B, FixedMatrix<R,R,A3>& M)
00279      {
00280          for (int i=0; i<R; ++i) {
00281              const Vector<N> ABi = B * A[i];
00282              for (int j=i; j<R; ++j)
00283                  F::eval(M[i][j], ABi * A[j]);
00284          }
00285      }
00286 
00287      template <class F, class A1, class M2, class M3> inline void transformCovarianceUpper(const DynamicMatrix<A1>& A, const M2 & B, M3 & M)
00288      {
00289         const int R = A.num_rows();
00290         const int N = A.num_cols();
00291         assert(M.num_rows() == R &&
00292                M.num_cols() == R &&
00293                B.num_rows() == N &&
00294                B.num_cols() == N);
00295          for (int i=0; i<R; ++i) {
00296              const Vector<> ABi = B * A[i];
00297              for (int j=i; j<R; ++j)
00298                  F::eval(M[i][j], ABi * A[j]);
00299          }
00300      }
00301 
00302      template <class F, class A1, class M2, class MatM> inline void transformCovariance(const DynamicMatrix<A1> & A, const M2 & B, MatM& M)
00303      {
00304         const int R = A.num_rows();
00305         const int N = A.num_cols();
00306         assert(M.num_rows() == R &&
00307                M.num_cols() == R &&
00308                B.num_rows() == N &&
00309                B.num_cols() == N);
00310         for (int i=0; i<R; ++i) {
00311              const Vector<> ABi = B * A[i];
00312              F::eval(M[i][i], ABi * A[i]);
00313              for (int j=i+1; j<R; ++j){
00314                 const double v = ABi * A[j];
00315                 F::eval(M[j][i], v);
00316                 F::eval(M[i][j], v);
00317              }
00318          }
00319      }
00320 
00321      template <class F, int R, int N, class A1, class A2, class MatM> inline void transformCovariance(const FixedMatrix<R,N,A1> & A, const DynamicMatrix<A2> & B, MatM& M)
00322      {
00323         assert(M.num_rows() == R &&
00324                M.num_cols() == R &&
00325                B.num_rows() == N &&
00326                B.num_cols() == N);
00327         for (int i=0; i<R; ++i) {
00328              const Vector<N> ABi = B * A[i];
00329              F::eval(M[i][i], ABi * A[i]);
00330              for (int j=i+1; j<R; ++j){
00331                 const double v = ABi * A[j];
00332                 F::eval(M[j][i], v);
00333                 F::eval(M[i][j], v);
00334              }
00335          }
00336      }
00337 
00338      template <class F, int R, int N, class A1, class A2, class A3> inline void transformCovariance(const FixedMatrix<R,N,A1> & A, const FixedMatrix<N,N,A2> & B, DynamicMatrix<A3> & M)
00339      {
00340         assert(M.num_rows() == R &&
00341                M.num_cols() == R );
00342         for (int i=0; i<R; ++i) {
00343              const Vector<N> ABi = B * A[i];
00344              F::eval(M[i][i], ABi * A[i]);
00345              for (int j=i+1; j<R; ++j){
00346                 const double v = ABi * A[j];
00347                 F::eval(M[j][i], v);
00348                 F::eval(M[i][j], v);
00349              }
00350          }
00351      }
00352 
00353  }
00354  
00355  template <class A1, class A2> inline Matrix<> transformCovariance(const DynamicMatrix<A1> & A, const A2 & B)
00356  {
00357      Matrix<> M(A.num_rows(), A.num_rows());
00358      util::transformCovariance<util::Assign>(A,B,M);
00359      return M;
00360  }
00361 
00362 template <int R, int N, class A1, class A2> inline Matrix<R> transformCovariance(const FixedMatrix<R, N, A1> & A, const DynamicMatrix<A2> & B)
00363  {
00364      assert(B.num_cols() == N && B.num_rows() == N);
00365      Matrix<R> M;
00366      util::transformCovariance<util::Assign>(A,B,M);
00367      return M;
00368  }
00369 
00370  template <int R, int N, class Accessor1, class Accessor2> inline Matrix<R> transformCovariance(const FixedMatrix<R,N,Accessor1>& A, const FixedMatrix<N,N,Accessor2>& B)
00371  {
00372      Matrix<R> M;
00373      util::transformCovariance<util::Assign>(A,B,M);
00374      return M;
00375  }
00376 
00377  template <class MatA, class MatB, class MatM> inline void transformCovariance(const MatA& A, const MatB& B, MatM& M)
00378  {
00379      util::transformCovariance<util::Assign>(A,B,M);
00380  }
00381 
00382  template <class F, class MatA, class MatB, class MatM> void transformCovariance(const MatA& A, const MatB& B, MatM& M)
00383  {
00384      util::transformCovariance<F>(A,B,M);
00385  }
00386 
00387  template <int M, int N, int C, class A, class B, class Mat> inline void add_product(const FixedMatrix<M,N,A>& Ma, const FixedMatrix<N,C,B>& Mb, Mat& Mc)
00388  {
00389      util::matrix_multiply<util::PlusEquals,M,N,C>(Ma,Mb,Mc);
00390  }
00391 
00392  template <int M, int N, class A, class B, class Vec> inline void add_product(const FixedMatrix<M,N,A>& m, const FixedVector<N,B>& v, Vec& r)
00393  {
00394      util::matrix_multiply<util::PlusEquals,M,N,1>(m,v.as_col(),r.as_col());
00395  }
00396  
00397  template <class A, class B, class C> inline void add_product(const DynamicMatrix<A>& Ma, const DynamicMatrix<B>& Mb, DynamicMatrix<C>& r)
00398  {
00399      const int M=Ma.num_rows();
00400      const int N=Ma.num_cols();
00401      const int R=Mb.num_cols();
00402      for (int i=0; i<M; ++i)
00403          for (int j=0; j<R; ++j) {
00404              double sum = 0;
00405              for (int k=0; k<N; ++k)
00406                  sum += Ma(i,k)*Mb(k,j);
00407              r(i,j) += sum;
00408          }
00409  }
00410 
00411  template <class A, class B, class Vec> inline void add_product(const DynamicMatrix<A>& Ma, const DynamicVector<B>& Mb, Vec & r)
00412  {
00413      const int M=Ma.num_rows();
00414      const int N=Ma.num_cols();
00415      assert(N == Mb.size());
00416      for (int i=0; i<M; ++i) {
00417         double sum = 0;
00418         for (int k=0; k<N; ++k)
00419             sum += Ma(i,k)*Mb[k];
00420         r[i] += sum;
00421      }
00422  }
00423 
00424  template <int M, int N, int C, class A1, class A2, class Mat> void matrix_multiply(const FixedMatrix<M,N,A1>& A, const FixedMatrix<N,C,A2>& B, Mat& R)
00425  {
00426      util::matrix_multiply<M,N,C>(A,B,R);
00427  }
00428 
00429 #ifndef TOON_NO_NAMESPACE
00430 }
00431 #endif 
00432 
00433 #endif

Generated on Fri Feb 22 18:26:54 2008 for QVision by  doxygen 1.5.3