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

static function & method for axis-angle with improved tests and fix for sign ambiguity

release/4.3a0
Varun Agrawal 2020-03-08 17:07:40 -04:00
parent 7019d6f4b8
commit 0fbe63aa17
4 changed files with 94 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam.h
View File

@ -642,7 +642,7 @@ 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 std::pair<gtsam::Unit3, double> ToAxisAngle(const gtsam::Rot3& R) const;
static pair<gtsam::Unit3, double> ToAxisAngle(const gtsam::Rot3& R);
// Testable
void print(string s) const;
@ -675,6 +675,7 @@ class Rot3 {
double roll() const;
double pitch() const;
double yaw() const;
pair<gtsam::Unit3, double> axisAngle() const;
// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly
Vector quaternion() const;
gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const;
@ -1309,7 +1310,7 @@ class SymbolicBayesTree {
// class SymbolicBayesTreeClique {
// BayesTreeClique();
// BayesTreeClique(CONDITIONAL* conditional);
// // BayesTreeClique(const std::pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
// // BayesTreeClique(const pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
//
// bool equals(const This& other, double tol) const;
// void print(string s) const;

6
gtsam/geometry/Rot3.cpp
View File

@ -16,6 +16,7 @@
* @author Christian Potthast
* @author Frank Dellaert
* @author Richard Roberts
* @author Varun Agrawal
*/
#include <gtsam/geometry/Rot3.h>
@ -185,6 +186,11 @@ Vector Rot3::quaternion() const {
return v;
}
/* ************************************************************************* */
pair<Unit3, double> Rot3::axisAngle() {
return Rot3::ToAxisAngle(*this);
}
/* ************************************************************************* */
Matrix3 Rot3::ExpmapDerivative(const Vector3& x) {
return SO3::ExpmapDerivative(x);

20
gtsam/geometry/Rot3.h
View File

@ -17,6 +17,7 @@
* @author Frank Dellaert
* @author Richard Roberts
* @author Luca Carlone
* @author Varun Agrawal
*/
// \callgraph
@ -217,13 +218,19 @@ namespace gtsam {
}
/**
* Compute to the Euler axis and angle (in radians) representation
* 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);
return std::pair<Unit3, double>(Unit3(omega), omega.norm());
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());
}
/**
@ -439,7 +446,7 @@ namespace gtsam {
/**
* Use RQ to calculate roll-pitch-yaw angle representation
* @return a vector containing ypr s.t. R = Rot3::Ypr(y,p,r)
* @return a vector containing rpy s.t. R = Rot3::Ypr(y,p,r)
*/
Vector3 rpy() const;
@ -471,6 +478,13 @@ namespace gtsam {
/// @name Advanced Interface
/// @{
/**
* Compute the Euler axis and angle (in radians) representation
* of this rotation.
* @return pair consisting of Unit3 axis and angle in radians
*/
std::pair<Unit3, double> axisAngle();
/** Compute the quaternion representation of this rotation.
* @return The quaternion
*/

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

@ -663,16 +663,75 @@ TEST(Rot3, ClosestTo) {
}
/* ************************************************************************* */
TEST(Rot3, angle) {
Rot3 R1 = Rot3::Ypr(M_PI/4, 0, 0);
TEST(Rot3, ToAxisAngle) {
/// Test rotations along each axis
Rot3 R1;
Unit3 expectedAxis(-0.350067, -0.472463, 0.808846);
double expectedAngle = 0.709876;
pair<Unit3, double> actual = Rot3::ToAxisAngle(R.between(R1));
EXPECT(assert_equal(expectedAxis, actual.first, 1e-6));
EXPECT(assert_equal(expectedAngle, actual.second, 1e-6));
// 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
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), yaw.between(R1).axisAngle().first, 1e-6));
EXPECT_DOUBLES_EQUAL(M_PI/4, yaw.between(R1).axisAngle().second, 1e-9);
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);
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);
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);
/// 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, R2.axisAngle().second, 1e-9);
theta = -M_PI/2; // 270 degrees
R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta);
EXPECT_DOUBLES_EQUAL(theta, R2.axisAngle().second, 1e-9);
}
/* ************************************************************************* */