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

More systematic tests

release/4.3a0
Frank Dellaert 2020-03-17 13:15:48 -04:00
parent 0d46932456
commit ca549630de
1 changed files with 46 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -664,49 +664,61 @@ TEST(Rot3, ClosestTo) {
/* ************************************************************************* */
TEST(Rot3, axisAngle) {
/// Test rotations along each axis
Rot3 R1;
Unit3 actualAxis;
double actualAngle;
// Negative because rotation is counterclockwise when looking at unchanging axis
Rot3 yaw = Rot3::Ypr(-M_PI/4, 0, 0);
Rot3 pitch = Rot3::Ypr(0, -M_PI/4, 0);
Rot3 roll = Rot3::Ypr(0, 0, -M_PI/4);
// not a lambda as otherwise we can't trace error easily
#define CHECK_AXIS_ANGLE(expectedAxis, expectedAngle, rotation) \
std::tie(actualAxis, actualAngle) = rotation.axisAngle(); \
EXPECT(assert_equal(expectedAxis, actualAxis, 1e-9)); \
EXPECT_DOUBLES_EQUAL(expectedAngle, actualAngle, 1e-9); \
EXPECT(assert_equal(rotation, Rot3::AxisAngle(expectedAxis, expectedAngle)))
EXPECT(assert_equal(Unit3(0, 0, 1), yaw.between(R1).axisAngle().first, 1e-6));
EXPECT_DOUBLES_EQUAL(M_PI/4, yaw.between(R1).axisAngle().second, 1e-9);
// CHECK R defined at top = Rot3::Rodrigues(0.1, 0.4, 0.2)
Vector3 omega(0.1, 0.4, 0.2);
Unit3 axis(omega), _axis(-omega);
CHECK_AXIS_ANGLE(axis, omega.norm(), R);
EXPECT(assert_equal(Unit3(0, 1, 0), pitch.between(R1).axisAngle().first, 1e-6));
EXPECT_DOUBLES_EQUAL(M_PI/4, pitch.between(R1).axisAngle().second, 1e-9);
// rotate by 90
CHECK_AXIS_ANGLE(Unit3(1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, M_PI_2))
CHECK_AXIS_ANGLE(Unit3(0, 1, 0), M_PI_2, Rot3::Ypr(0, M_PI_2, 0))
CHECK_AXIS_ANGLE(Unit3(0, 0, 1), M_PI_2, Rot3::Ypr(M_PI_2, 0, 0))
CHECK_AXIS_ANGLE(axis, M_PI_2, Rot3::AxisAngle(axis, M_PI_2))
EXPECT(assert_equal(Unit3(1, 0, 0), roll.between(R1).axisAngle().first, 1e-6));
EXPECT_DOUBLES_EQUAL(M_PI/4, roll.between(R1).axisAngle().second, 1e-9);
// rotate by -90
CHECK_AXIS_ANGLE(Unit3(-1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, -M_PI_2))
CHECK_AXIS_ANGLE(Unit3(0, -1, 0), M_PI_2, Rot3::Ypr(0, -M_PI_2, 0))
CHECK_AXIS_ANGLE(Unit3(0, 0, -1), M_PI_2, Rot3::Ypr(-M_PI_2, 0, 0))
CHECK_AXIS_ANGLE(_axis, M_PI_2, Rot3::AxisAngle(axis, -M_PI_2))
Unit3 axis; double angle;
std::tie(axis,angle) = R.axisAngle();
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);
// rotate by 270
const double theta270 = M_PI + M_PI / 2;
CHECK_AXIS_ANGLE(Unit3(-1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, theta270))
CHECK_AXIS_ANGLE(Unit3(0, -1, 0), M_PI_2, Rot3::Ypr(0, theta270, 0))
CHECK_AXIS_ANGLE(Unit3(0, 0, -1), M_PI_2, Rot3::Ypr(theta270, 0, 0))
CHECK_AXIS_ANGLE(_axis, M_PI_2, Rot3::AxisAngle(axis, theta270))
double theta;
// rotate by -270
const double theta_270 = -(M_PI + M_PI / 2); // 90 (or -270) degrees
CHECK_AXIS_ANGLE(Unit3(1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, theta_270))
CHECK_AXIS_ANGLE(Unit3(0, 1, 0), M_PI_2, Rot3::Ypr(0, theta_270, 0))
CHECK_AXIS_ANGLE(Unit3(0, 0, 1), M_PI_2, Rot3::Ypr(theta_270, 0, 0))
CHECK_AXIS_ANGLE(axis, M_PI_2, Rot3::AxisAngle(axis, theta_270))
/// 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(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);
const double theta195 = 195 * M_PI / 180;
const double theta165 = 165 * M_PI / 180;
theta = -(M_PI + M_PI/2); // 90 (or -270) degrees
R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta);
EXPECT_DOUBLES_EQUAL(2*M_PI + theta, R2.axisAngle().second, 1e-9);
/// Non-trivial angle 165
CHECK_AXIS_ANGLE(Unit3(1, 0, 0), theta165, Rot3::Ypr(0, 0, theta165))
CHECK_AXIS_ANGLE(Unit3(0, 1, 0), theta165, Rot3::Ypr(0, theta165, 0))
CHECK_AXIS_ANGLE(Unit3(0, 0, 1), theta165, Rot3::Ypr(theta165, 0, 0))
CHECK_AXIS_ANGLE(axis, theta165, Rot3::AxisAngle(axis, theta165))
/// Non-trivial angles
theta = 195 * M_PI / 180; // 195 degrees
Rot3 R3 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta);
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);
EXPECT_DOUBLES_EQUAL(2*M_PI + theta, R3.axisAngle().second, 1e-9);
/// Non-trivial angle 195
CHECK_AXIS_ANGLE(Unit3(-1, 0, 0), theta165, Rot3::Ypr(0, 0, theta195))
CHECK_AXIS_ANGLE(Unit3(0, -1, 0), theta165, Rot3::Ypr(0, theta195, 0))
CHECK_AXIS_ANGLE(Unit3(0, 0, -1), theta165, Rot3::Ypr(theta195, 0, 0))
CHECK_AXIS_ANGLE(_axis, theta165, Rot3::AxisAngle(axis, theta195))
}
/* ************************************************************************* */