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

Conditioned logmap test on rotation format

release/4.3a0
Binit Shah 2020-06-11 04:55:55 -04:00
parent d0d80f55a6
commit 7c362c3e92
2 changed files with 10 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/SO3.cpp
View File

@ -262,12 +262,12 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
// we do something special // we do something special
if (tr + 1.0 < 1e-10) { 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); 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); omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
else 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); omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
} else { } else {
double magnitude; double magnitude;

9
gtsam/geometry/tests/testRot3.cpp
View File

@ -243,8 +243,13 @@ TEST(Rot3, log) {
Rot3 Rlund(-0.98582676, -0.03958746, -0.16303092, // Rot3 Rlund(-0.98582676, -0.03958746, -0.16303092, //
-0.03997006, -0.88835923, 0.45740671, // -0.03997006, -0.88835923, 0.45740671, //
-0.16293753, 0.45743998, 0.87418537); -0.16293753, 0.45743998, 0.87418537);
EXPECT(assert_equal(Vector3(-0.264544406, 0.742217405, 3.04117314), #if defined(GTSAM_USE_QUATERNIONS)
(Vector)Rot3::Logmap(Rlund), 1e-8)); 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
} }
/* ************************************************************************* */ /* ************************************************************************* */