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

axisAngle maintains angle between [0, π] so updated docs and tests accordingly

release/4.3a0
Varun Agrawal 2020-03-16 23:33:34 -04:00
parent 8f2a00e4bd
commit 170d1526b7
3 changed files with 8 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/geometry/Rot3.cpp
View File

@ -189,17 +189,7 @@ Vector Rot3::quaternion() const {
/* ************************************************************************* */
pair<Unit3, double> Rot3::axisAngle() {
Vector3 omega = Rot3::Logmap(*this);
// Get the rotation angle.
double theta = omega.norm();
// Check if angle `theta` belongs to (-pi, pi].
// If yes, rotate in opposite direction to maintain range.
// Since omega = theta * u, if all coefficients are negative,
// then theta is outside the expected range.
if ((omega.array() < 0).all()) {
theta = -theta;
}
return std::pair<Unit3, double>(Unit3(omega), theta);
return std::pair<Unit3, double>(Unit3(omega), omega.norm());
}
/* ************************************************************************* */

3
gtsam/geometry/Rot3.h
View File

@ -465,6 +465,9 @@ namespace gtsam {
/**
* Compute the Euler axis and angle (in radians) representation
* of this rotation.
* The angle is in the range [0, π]. If the angle is not in the range,
* the axis is flipped around accordingly so that the returned angle is
* within the specified range.
* @return pair consisting of Unit3 axis and angle in radians
*/
std::pair<Unit3, double> axisAngle();

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

@ -691,7 +691,8 @@ TEST(Rot3, axisAngle) {
/// 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(assert_equal(Unit3(-0.1, -0.3, -0.4), R2.axisAngle().first, 1e-9));
EXPECT_DOUBLES_EQUAL(M_PI/2, R2.axisAngle().second, 1e-9);
theta = -(M_PI + M_PI/2); // 90 (or -270) degrees
R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta);
@ -700,7 +701,8 @@ TEST(Rot3, axisAngle) {
/// 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);
EXPECT(assert_equal(Unit3(-0.1, -0.3, -0.4), R3.axisAngle().first, 1e-9));
EXPECT_DOUBLES_EQUAL(165*M_PI/180, R3.axisAngle().second, 1e-9);
theta = -195 * M_PI / 180; // 165 degrees
R3 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta);