[Toon-members] TooN helpers.h generated.h

toon-members
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[Toon-members] TooN helpers.h generated.h

From:	Ethan Eade
Subject:	[Toon-members] TooN helpers.h generated.h
Date:	Mon, 05 Jun 2006 17:54:37 +0000
CVSROOT:        /cvsroot/toon
Module name:    TooN
Changes by:     Ethan Eade <ethaneade>  06/06/05 17:54:37

Modified files:
        .              : helpers.h generated.h 

Log message:
        Added transformCovariance to helpers.h and generated variants to 
generated.h.

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/TooN/helpers.h?cvsroot=toon&r1=1.11&r2=1.12
http://cvs.savannah.gnu.org/viewcvs/TooN/generated.h?cvsroot=toon&r1=1.1&r2=1.2

Patches:
Index: helpers.h
===================================================================
RCS file: /cvsroot/toon/TooN/helpers.h,v
retrieving revision 1.11
retrieving revision 1.12
diff -u -b -r1.11 -r1.12
--- helpers.h   24 May 2006 17:04:33 -0000      1.11
+++ helpers.h   5 Jun 2006 17:54:36 -0000       1.12
@@ -239,6 +239,31 @@
   }
 }
 
+template <int R, int N, class Accessor1, class Accessor2> Matrix<R,R> 
transformCovariance(const FixedMatrix<R,N,Accessor1>& A, const 
FixedMatrix<N,N,Accessor2>& B)
+{
+    Matrix<R> M;
+    for (int i=0; i<R; i++) {
+       double sum = 0;
+       for (int k=0; k<N; k++) {
+           double psum = 0;
+           for (int l=k+1; l<N;l++)
+               psum += A[i][l] * B[k][l];
+           sum += (psum * 2 + A[i][k]*B[k][k]) * A[i][k];
+       }
+       M[i][i] = sum;
+       for (int j=i+1; j<R;j++) {
+           double sum = 0;
+           for (int k=0; k<N; k++) {
+               sum += A[i][k]*A[j][k]*B[k][k];
+               for (int l=k+1; l<N;l++)
+                   sum += (A[i][l]*A[j][k] + A[i][k]*A[j][l]) * B[k][l];
+           }
+           M[i][j] = M[j][i] = sum;
+       }
+    }
+    return M;
+}
+
 #ifndef TOON_NO_NAMESPACE
 }
 #endif 

Index: generated.h
===================================================================
RCS file: /cvsroot/toon/TooN/generated.h,v
retrieving revision 1.1
retrieving revision 1.2
diff -u -b -r1.1 -r1.2
--- generated.h 24 May 2006 17:04:33 -0000      1.1
+++ generated.h 5 Jun 2006 17:54:36 -0000       1.2
@@ -112,3 +112,143 @@
        }
        return rank;
 }
+
+// Generated for J*C*J^T, C symmetric
+template <class Accessor1, class Accessor2> Matrix<2> 
transformCovariance(const FixedMatrix<2,2,Accessor1>& A, const 
FixedMatrix<2,2,Accessor2>& B)
+{
+       Matrix<2> M;
+       M[0][0] = A[0][0]*A[0][0]*B[0][0]+ 2*A[0][0]*(A[0][1]*B[0][1])
+               + A[0][1]*A[0][1]*B[1][1];
+       M[0][1] = M[1][0] = A[0][0]*A[1][0]*B[0][0] + 
(A[0][1]*A[1][0]+A[0][0]*A[1][1])*B[0][1]
+                + A[0][1]*A[1][1]*B[1][1]
+               ;
+       M[1][1] = A[1][0]*A[1][0]*B[0][0]+ 2*A[1][0]*(A[1][1]*B[0][1])
+               + A[1][1]*A[1][1]*B[1][1];
+       return M;
+}
+// Generated for J*C*J^T, C symmetric
+template <class Accessor1, class Accessor2> Matrix<2> 
transformCovariance(const FixedMatrix<2,3,Accessor1>& A, const 
FixedMatrix<3,3,Accessor2>& B)
+{
+       Matrix<2> M;
+       M[0][0] = A[0][0]*A[0][0]*B[0][0]+ 2*A[0][0]*(A[0][1]*B[0][1] + 
A[0][2]*B[0][2])
+               + A[0][1]*A[0][1]*B[1][1]+ 2*A[0][1]*(A[0][2]*B[1][2])
+               + A[0][2]*A[0][2]*B[2][2];
+       M[0][1] = M[1][0] = A[0][0]*A[1][0]*B[0][0] + 
(A[0][1]*A[1][0]+A[0][0]*A[1][1])*B[0][1] + 
(A[0][2]*A[1][0]+A[0][0]*A[1][2])*B[0][2]
+                + A[0][1]*A[1][1]*B[1][1] + 
(A[0][2]*A[1][1]+A[0][1]*A[1][2])*B[1][2]
+                + A[0][2]*A[1][2]*B[2][2]
+               ;
+       M[1][1] = A[1][0]*A[1][0]*B[0][0]+ 2*A[1][0]*(A[1][1]*B[0][1] + 
A[1][2]*B[0][2])
+               + A[1][1]*A[1][1]*B[1][1]+ 2*A[1][1]*(A[1][2]*B[1][2])
+               + A[1][2]*A[1][2]*B[2][2];
+       return M;
+}
+// Generated for J*C*J^T, C symmetric
+template <class Accessor1, class Accessor2> Matrix<2> 
transformCovariance(const FixedMatrix<2,6,Accessor1>& A, const 
FixedMatrix<6,6,Accessor2>& B)
+{
+       Matrix<2> M;
+       M[0][0] = A[0][0]*A[0][0]*B[0][0]+ 2*A[0][0]*(A[0][1]*B[0][1] + 
A[0][2]*B[0][2] + A[0][3]*B[0][3] + A[0][4]*B[0][4] + A[0][5]*B[0][5])
+               + A[0][1]*A[0][1]*B[1][1]+ 2*A[0][1]*(A[0][2]*B[1][2] + 
A[0][3]*B[1][3] + A[0][4]*B[1][4] + A[0][5]*B[1][5])
+               + A[0][2]*A[0][2]*B[2][2]+ 2*A[0][2]*(A[0][3]*B[2][3] + 
A[0][4]*B[2][4] + A[0][5]*B[2][5])
+               + A[0][3]*A[0][3]*B[3][3]+ 2*A[0][3]*(A[0][4]*B[3][4] + 
A[0][5]*B[3][5])
+               + A[0][4]*A[0][4]*B[4][4]+ 2*A[0][4]*(A[0][5]*B[4][5])
+               + A[0][5]*A[0][5]*B[5][5];
+       M[0][1] = M[1][0] = A[0][0]*A[1][0]*B[0][0] + 
(A[0][1]*A[1][0]+A[0][0]*A[1][1])*B[0][1] + 
(A[0][2]*A[1][0]+A[0][0]*A[1][2])*B[0][2] + 
(A[0][3]*A[1][0]+A[0][0]*A[1][3])*B[0][3] + 
(A[0][4]*A[1][0]+A[0][0]*A[1][4])*B[0][4] + 
(A[0][5]*A[1][0]+A[0][0]*A[1][5])*B[0][5]
+                + A[0][1]*A[1][1]*B[1][1] + 
(A[0][2]*A[1][1]+A[0][1]*A[1][2])*B[1][2] + 
(A[0][3]*A[1][1]+A[0][1]*A[1][3])*B[1][3] + 
(A[0][4]*A[1][1]+A[0][1]*A[1][4])*B[1][4] + 
(A[0][5]*A[1][1]+A[0][1]*A[1][5])*B[1][5]
+                + A[0][2]*A[1][2]*B[2][2] + 
(A[0][3]*A[1][2]+A[0][2]*A[1][3])*B[2][3] + 
(A[0][4]*A[1][2]+A[0][2]*A[1][4])*B[2][4] + 
(A[0][5]*A[1][2]+A[0][2]*A[1][5])*B[2][5]
+                + A[0][3]*A[1][3]*B[3][3] + 
(A[0][4]*A[1][3]+A[0][3]*A[1][4])*B[3][4] + 
(A[0][5]*A[1][3]+A[0][3]*A[1][5])*B[3][5]
+                + A[0][4]*A[1][4]*B[4][4] + 
(A[0][5]*A[1][4]+A[0][4]*A[1][5])*B[4][5]
+                + A[0][5]*A[1][5]*B[5][5]
+               ;
+       M[1][1] = A[1][0]*A[1][0]*B[0][0]+ 2*A[1][0]*(A[1][1]*B[0][1] + 
A[1][2]*B[0][2] + A[1][3]*B[0][3] + A[1][4]*B[0][4] + A[1][5]*B[0][5])
+               + A[1][1]*A[1][1]*B[1][1]+ 2*A[1][1]*(A[1][2]*B[1][2] + 
A[1][3]*B[1][3] + A[1][4]*B[1][4] + A[1][5]*B[1][5])
+               + A[1][2]*A[1][2]*B[2][2]+ 2*A[1][2]*(A[1][3]*B[2][3] + 
A[1][4]*B[2][4] + A[1][5]*B[2][5])
+               + A[1][3]*A[1][3]*B[3][3]+ 2*A[1][3]*(A[1][4]*B[3][4] + 
A[1][5]*B[3][5])
+               + A[1][4]*A[1][4]*B[4][4]+ 2*A[1][4]*(A[1][5]*B[4][5])
+               + A[1][5]*A[1][5]*B[5][5];
+       return M;
+}
+// Generated for J*C*J^T, C symmetric
+template <class Accessor1, class Accessor2> Matrix<6> 
transformCovariance(const FixedMatrix<6,2,Accessor1>& A, const 
FixedMatrix<2,2,Accessor2>& B)
+{
+       Matrix<6> M;
+       M[0][0] = A[0][0]*A[0][0]*B[0][0]+ 2*A[0][0]*(A[0][1]*B[0][1])
+               + A[0][1]*A[0][1]*B[1][1];
+       M[0][1] = M[1][0] = A[0][0]*A[1][0]*B[0][0] + 
(A[0][1]*A[1][0]+A[0][0]*A[1][1])*B[0][1]
+                + A[0][1]*A[1][1]*B[1][1]
+               ;
+       M[0][2] = M[2][0] = A[0][0]*A[2][0]*B[0][0] + 
(A[0][1]*A[2][0]+A[0][0]*A[2][1])*B[0][1]
+                + A[0][1]*A[2][1]*B[1][1]
+               ;
+       M[0][3] = M[3][0] = A[0][0]*A[3][0]*B[0][0] + 
(A[0][1]*A[3][0]+A[0][0]*A[3][1])*B[0][1]
+                + A[0][1]*A[3][1]*B[1][1]
+               ;
+       M[0][4] = M[4][0] = A[0][0]*A[4][0]*B[0][0] + 
(A[0][1]*A[4][0]+A[0][0]*A[4][1])*B[0][1]
+                + A[0][1]*A[4][1]*B[1][1]
+               ;
+       M[0][5] = M[5][0] = A[0][0]*A[5][0]*B[0][0] + 
(A[0][1]*A[5][0]+A[0][0]*A[5][1])*B[0][1]
+                + A[0][1]*A[5][1]*B[1][1]
+               ;
+       M[1][1] = A[1][0]*A[1][0]*B[0][0]+ 2*A[1][0]*(A[1][1]*B[0][1])
+               + A[1][1]*A[1][1]*B[1][1];
+       M[1][2] = M[2][1] = A[1][0]*A[2][0]*B[0][0] + 
(A[1][1]*A[2][0]+A[1][0]*A[2][1])*B[0][1]
+                + A[1][1]*A[2][1]*B[1][1]
+               ;
+       M[1][3] = M[3][1] = A[1][0]*A[3][0]*B[0][0] + 
(A[1][1]*A[3][0]+A[1][0]*A[3][1])*B[0][1]
+                + A[1][1]*A[3][1]*B[1][1]
+               ;
+       M[1][4] = M[4][1] = A[1][0]*A[4][0]*B[0][0] + 
(A[1][1]*A[4][0]+A[1][0]*A[4][1])*B[0][1]
+                + A[1][1]*A[4][1]*B[1][1]
+               ;
+       M[1][5] = M[5][1] = A[1][0]*A[5][0]*B[0][0] + 
(A[1][1]*A[5][0]+A[1][0]*A[5][1])*B[0][1]
+                + A[1][1]*A[5][1]*B[1][1]
+               ;
+       M[2][2] = A[2][0]*A[2][0]*B[0][0]+ 2*A[2][0]*(A[2][1]*B[0][1])
+               + A[2][1]*A[2][1]*B[1][1];
+       M[2][3] = M[3][2] = A[2][0]*A[3][0]*B[0][0] + 
(A[2][1]*A[3][0]+A[2][0]*A[3][1])*B[0][1]
+                + A[2][1]*A[3][1]*B[1][1]
+               ;
+       M[2][4] = M[4][2] = A[2][0]*A[4][0]*B[0][0] + 
(A[2][1]*A[4][0]+A[2][0]*A[4][1])*B[0][1]
+                + A[2][1]*A[4][1]*B[1][1]
+               ;
+       M[2][5] = M[5][2] = A[2][0]*A[5][0]*B[0][0] + 
(A[2][1]*A[5][0]+A[2][0]*A[5][1])*B[0][1]
+                + A[2][1]*A[5][1]*B[1][1]
+               ;
+       M[3][3] = A[3][0]*A[3][0]*B[0][0]+ 2*A[3][0]*(A[3][1]*B[0][1])
+               + A[3][1]*A[3][1]*B[1][1];
+       M[3][4] = M[4][3] = A[3][0]*A[4][0]*B[0][0] + 
(A[3][1]*A[4][0]+A[3][0]*A[4][1])*B[0][1]
+                + A[3][1]*A[4][1]*B[1][1]
+               ;
+       M[3][5] = M[5][3] = A[3][0]*A[5][0]*B[0][0] + 
(A[3][1]*A[5][0]+A[3][0]*A[5][1])*B[0][1]
+                + A[3][1]*A[5][1]*B[1][1]
+               ;
+       M[4][4] = A[4][0]*A[4][0]*B[0][0]+ 2*A[4][0]*(A[4][1]*B[0][1])
+               + A[4][1]*A[4][1]*B[1][1];
+       M[4][5] = M[5][4] = A[4][0]*A[5][0]*B[0][0] + 
(A[4][1]*A[5][0]+A[4][0]*A[5][1])*B[0][1]
+                + A[4][1]*A[5][1]*B[1][1]
+               ;
+       M[5][5] = A[5][0]*A[5][0]*B[0][0]+ 2*A[5][0]*(A[5][1]*B[0][1])
+               + A[5][1]*A[5][1]*B[1][1];
+       return M;
+}
+
+// Generated for J*C*J^T, C symmetric
+template <class Accessor1, class Accessor2> Matrix<3> 
transformCovariance(const FixedMatrix<3,2,Accessor1>& A, const 
FixedMatrix<2,2,Accessor2>& B)
+{
+       Matrix<3> M;
+       M[0][0] = A[0][0]*A[0][0]*B[0][0]+ 2*A[0][0]*(A[0][1]*B[0][1])
+               + A[0][1]*A[0][1]*B[1][1];
+       M[0][1] = M[1][0] = A[0][0]*A[1][0]*B[0][0] + 
(A[0][1]*A[1][0]+A[0][0]*A[1][1])*B[0][1]
+                + A[0][1]*A[1][1]*B[1][1]
+               ;
+       M[0][2] = M[2][0] = A[0][0]*A[2][0]*B[0][0] + 
(A[0][1]*A[2][0]+A[0][0]*A[2][1])*B[0][1]
+                + A[0][1]*A[2][1]*B[1][1]
+               ;
+       M[1][1] = A[1][0]*A[1][0]*B[0][0]+ 2*A[1][0]*(A[1][1]*B[0][1])
+               + A[1][1]*A[1][1]*B[1][1];
+       M[1][2] = M[2][1] = A[1][0]*A[2][0]*B[0][0] + 
(A[1][1]*A[2][0]+A[1][0]*A[2][1])*B[0][1]
+                + A[1][1]*A[2][1]*B[1][1]
+               ;
+       M[2][2] = A[2][0]*A[2][0]*B[0][0]+ 2*A[2][0]*(A[2][1]*B[0][1])
+               + A[2][1]*A[2][1]*B[1][1];
+       return M;
+}
[Prev in Thread]
Current Thread
[Next in Thread]
[Toon-members] TooN helpers.h generated.h, Ethan Eade <=
Prev by Date: [Toon-members] tag/tag constantvelocity.h kalmanfilter.h measu...
Next by Date: [Toon-members] tag/tag kalmanfilter.h
Previous by thread: [Toon-members] tag/tag constantvelocity.h kalmanfilter.h measu...
Next by thread: [Toon-members] tag/tag kalmanfilter.h
Index(es):
- Date
- Thread