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

Merge pull request #243 from borglab/feature/rot3-angle 

Axis-Angle (in radians) representation for Rot3 matrices
release/4.3a0
Frank Dellaert 2020-03-18 14:30:08 -04:00 committed by GitHub
parent 16dbf27375 ca549630de
commit 2c44ee459b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 83 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam.h
View File

@ -639,6 +639,7 @@ class Rot3 {
static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft)
static gtsam::Rot3 Ypr(double y, double p, double r);
static gtsam::Rot3 Quaternion(double w, double x, double y, double z);
static gtsam::Rot3 AxisAngle(const gtsam::Point3& axis, double angle);
static gtsam::Rot3 Rodrigues(Vector v);
static gtsam::Rot3 Rodrigues(double wx, double wy, double wz);
static gtsam::Rot3 ClosestTo(const Matrix M);
@ -674,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;
@ -1310,7 +1312,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;

7
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,12 @@ Vector Rot3::quaternion() const {
return v;
}
/* ************************************************************************* */
pair<Unit3, double> Rot3::axisAngle() const {
const Vector3 omega = Rot3::Logmap(*this);
return std::pair<Unit3, double>(Unit3(omega), omega.norm());
}
/* ************************************************************************* */
Matrix3 Rot3::ExpmapDerivative(const Vector3& x) {
return SO3::ExpmapDerivative(x);

15
gtsam/geometry/Rot3.h
View File

@ -17,6 +17,7 @@
* @author Frank Dellaert
* @author Richard Roberts
* @author Luca Carlone
* @author Varun Agrawal
*/
// \callgraph
@ -194,7 +195,7 @@ namespace gtsam {
/**
* Convert from axis/angle representation
* @param axisw is the rotation axis, unit length
* @param axis is the rotation axis, unit length
* @param angle rotation angle
* @return incremental rotation
*/
@ -429,7 +430,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;
@ -461,6 +462,16 @@ namespace gtsam {
/// @name Advanced Interface
/// @{
/**
* 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() const;
/** Compute the quaternion representation of this rotation.
* @return The quaternion
*/

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

@ -14,6 +14,7 @@
* @brief Unit tests for Rot3 class - common between Matrix and Quaternion
* @author Alireza Fathi
* @author Frank Dellaert
* @author Varun Agrawal
*/
#include <gtsam/geometry/Point3.h>
@ -661,6 +662,65 @@ TEST(Rot3, ClosestTo) {
EXPECT(assert_equal(expected, actual.matrix(), 1e-6));
}
/* ************************************************************************* */
TEST(Rot3, axisAngle) {
Unit3 actualAxis;
double actualAngle;
// 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)))
// 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);
// 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))
// 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))
// 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))
// 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))
const double theta195 = 195 * M_PI / 180;
const double theta165 = 165 * M_PI / 180;
/// 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 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))
}
/* ************************************************************************* */
int main() {
TestResult tr;