00001
00002 template <class A1, class A2, class A3> inline void transformCovariance(const FixedMatrix<2,2,A1>& A, const FixedMatrix<2,2,A2>& B, FixedMatrix<2,2,A3>& M)
00003 {
00004 M = A*B*A.T();
00005 }
00006
00007
00008 template <int N, class A1, class A2, class A3> inline void transformCovariance(const FixedMatrix<2,N,A1>& A, const FixedMatrix<N,N,A2>& B, FixedMatrix<2,2,A3>& M)
00009 {
00010 { const Vector<N> ABi = B * A[0];
00011 M[0][0] = ABi * A[0];
00012 M[0][1] = M[1][0] = ABi * A[1];
00013 }
00014 M[1][1] = (B * A[1]) * A[1];
00015 }
00016
00017
00018 template <int N, class A1, class A2, class A3> inline void transformCovariance(const FixedMatrix<3,N,A1>& A, const FixedMatrix<N,N,A2>& B, FixedMatrix<3,3,A3>& M)
00019 {
00020 { const Vector<N> ABi = B * A[0];
00021 M[0][0] = ABi * A[0];
00022 M[0][1] = M[1][0] = ABi * A[1];
00023 M[0][2] = M[2][0] = ABi * A[2];
00024 }
00025 { const Vector<N> ABi = B * A[1];
00026 M[1][1] = ABi * A[1];
00027 M[1][2] = M[2][1] = ABi * A[2];
00028 }
00029 M[2][2] = (B * A[2]) * A[2];
00030 }
00031
00032 #if 0
00033
00034 template <int N, class A1, class A2, class A3> inline void transformCovariance(const FixedMatrix<6,N,A1>& A, const FixedMatrix<N,N,A2>& B, FixedMatrix<6,6,A3>& M)
00035 {
00036 { const Vector<N> ABi = B * A[0];
00037 M[0][0] = ABi * A[0];
00038 M[0][1] = M[1][0] = ABi * A[1];
00039 M[0][2] = M[2][0] = ABi * A[2];
00040 M[0][3] = M[3][0] = ABi * A[3];
00041 M[0][4] = M[4][0] = ABi * A[4];
00042 M[0][5] = M[5][0] = ABi * A[5];
00043 }
00044 { const Vector<N> ABi = B * A[1];
00045 M[1][1] = ABi * A[1];
00046 M[1][2] = M[2][1] = ABi * A[2];
00047 M[1][3] = M[3][1] = ABi * A[3];
00048 M[1][4] = M[4][1] = ABi * A[4];
00049 M[1][5] = M[5][1] = ABi * A[5];
00050 }
00051 { const Vector<N> ABi = B * A[2];
00052 M[2][2] = ABi * A[2];
00053 M[2][3] = M[3][2] = ABi * A[3];
00054 M[2][4] = M[4][2] = ABi * A[4];
00055 M[2][5] = M[5][2] = ABi * A[5];
00056 }
00057 { const Vector<N> ABi = B * A[3];
00058 M[3][3] = ABi * A[3];
00059 M[3][4] = M[4][3] = ABi * A[4];
00060 M[3][5] = M[5][3] = ABi * A[5];
00061 }
00062 { const Vector<N> ABi = B * A[4];
00063 M[4][4] = ABi * A[4];
00064 M[4][5] = M[5][4] = ABi * A[5];
00065 }
00066 M[5][5] = (B * A[5]) * A[5];
00067 }
00068 #endif