12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix matrix inverse square root so it, once again, returns an upper triangular matrix

release/4.3a0
justinca 2010-02-26 18:53:41 +00:00
parent 940202226b
commit 9941b28128
2 changed files with 19 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cpp/Matrix.cpp
View File

@ -1021,9 +1021,9 @@ Matrix inverse_square_root(const Matrix& A) {
// inv(B) * inv(B)' == A // inv(B) * inv(B)' == A
// inv(B' * B) == A // inv(B' * B) == A
Matrix inverse_square_root(const Matrix& A) { Matrix inverse_square_root(const Matrix& A) {
Matrix L = LLt(A); Matrix R = RtR(A);
Matrix inv(boost::numeric::ublas::identity_matrix<double>(A.size1())); Matrix inv(boost::numeric::ublas::identity_matrix<double>(A.size1()));
inplace_solve (L, inv, BNU::lower_tag ()); inplace_solve (R, inv, BNU::upper_tag ());
return inv; return inv;
} }

15
cpp/testMatrix.cpp
View File

@ -817,18 +817,19 @@ TEST( matrix, inverse_square_root )
// use the same inverse routing inside inverse_square_root() // use the same inverse routing inside inverse_square_root()
// as we use here to check it. // as we use here to check it.
Matrix M = Matrix_(5, 5, 0.0785892, 0.0137923, -0.0142219, -0.0171880, Matrix M = Matrix_(5, 5,
0.0028726, 0.0137923, 0.0908911, 0.0020775, -0.0101952, 0.0175868, 0.0785892, 0.0137923, -0.0142219, -0.0171880, 0.0028726,
-0.0142219, 0.0020775, 0.0973051, 0.0054906, 0.0047064, -0.0171880, 0.0137923, 0.0908911, 0.0020775, -0.0101952, 0.0175868,
-0.0101952, 0.0054906, 0.0892453, -0.0059468, 0.0028726, 0.0175868, -0.0142219, 0.0020775, 0.0973051, 0.0054906, 0.0047064,
0.0047064, -0.0059468, 0.0816517); -0.0171880,-0.0101952, 0.0054906, 0.0892453, -0.0059468,
0.0028726, 0.0175868, 0.0047064, -0.0059468, 0.0816517);
expected = Matrix_(5, 5, expected = trans(Matrix_(5, 5,
3.567126953241796, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, 3.567126953241796, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000,
-0.590030436566913, 3.362022286742925, 0.000000000000000, 0.000000000000000, 0.000000000000000, -0.590030436566913, 3.362022286742925, 0.000000000000000, 0.000000000000000, 0.000000000000000,
0.618207860252376, -0.168166020746503, 3.253086082942785, 0.000000000000000, 0.000000000000000, 0.618207860252376, -0.168166020746503, 3.253086082942785, 0.000000000000000, 0.000000000000000,
0.683045380655496, 0.283773848115276, -0.099969232183396, 3.433537147891568, 0.000000000000000, 0.683045380655496, 0.283773848115276, -0.099969232183396, 3.433537147891568, 0.000000000000000,
-0.006740136923185, -0.669325697387650, -0.169716689114923, 0.171493059476284, 3.583921085468937); -0.006740136923185, -0.669325697387650, -0.169716689114923, 0.171493059476284, 3.583921085468937));
EQUALITY(expected, inverse_square_root(M)); EQUALITY(expected, inverse_square_root(M));
} }