00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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
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
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
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
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
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
00058 template <class Accessor> void Symmetrize(MatrixBase<Accessor>& m);
00059
00060
00061 template <class Accessor> void Transpose(MatrixBase<Accessor>& m);
00062
00063
00064 template <class Accessor> void Zero(MatrixBase<Accessor>&m);
00065
00066
00067 template <class Accessor> inline void Zero(VectorBase<Accessor>& v);
00068
00069
00070 template <class Accessor> void Fill(MatrixBase<Accessor>&m, double value);
00071
00072
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
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
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
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
00183
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
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
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);
00215 }
00216 }
00217 }
00218
00219
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
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
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
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
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