diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index a03ff7648..c86b9b860 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -262,12 +262,12 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // we do something special if (tr + 1.0 < 1e-10) { - if (std::abs(R33 + 1.0) > 1e-10) + if (std::abs(R33 + 1.0) > 1e-5) omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33); - else if (std::abs(R22 + 1.0) > 1e-10) + else if (std::abs(R22 + 1.0) > 1e-5) omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32); else - // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit + // if(std::abs(R.r1_.x()+1.0) > 1e-5) This is implicit omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); } else { double magnitude; diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 74fa829d8..19d696abd 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -243,8 +243,13 @@ TEST(Rot3, log) { Rot3 Rlund(-0.98582676, -0.03958746, -0.16303092, // -0.03997006, -0.88835923, 0.45740671, // -0.16293753, 0.45743998, 0.87418537); - EXPECT(assert_equal(Vector3(-0.264544406, 0.742217405, 3.04117314), - (Vector)Rot3::Logmap(Rlund), 1e-8)); + #if defined(GTSAM_USE_QUATERNIONS) + EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211), + (Vector)Rot3::Logmap(Rlund), 1e-8)); + #else + EXPECT(assert_equal(Vector3(-0.264544406, 0.742217405, 3.04117314), + (Vector)Rot3::Logmap(Rlund), 1e-8)); + #endif } /* ************************************************************************* */