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

Fixed numerical problems near +-pi

release/4.3a0
Fan Jiang 2021-10-12 19:02:40 -04:00
parent 21279e6d51
commit 1a97b14138
2 changed files with 37 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
gtsam/geometry/SO3.cpp
View File

@ -261,25 +261,37 @@ 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-5)
omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
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 (tr + 1.0 < 1e-4) {
std::vector<double> diags = {R11, R22, R33};
size_t max_elem = std::distance(diags.begin(), std::max_element(diags.begin(), diags.end()));
if (max_elem == 2) {
const double sgn_w = (R21 - R12) < 0 ? -1.0 : 1.0;
const double r = sqrt(2.0 + 2.0 * R33);
const double correction = 1.0 - M_1_PI / r * (R21 - R12);
omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R33)) * Vector3(R31 + R13, R32 + R23, 2.0 + 2.0 * R33);
} else if (max_elem == 1) {
const double sgn_w = (R13 - R31) < 0 ? -1.0 : 1.0;
const double r = sqrt(2.0 + 2.0 * R22);
const double correction = 1.0 - M_1_PI / r * (R13 - R31);
omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R22)) * Vector3(R21 + R12, 2.0 + 2.0 * R22, R23 + R32);
} else {
// 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);
const double sgn_w = (R32 - R23) < 0 ? -1.0 : 1.0;
const double r = sqrt(2.0 + 2.0 * R11);
const double correction = 1.0 - M_1_PI / r * (R32 - R23);
omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R11)) * Vector3(2.0 + 2.0 * R11, R12 + R21, R13 + R31);
}
} else {
double magnitude;
const double tr_3 = tr - 3.0; // always negative
if (tr_3 < -1e-7) {
if (tr_3 < -1e-6) {
double theta = acos((tr - 1.0) / 2.0);
magnitude = theta / (2.0 * sin(theta));
} else {
// when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
// use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
// see https://github.com/borglab/gtsam/issues/746 for details
magnitude = 0.5 - tr_3 / 12.0;
magnitude = 0.5 - tr_3 / 12.0 + tr_3*tr_3/60.0;
}
omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
}

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

@ -134,7 +134,7 @@ TEST( Rot3, AxisAngle2)
std::tie(actualAxis, actualAngle) = R1.axisAngle();
double expectedAngle = 3.1396582;
CHECK(assert_equal(expectedAngle, actualAngle, 1e-7));
CHECK(assert_equal(expectedAngle, actualAngle, 1e-5));
}
/* ************************************************************************* */
@ -196,13 +196,13 @@ TEST( Rot3, retract)
}
/* ************************************************************************* */
TEST(Rot3, log) {
TEST( Rot3, log) {
static const double PI = boost::math::constants::pi<double>();
Vector w;
Rot3 R;
#define CHECK_OMEGA(X, Y, Z) \
w = (Vector(3) << X, Y, Z).finished(); \
w = (Vector(3) << (X), (Y), (Z)).finished(); \
R = Rot3::Rodrigues(w); \
EXPECT(assert_equal(w, Rot3::Logmap(R), 1e-12));
@ -234,17 +234,17 @@ TEST(Rot3, log) {
CHECK_OMEGA(0, 0, PI)
// Windows and Linux have flipped sign in quaternion mode
#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS)
//#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS)
w = (Vector(3) << x * PI, y * PI, z * PI).finished();
R = Rot3::Rodrigues(w);
EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R), 1e-12));
#else
CHECK_OMEGA(x * PI, y * PI, z * PI)
#endif
//#else
// CHECK_OMEGA(x * PI, y * PI, z * PI)
//#endif
// Check 360 degree rotations
#define CHECK_OMEGA_ZERO(X, Y, Z) \
w = (Vector(3) << X, Y, Z).finished(); \
w = (Vector(3) << (X), (Y), (Z)).finished(); \
R = Rot3::Rodrigues(w); \
EXPECT(assert_equal((Vector)Z_3x1, Rot3::Logmap(R)));
@ -262,15 +262,15 @@ TEST(Rot3, log) {
// Rot3's Logmap returns different, but equivalent compacted
// axis-angle vectors depending on whether Rot3 is implemented
// by Quaternions or SO3.
#if defined(GTSAM_USE_QUATERNIONS)
// Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees
EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211),
#if defined(GTSAM_USE_QUATERNIONS)
// Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees
EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211),
(Vector)Rot3::Logmap(Rlund), 1e-8));
#else
// SO3 will be approximate because of the non-orthogonality
EXPECT(assert_equal(Vector3(0.264485272, -0.742291088, -3.04136444),
(Vector)Rot3::Logmap(Rlund), 1e-8));
#else
// SO3 does not bound angle resulting in ~180.1 degrees
EXPECT(assert_equal(Vector3(-0.264544406, 0.742217405, 3.04117314),
(Vector)Rot3::Logmap(Rlund), 1e-8));
#endif
#endif
}
/* ************************************************************************* */