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

convert axisAngle to static ToAxisAngle, update tests

release/4.3a0
Varun Agrawal 2020-03-07 08:35:08 -05:00
parent 087247c6e0
commit 7019d6f4b8
4 changed files with 17 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam.h
View File

@ -642,6 +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;
// Testable
void print(string s) const;
@ -677,7 +678,6 @@ class Rot3 {
// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly
Vector quaternion() const;
gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const;
double angle(const gtsam::Rot3& other) const;
// enabling serialization functionality
void serialize() const;

6
gtsam/geometry/Rot3.cpp
View File

@ -228,12 +228,6 @@ Rot3 Rot3::slerp(double t, const Rot3& other) const {
return interpolate(*this, other, t);
}
/* ************************************************************************* */
pair<Unit3, double> Rot3::axisAngle(const Rot3& other) const {
Vector3 rot = Rot3::Logmap(this->between(other));
return pair<Unit3, double>(Unit3(rot), rot.norm());
}
/* ************************************************************************* */
} // namespace gtsam

18
gtsam/geometry/Rot3.h
View File

@ -194,7 +194,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
*/
@ -216,6 +216,16 @@ namespace gtsam {
return AxisAngle(axis.unitVector(),angle);
}
/**
* Compute to 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());
}
/**
* Rodrigues' formula to compute an incremental rotation
* @param w a vector of incremental roll,pitch,yaw
@ -479,12 +489,6 @@ namespace gtsam {
*/
Rot3 slerp(double t, const Rot3& other) const;
/**
* @brief Compute the Euler axis and angle (in radians) between *this and other
* @param other Rot3 element
*/
std::pair<Unit3, double> axisAngle(const Rot3& other) const;
/// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);

11
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>
@ -663,14 +664,12 @@ TEST(Rot3, ClosestTo) {
/* ************************************************************************* */
TEST(Rot3, angle) {
Rot3 R1 = Rot3::Ypr(0, 0, 0);
Rot3 R2 = Rot3::Ypr(M_PI/4, 0, 0);
Rot3 R1 = Rot3::Ypr(M_PI/4, 0, 0);
Unit3 expectedAxis(0, 0, 1);
double expectedAngle = M_PI/4;
Unit3 expectedAxis(-0.350067, -0.472463, 0.808846);
double expectedAngle = 0.709876;
pair<Unit3, double> actual = R1.axisAngle(R2);
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));