diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index e0cb055b7..cde1ba2ef 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -616,7 +616,7 @@ Matrix inverse_square_root(const Matrix& A) { Matrix R = RtR(A); Matrix inv = eye(A.rows()); R.triangularView().solveInPlace(inv); - return inv; + return inv.transpose(); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index be46b3ddf..a6b1960dd 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -877,12 +877,12 @@ TEST( matrix, inverse_square_root ) -0.0171880,-0.0101952, 0.0054906, 0.0892453, -0.0059468, 0.0028726, 0.0175868, 0.0047064, -0.0059468, 0.0816517); - expected = trans(Matrix_(5, 5, + expected = Matrix_(5, 5, 3.567126953241796, 0.000000000000000, 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.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)); }