[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[Toon-members] TooN Cholesky.h generated.h helpers.h maccessor...
|
From: |
Ethan Eade |
|
Subject: |
[Toon-members] TooN Cholesky.h generated.h helpers.h maccessor... |
|
Date: |
Mon, 03 Jul 2006 10:03:31 +0000 |
CVSROOT: /cvsroot/toon
Module name: TooN
Changes by: Ethan Eade <ethaneade> 06/07/03 10:03:31
Modified files:
. : Cholesky.h generated.h helpers.h maccessor.hh
util.h vaccessor.hh
Log message:
- Getting the inverse using Cholesky::get_inverse is now more than
twice as fast.
- transformCovariance is faster
- slice<...>() on fixed vectors now checks bounds at compile time
- generated code updated to newer, better algorithms
CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/TooN/Cholesky.h?cvsroot=toon&r1=1.6&r2=1.7
http://cvs.savannah.gnu.org/viewcvs/TooN/generated.h?cvsroot=toon&r1=1.2&r2=1.3
http://cvs.savannah.gnu.org/viewcvs/TooN/helpers.h?cvsroot=toon&r1=1.13&r2=1.14
http://cvs.savannah.gnu.org/viewcvs/TooN/maccessor.hh?cvsroot=toon&r1=1.7&r2=1.8
http://cvs.savannah.gnu.org/viewcvs/TooN/util.h?cvsroot=toon&r1=1.2&r2=1.3
http://cvs.savannah.gnu.org/viewcvs/TooN/vaccessor.hh?cvsroot=toon&r1=1.9&r2=1.10
Patches:
Index: Cholesky.h
===================================================================
RCS file: /cvsroot/toon/TooN/Cholesky.h,v
retrieving revision 1.6
retrieving revision 1.7
diff -u -b -r1.6 -r1.7
--- Cholesky.h 24 May 2006 17:04:33 -0000 1.6
+++ Cholesky.h 3 Jul 2006 10:03:31 -0000 1.7
@@ -77,6 +77,30 @@
}
return rank;
}
+
+ template <int S, class A1, class A2, class A3> inline void
cholesky_inverse(const FixedMatrix<S,S,A1>& L, const FixedVector<S,A2>&
invdiag, FixedMatrix<S,S,A3>& I)
+ {
+ for (int col = 0; col<S; col++) {
+ Vector<S> t,x;
+ for (int i=col; i<S; i++) {
+ double psum = v[i];
+ for (int j=col; j<i; j++)
+ psum -= L[i][j]*t[j];
+ t[i] = invdiag[i] * psum;
+ }
+ for (int i=S-1; i>col; i--) {
+ double psum = t[i];
+ for (int j=i+1; j<S; j++)
+ psum -= L[j][i]*x[j];
+ I[i][col] = I[col][i] = x[i] = invdiag[i] * psum;
+ }
+ double psum = t[col];
+ for (int j=col+1; j<S; j++)
+ psum -= L[j][col]*x[j];
+ I[col][col] = invdiag[col]*psum;
+ }
+ }
+
}
template <int Size=-1>
@@ -122,12 +146,7 @@
}
template <class A> void get_inverse(FixedMatrix<Size,Size,A>& M) const {
- Vector<Size> id_row=zeros<Size>();
- for (int r=0; r<Size; r++) {
- id_row[r] = 1.0;
- util::cholesky_backsub(L, invdiag, id_row, M[r]);
- id_row[r] = 0.0;
- }
+ util::cholesky_inverse(L, invdiag, M);
}
Matrix<Size,Size> get_inverse() const {
Index: generated.h
===================================================================
RCS file: /cvsroot/toon/TooN/generated.h,v
retrieving revision 1.2
retrieving revision 1.3
diff -u -b -r1.2 -r1.3
--- generated.h 5 Jun 2006 17:54:36 -0000 1.2
+++ generated.h 3 Jul 2006 10:03:31 -0000 1.3
@@ -1,3 +1,125 @@
+// Generated for J*C*J^T, C symmetric
+template <class Accessor1, class Accessor2> inline Matrix<2>
transformCovariance(const FixedMatrix<2,2,Accessor1>& A, const
FixedMatrix<2,2,Accessor2>& B)
+{
+ Matrix<2> M;
+ M[0][0] = A[0][0]*(2*Dot<1,1>::eval(A[0], B[0]) + A[0][0]*B[0][0])
+ + A[0][1]*A[0][1]*B[1][1];
+ M[0][1] = M[1][0] = A[0][0]*(B[0]*A[1])
+ + A[0][1]*(B[1]*A[1]);
+ M[1][1] = A[1][0]*(2*Dot<1,1>::eval(A[1], B[0]) + A[1][0]*B[0][0])
+ + A[1][1]*A[1][1]*B[1][1];
+ return M;
+}
+
+// Generated for J*C*J^T, C symmetric
+template <class Accessor1, class Accessor2> inline Matrix<2>
transformCovariance(const FixedMatrix<2,3,Accessor1>& A, const
FixedMatrix<3,3,Accessor2>& B)
+{
+ Matrix<2> M;
+ M[0][0] = A[0][0]*(2*Dot<1,2>::eval(A[0], B[0]) + A[0][0]*B[0][0])
+ + A[0][1]*(2*Dot<2,2>::eval(A[0], B[1]) + A[0][1]*B[1][1])
+ + A[0][2]*A[0][2]*B[2][2];
+ M[0][1] = M[1][0] = A[0][0]*(B[0]*A[1])
+ + A[0][1]*(B[1]*A[1])
+ + A[0][2]*(B[2]*A[1]);
+ M[1][1] = A[1][0]*(2*Dot<1,2>::eval(A[1], B[0]) + A[1][0]*B[0][0])
+ + A[1][1]*(2*Dot<2,2>::eval(A[1], B[1]) + A[1][1]*B[1][1])
+ + A[1][2]*A[1][2]*B[2][2];
+ return M;
+}
+
+// Generated for J*C*J^T, C symmetric
+template <class Accessor1, class Accessor2> inline Matrix<3>
transformCovariance(const FixedMatrix<3,2,Accessor1>& A, const
FixedMatrix<2,2,Accessor2>& B)
+{
+ Matrix<3> M;
+ M[0][0] = A[0][0]*(2*Dot<1,1>::eval(A[0], B[0]) + A[0][0]*B[0][0])
+ + A[0][1]*A[0][1]*B[1][1];
+ M[0][1] = M[1][0] = A[0][0]*(B[0]*A[1])
+ + A[0][1]*(B[1]*A[1]);
+ M[0][2] = M[2][0] = A[0][0]*(B[0]*A[2])
+ + A[0][1]*(B[1]*A[2]);
+ M[1][1] = A[1][0]*(2*Dot<1,1>::eval(A[1], B[0]) + A[1][0]*B[0][0])
+ + A[1][1]*A[1][1]*B[1][1];
+ M[1][2] = M[2][1] = A[1][0]*(B[0]*A[2])
+ + A[1][1]*(B[1]*A[2]);
+ M[2][2] = A[2][0]*(2*Dot<1,1>::eval(A[2], B[0]) + A[2][0]*B[0][0])
+ + A[2][1]*A[2][1]*B[1][1];
+ return M;
+}
+
+// Generated for J*C*J^T, C symmetric
+template <class Accessor1, class Accessor2> inline Matrix<2>
transformCovariance(const FixedMatrix<2,6,Accessor1>& A, const
FixedMatrix<6,6,Accessor2>& B)
+{
+ Matrix<2> M;
+ M[0][0] = A[0][0]*(2*Dot<1,5>::eval(A[0], B[0]) + A[0][0]*B[0][0])
+ + A[0][1]*(2*Dot<2,5>::eval(A[0], B[1]) + A[0][1]*B[1][1])
+ + A[0][2]*(2*Dot<3,5>::eval(A[0], B[2]) + A[0][2]*B[2][2])
+ + A[0][3]*(2*Dot<4,5>::eval(A[0], B[3]) + A[0][3]*B[3][3])
+ + A[0][4]*(2*Dot<5,5>::eval(A[0], B[4]) + A[0][4]*B[4][4])
+ + A[0][5]*A[0][5]*B[5][5];
+ M[0][1] = M[1][0] = A[0][0]*(B[0]*A[1])
+ + A[0][1]*(B[1]*A[1])
+ + A[0][2]*(B[2]*A[1])
+ + A[0][3]*(B[3]*A[1])
+ + A[0][4]*(B[4]*A[1])
+ + A[0][5]*(B[5]*A[1]);
+ M[1][1] = A[1][0]*(2*Dot<1,5>::eval(A[1], B[0]) + A[1][0]*B[0][0])
+ + A[1][1]*(2*Dot<2,5>::eval(A[1], B[1]) + A[1][1]*B[1][1])
+ + A[1][2]*(2*Dot<3,5>::eval(A[1], B[2]) + A[1][2]*B[2][2])
+ + A[1][3]*(2*Dot<4,5>::eval(A[1], B[3]) + A[1][3]*B[3][3])
+ + A[1][4]*(2*Dot<5,5>::eval(A[1], B[4]) + A[1][4]*B[4][4])
+ + A[1][5]*A[1][5]*B[5][5];
+ return M;
+}
+
+// Generated for J*C*J^T, C symmetric
+template <class Accessor1, class Accessor2> inline Matrix<6>
transformCovariance(const FixedMatrix<6,2,Accessor1>& A, const
FixedMatrix<2,2,Accessor2>& B)
+{
+ Matrix<6> M;
+ M[0][0] = A[0][0]*(2*Dot<1,1>::eval(A[0], B[0]) + A[0][0]*B[0][0])
+ + A[0][1]*A[0][1]*B[1][1];
+ M[0][1] = M[1][0] = A[0][0]*(B[0]*A[1])
+ + A[0][1]*(B[1]*A[1]);
+ M[0][2] = M[2][0] = A[0][0]*(B[0]*A[2])
+ + A[0][1]*(B[1]*A[2]);
+ M[0][3] = M[3][0] = A[0][0]*(B[0]*A[3])
+ + A[0][1]*(B[1]*A[3]);
+ M[0][4] = M[4][0] = A[0][0]*(B[0]*A[4])
+ + A[0][1]*(B[1]*A[4]);
+ M[0][5] = M[5][0] = A[0][0]*(B[0]*A[5])
+ + A[0][1]*(B[1]*A[5]);
+ M[1][1] = A[1][0]*(2*Dot<1,1>::eval(A[1], B[0]) + A[1][0]*B[0][0])
+ + A[1][1]*A[1][1]*B[1][1];
+ M[1][2] = M[2][1] = A[1][0]*(B[0]*A[2])
+ + A[1][1]*(B[1]*A[2]);
+ M[1][3] = M[3][1] = A[1][0]*(B[0]*A[3])
+ + A[1][1]*(B[1]*A[3]);
+ M[1][4] = M[4][1] = A[1][0]*(B[0]*A[4])
+ + A[1][1]*(B[1]*A[4]);
+ M[1][5] = M[5][1] = A[1][0]*(B[0]*A[5])
+ + A[1][1]*(B[1]*A[5]);
+ M[2][2] = A[2][0]*(2*Dot<1,1>::eval(A[2], B[0]) + A[2][0]*B[0][0])
+ + A[2][1]*A[2][1]*B[1][1];
+ M[2][3] = M[3][2] = A[2][0]*(B[0]*A[3])
+ + A[2][1]*(B[1]*A[3]);
+ M[2][4] = M[4][2] = A[2][0]*(B[0]*A[4])
+ + A[2][1]*(B[1]*A[4]);
+ M[2][5] = M[5][2] = A[2][0]*(B[0]*A[5])
+ + A[2][1]*(B[1]*A[5]);
+ M[3][3] = A[3][0]*(2*Dot<1,1>::eval(A[3], B[0]) + A[3][0]*B[0][0])
+ + A[3][1]*A[3][1]*B[1][1];
+ M[3][4] = M[4][3] = A[3][0]*(B[0]*A[4])
+ + A[3][1]*(B[1]*A[4]);
+ M[3][5] = M[5][3] = A[3][0]*(B[0]*A[5])
+ + A[3][1]*(B[1]*A[5]);
+ M[4][4] = A[4][0]*(2*Dot<1,1>::eval(A[4], B[0]) + A[4][0]*B[0][0])
+ + A[4][1]*A[4][1]*B[1][1];
+ M[4][5] = M[5][4] = A[4][0]*(B[0]*A[5])
+ + A[4][1]*(B[1]*A[5]);
+ M[5][5] = A[5][0]*(2*Dot<1,1>::eval(A[5], B[0]) + A[5][0]*B[0][0])
+ + A[5][1]*A[5][1]*B[1][1];
+ return M;
+}
+
// Generated for lower triangular L, inverse diagonal invdiag
template <class A1, class A2, class A3, class A4> inline void
cholesky_backsub(const FixedMatrix<6,6,A1>& L, const FixedVector<6,A2>&
invdiag, const FixedVector<6,A3>& v, FixedVector<6,A4>& x)
{
@@ -112,143 +234,94 @@
}
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)
+// Generated for lower triangular L, inverse diagonal invdiag
+template <class A1, class A2, class A3> inline void cholesky_inverse(const
FixedMatrix<6,6,A1>& L, const FixedVector<6,A2>& invdiag, FixedMatrix<6,6,A3>&
I)
{
- 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;
+ { // column 0
+ //forward substitution
+ const double t0 = invdiag[0];
+ const double t1 = -invdiag[1]*(L[1][0]*t0);
+ const double t2 = -invdiag[2]*(L[2][0]*t0 + L[2][1]*t1);
+ const double t3 = -invdiag[3]*(L[3][0]*t0 + L[3][1]*t1 +
L[3][2]*t2);
+ const double t4 = -invdiag[4]*(L[4][0]*t0 + L[4][1]*t1 +
L[4][2]*t2 + L[4][3]*t3);
+ const double t5 = -invdiag[5]*(L[5][0]*t0 + L[5][1]*t1 +
L[5][2]*t2 + L[5][3]*t3 + L[5][4]*t4);
+ //backward substitution
+ const double x5 = invdiag[5]*(t5);
+ const double x4 = invdiag[4]*(t4-L[5][4]*x5);
+ const double x3 = invdiag[3]*(t3-L[4][3]*x4-L[5][3]*x5);
+ const double x2 =
invdiag[2]*(t2-L[3][2]*x3-L[4][2]*x4-L[5][2]*x5);
+ const double x1 =
invdiag[1]*(t1-L[2][1]*x2-L[3][1]*x3-L[4][1]*x4-L[5][1]*x5);
+ const double x0 =
invdiag[0]*(t0-L[1][0]*x1-L[2][0]*x2-L[3][0]*x3-L[4][0]*x4-L[5][0]*x5);
+ I[5][0] = I[0][5] = x5;
+ I[4][0] = I[0][4] = x4;
+ I[3][0] = I[0][3] = x3;
+ I[2][0] = I[0][2] = x2;
+ I[1][0] = I[0][1] = x1;
+ I[0][0] = x0;
+ }
+ { // column 1
+ //forward substitution
+ const double t1 = invdiag[1];
+ const double t2 = -invdiag[2]*(L[2][1]*t1);
+ const double t3 = -invdiag[3]*(L[3][1]*t1 + L[3][2]*t2);
+ const double t4 = -invdiag[4]*(L[4][1]*t1 + L[4][2]*t2 +
L[4][3]*t3);
+ const double t5 = -invdiag[5]*(L[5][1]*t1 + L[5][2]*t2 +
L[5][3]*t3 + L[5][4]*t4);
+ //backward substitution
+ const double x5 = invdiag[5]*(t5);
+ const double x4 = invdiag[4]*(t4-L[5][4]*x5);
+ const double x3 = invdiag[3]*(t3-L[4][3]*x4-L[5][3]*x5);
+ const double x2 =
invdiag[2]*(t2-L[3][2]*x3-L[4][2]*x4-L[5][2]*x5);
+ const double x1 =
invdiag[1]*(t1-L[2][1]*x2-L[3][1]*x3-L[4][1]*x4-L[5][1]*x5);
+ I[5][1] = I[1][5] = x5;
+ I[4][1] = I[1][4] = x4;
+ I[3][1] = I[1][3] = x3;
+ I[2][1] = I[1][2] = x2;
+ I[1][1] = x1;
+ }
+ { // column 2
+ //forward substitution
+ const double t2 = invdiag[2];
+ const double t3 = -invdiag[3]*(L[3][2]*t2);
+ const double t4 = -invdiag[4]*(L[4][2]*t2 + L[4][3]*t3);
+ const double t5 = -invdiag[5]*(L[5][2]*t2 + L[5][3]*t3 +
L[5][4]*t4);
+ //backward substitution
+ const double x5 = invdiag[5]*(t5);
+ const double x4 = invdiag[4]*(t4-L[5][4]*x5);
+ const double x3 = invdiag[3]*(t3-L[4][3]*x4-L[5][3]*x5);
+ const double x2 =
invdiag[2]*(t2-L[3][2]*x3-L[4][2]*x4-L[5][2]*x5);
+ I[5][2] = I[2][5] = x5;
+ I[4][2] = I[2][4] = x4;
+ I[3][2] = I[2][3] = x3;
+ I[2][2] = x2;
+ }
+ { // column 3
+ //forward substitution
+ const double t3 = invdiag[3];
+ const double t4 = -invdiag[4]*(L[4][3]*t3);
+ const double t5 = -invdiag[5]*(L[5][3]*t3 + L[5][4]*t4);
+ //backward substitution
+ const double x5 = invdiag[5]*(t5);
+ const double x4 = invdiag[4]*(t4-L[5][4]*x5);
+ const double x3 = invdiag[3]*(t3-L[4][3]*x4-L[5][3]*x5);
+ I[5][3] = I[3][5] = x5;
+ I[4][3] = I[3][4] = x4;
+ I[3][3] = x3;
+ }
+ { // column 4
+ //forward substitution
+ const double t4 = invdiag[4];
+ const double t5 = -invdiag[5]*(L[5][4]*t4);
+ //backward substitution
+ const double x5 = invdiag[5]*(t5);
+ const double x4 = invdiag[4]*(t4-L[5][4]*x5);
+ I[5][4] = I[4][5] = x5;
+ I[4][4] = x4;
+ }
+ { // column 5
+ //forward substitution
+ const double t5 = invdiag[5];
+ //backward substitution
+ const double x5 = invdiag[5]*(t5);
+ I[5][5] = x5;
+ }
}
Index: helpers.h
===================================================================
RCS file: /cvsroot/toon/TooN/helpers.h,v
retrieving revision 1.13
retrieving revision 1.14
diff -u -b -r1.13 -r1.14
--- helpers.h 14 Jun 2006 11:46:23 -0000 1.13
+++ helpers.h 3 Jul 2006 10:03:31 -0000 1.14
@@ -254,11 +254,8 @@
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];
- }
+ for (int k=0; k<N; k++)
+ sum += A[i][k] * (B[k]*A[j]);
M[i][j] = M[j][i] = sum;
}
}
Index: maccessor.hh
===================================================================
RCS file: /cvsroot/toon/TooN/maccessor.hh,v
retrieving revision 1.7
retrieving revision 1.8
diff -u -b -r1.7 -r1.8
--- maccessor.hh 22 Nov 2005 17:10:06 -0000 1.7
+++ maccessor.hh 3 Jul 2006 10:03:31 -0000 1.8
@@ -380,12 +380,16 @@
// slice
template<int Rstart, int Cstart, int Rsize, int Csize>
inline FixedMatrix<Rsize,Csize,SkipMAccessor<Rsize,Csize,Cols,RowMajor> >&
slice(){
+ util::Assert<(Rstart+Rsize <= Rows)>();
+ util::Assert<(Cstart+Csize <= Cols)>();
return
reinterpret_cast<FixedMatrix<Rsize,Csize,SkipMAccessor<Rsize,Csize,Cols,RowMajor>
>&>
(this->my_values[Rstart*Cols+Cstart]);
}
template<int Rstart, int Cstart, int Rsize, int Csize>
inline const
FixedMatrix<Rsize,Csize,SkipMAccessor<Rsize,Csize,Cols,RowMajor> >& slice()
const {
+ util::Assert<(Rstart+Rsize <= Rows)>();
+ util::Assert<(Cstart+Csize <= Cols)>();
return reinterpret_cast<const
FixedMatrix<Rsize,Csize,SkipMAccessor<Rsize,Csize,Cols,RowMajor> >&>
(this->my_values[Rstart*Cols+Cstart]);
}
Index: util.h
===================================================================
RCS file: /cvsroot/toon/TooN/util.h,v
retrieving revision 1.2
retrieving revision 1.3
diff -u -b -r1.2 -r1.3
--- util.h 24 May 2006 17:04:33 -0000 1.2
+++ util.h 3 Jul 2006 10:03:31 -0000 1.3
@@ -5,6 +5,10 @@
namespace TooN {
#endif
namespace util {
+
+ template <bool Cond> struct Assert;
+ template <> struct Assert<true> {};
+
template <int B, int E> struct Dot {
template <class V1, class V2> static inline double eval(const V1&
v1, const V2& v2) { return v1[B]* v2[B] + Dot<B+1,E>::eval(v1,v2); }
};
Index: vaccessor.hh
===================================================================
RCS file: /cvsroot/toon/TooN/vaccessor.hh,v
retrieving revision 1.9
retrieving revision 1.10
diff -u -b -r1.9 -r1.10
--- vaccessor.hh 27 Mar 2006 14:24:08 -0000 1.9
+++ vaccessor.hh 3 Jul 2006 10:03:31 -0000 1.10
@@ -170,7 +170,6 @@
///////////// FIXED SIZED ACCESSORS ////////////////
-
template <int Size, class AllocZone>
class FixedVAccessor : public AllocZone {
public:
@@ -191,6 +190,7 @@
template<int Start, int Length>
inline FixedVector<Length,FixedVAccessor<Length,Stack<Length> > >& slice()
{
+ util::Assert<(Start+Length <= Size)>();
return
reinterpret_cast<FixedVector<Length,FixedVAccessor<Length,Stack<Length> > >&>
(parent::my_values[Start]);
}
@@ -210,6 +210,7 @@
template<int Start, int Length>
inline const FixedVector<Length,FixedVAccessor<Length,Stack<Length> > >&
slice() const
{
+ util::Assert<(Start+Length <= Size)>();
return reinterpret_cast<const
FixedVector<Length,FixedVAccessor<Length,Stack<Length> > >&>
(parent::my_values[Start]);
}
@@ -260,12 +261,14 @@
template<int Start, int Length>
inline FixedVector<Length, SkipAccessor<Size, Skip> >& slice()
{
+ util::Assert<(Start+Length <= Size)>();
return reinterpret_cast<FixedVector<Length, SkipAccessor<Size, Skip>
>&>(parent::my_values[Start*Skip]);
}
template<int Start, int Length>
inline const FixedVector<Length, SkipAccessor<Size, Skip> >& slice() const
{
+ util::Assert<(Start+Length <= Size)>();
return reinterpret_cast<const FixedVector<Length, SkipAccessor<Size, Skip>
>&>(parent::my_values[Start*Skip]);
}
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [Toon-members] TooN Cholesky.h generated.h helpers.h maccessor...,
Ethan Eade <=