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

fix for angle outside the range (-pi,pi] and added more tests to verify

release/4.3a0
Varun Agrawal 2020-03-14 10:10:00 -04:00
parent 1553d4321c
commit 8f2a00e4bd
2 changed files with 19 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/geometry/Rot3.cpp
View File

@ -192,14 +192,12 @@ pair<Unit3, double> Rot3::axisAngle() {
// Get the rotation angle. // Get the rotation angle.
double theta = omega.norm(); double theta = omega.norm();
// Check if angle belongs to (-pi, pi]. // Check if angle `theta` belongs to (-pi, pi].
// If yes, rotate in opposite direction to maintain range. // If yes, rotate in opposite direction to maintain range.
if (theta > M_PI) { // Since omega = theta * u, if all coefficients are negative,
theta = theta - 2*M_PI; // then theta is outside the expected range.
omega = -omega; if ((omega.array() < 0).all()) {
} else if (theta <= -M_PI) { theta = -theta;
theta = theta + 2*M_PI;
omega = -omega;
} }
return std::pair<Unit3, double>(Unit3(omega), theta); return std::pair<Unit3, double>(Unit3(omega), theta);
} }

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

@ -686,16 +686,25 @@ TEST(Rot3, axisAngle) {
EXPECT(assert_equal(Unit3(0.1, 0.4, 0.2), axis)); EXPECT(assert_equal(Unit3(0.1, 0.4, 0.2), axis));
EXPECT_DOUBLES_EQUAL(Vector3(0.1, 0.4, 0.2).norm(), angle, 1e-9); EXPECT_DOUBLES_EQUAL(Vector3(0.1, 0.4, 0.2).norm(), angle, 1e-9);
/// Test for sign ambiguity double theta;
double theta = M_PI + M_PI/2; // 270 degrees
Rot3 R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta);
/// Test for sign ambiguity
theta = M_PI + M_PI/2; // 270 degrees
Rot3 R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta);
EXPECT_DOUBLES_EQUAL(theta - 2*M_PI, R2.axisAngle().second, 1e-9); EXPECT_DOUBLES_EQUAL(theta - 2*M_PI, R2.axisAngle().second, 1e-9);
theta = -M_PI/2; // 270 degrees theta = -(M_PI + M_PI/2); // 90 (or -270) degrees
R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta);
EXPECT_DOUBLES_EQUAL(2*M_PI + theta, R2.axisAngle().second, 1e-9);
EXPECT_DOUBLES_EQUAL(theta, R2.axisAngle().second, 1e-9); /// Non-trivial angles
theta = 195 * M_PI / 180; // 195 degrees
Rot3 R3 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta);
EXPECT_DOUBLES_EQUAL(theta - 2*M_PI, R3.axisAngle().second, 1e-9);
theta = -195 * M_PI / 180; // 165 degrees
R3 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta);
EXPECT_DOUBLES_EQUAL(2*M_PI + theta, R3.axisAngle().second, 1e-9);
} }
/* ************************************************************************* */ /* ************************************************************************* */