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

inverted sign of square root to make inv sqrt positive for simple covariance matrices

release/4.3a0
Frank Dellaert 2010-01-10 13:53:31 +00:00
parent bf315781af
commit f1f21a8589
2 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cpp/Matrix.cpp
View File

@ -618,7 +618,8 @@ Matrix inverse_square_root(const Matrix& A) {
svd(A,U,S,V); svd(A,U,S,V);
// invert and sqrt diagonal of S // invert and sqrt diagonal of S
for(size_t i = 0; i<m; i++) S(i) = pow(S(i),-0.5); // We also arbitrarily choose sign to make result have positive signs
for(size_t i = 0; i<m; i++) S(i) = - pow(S(i),-0.5);
return vector_scale(S, V); // V*S; return vector_scale(S, V); // V*S;
} }

11
cpp/testMatrix.cpp
View File

@ -593,13 +593,14 @@ TEST( matrix, inverse_square_root )
); );
Matrix actual = inverse_square_root(measurement_covariance); Matrix actual = inverse_square_root(measurement_covariance);
Matrix square_root_inverse_covariance = Matrix_(3,3, Matrix expected = Matrix_(3,3,
-2.0, 0.0, 0.0, 2.0, 0.0, 0.0,
0.0, -2.0, 0.0, 0.0, 2.0, 0.0,
0.0, 0.0, -10.0 0.0, 0.0, 10.0
); );
EQUALITY(square_root_inverse_covariance,actual); EQUALITY(expected,actual);
EQUALITY(measurement_covariance,inverse(actual*actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */