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

removed static ToAxisAngle function

release/4.3a0
Varun Agrawal 2020-03-09 16:01:24 -04:00
parent 0fbe63aa17
commit ab46c7c3ce
4 changed files with 8 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam.h
View File

@ -642,7 +642,6 @@ class Rot3 {
static gtsam::Rot3 Rodrigues(Vector v);
static gtsam::Rot3 Rodrigues(double wx, double wy, double wz);
static gtsam::Rot3 ClosestTo(const Matrix M);
static pair<gtsam::Unit3, double> ToAxisAngle(const gtsam::Rot3& R);
// Testable
void print(string s) const;

9
gtsam/geometry/Rot3.cpp
View File

@ -188,7 +188,14 @@ Vector Rot3::quaternion() const {
/* ************************************************************************* */
pair<Unit3, double> Rot3::axisAngle() {
return Rot3::ToAxisAngle(*this);
const Vector3 omega = Rot3::Logmap(*this);
int direction = 1;
// Check if any element in axis is negative.
// This implies that the rotation is clockwise and not counterclockwise.
if (omega.minCoeff() < 0.0) {
direction = -1;
}
return std::pair<Unit3, double>(Unit3(omega), direction * omega.norm());
}
/* ************************************************************************* */

16
gtsam/geometry/Rot3.h
View File

@ -217,22 +217,6 @@ namespace gtsam {
return AxisAngle(axis.unitVector(),angle);
}
/**
* Compute the Euler axis and angle (in radians) representation
* @param R is the rotation matrix
* @return pair consisting of Unit3 axis and angle in radians
*/
static std::pair<Unit3, double> ToAxisAngle(const Rot3& R) {
const Vector3 omega = Rot3::Logmap(R);
int direction = 1;
// Check if any element in axis is negative.
// This implies that the rotation is clockwise and not counterclockwise.
if (omega.minCoeff() < 0.0) {
direction = -1;
}
return std::pair<Unit3, double>(Unit3(omega), direction * omega.norm());
}
/**
* Rodrigues' formula to compute an incremental rotation
* @param w a vector of incremental roll,pitch,yaw

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

@ -662,42 +662,6 @@ TEST(Rot3, ClosestTo) {
EXPECT(assert_equal(expected, actual.matrix(), 1e-6));
}
/* ************************************************************************* */
TEST(Rot3, ToAxisAngle) {
/// Test rotations along each axis
Rot3 R1;
// 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);
EXPECT(assert_equal(Unit3(0, 0, 1), Rot3::ToAxisAngle(yaw.between(R1)).first, 1e-6));
EXPECT_DOUBLES_EQUAL(M_PI/4, Rot3::ToAxisAngle(yaw.between(R1)).second, 1e-9);
EXPECT(assert_equal(Unit3(0, 1, 0), Rot3::ToAxisAngle(pitch.between(R1)).first, 1e-6));
EXPECT_DOUBLES_EQUAL(M_PI/4, Rot3::ToAxisAngle(pitch.between(R1)).second, 1e-9);
EXPECT(assert_equal(Unit3(1, 0, 0), Rot3::ToAxisAngle(roll.between(R1)).first, 1e-6));
EXPECT_DOUBLES_EQUAL(M_PI/4, Rot3::ToAxisAngle(roll.between(R1)).second, 1e-9);
Unit3 axis; double angle;
std::tie(axis,angle) = Rot3::ToAxisAngle(R);
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);
/// Test for sign ambiguity
double 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, Rot3::ToAxisAngle(R2).second, 1e-9);
theta = -M_PI/2; // -90 degrees
R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta);
EXPECT_DOUBLES_EQUAL(theta, Rot3::ToAxisAngle(R2).second, 1e-9);
}
/* ************************************************************************* */
TEST(Rot3, axisAngle) {
/// Test rotations along each axis