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

return axis-angle representation instead of just angle, updated test to be much simpler

release/4.3a0
Varun Agrawal 2020-03-06 17:54:25 -05:00
parent 0e6efb98d7
commit 087247c6e0
3 changed files with 14 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/geometry/Rot3.cpp
View File

@ -229,8 +229,9 @@ Rot3 Rot3::slerp(double t, const Rot3& other) const {
}
/* ************************************************************************* */
double Rot3::angle(const Rot3& other) const {
return Rot3::Logmap(this->between(other)).norm() * 180.0 / M_PI;
pair<Unit3, double> Rot3::axisAngle(const Rot3& other) const {
Vector3 rot = Rot3::Logmap(this->between(other));
return pair<Unit3, double>(Unit3(rot), rot.norm());
}
/* ************************************************************************* */

4
gtsam/geometry/Rot3.h
View File

@ -480,10 +480,10 @@ namespace gtsam {
Rot3 slerp(double t, const Rot3& other) const;
/**
* @brief Compute the angle (in degrees) between *this and other
* @brief Compute the Euler axis and angle (in radians) between *this and other
* @param other Rot3 element
*/
double angle(const Rot3& other) const;
std::pair<Unit3, double> axisAngle(const Rot3& other) const;
/// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);

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

@ -663,17 +663,17 @@ TEST(Rot3, ClosestTo) {
/* ************************************************************************* */
TEST(Rot3, angle) {
Rot3 R1(0.996136, -0.0564186, -0.0672982, //
0.0419472, 0.978941, -0.199787, //
0.0771527, 0.196192, 0.977525);
Rot3 R1 = Rot3::Ypr(0, 0, 0);
Rot3 R2(0.99755, -0.0377748, -0.0588831, //
0.0238455, 0.974883, -0.221437, //
0.0657689, 0.21949, 0.973395);
Rot3 R2 = Rot3::Ypr(M_PI/4, 0, 0);
double expected = 1.7765686464446904;
Unit3 expectedAxis(0, 0, 1);
double expectedAngle = M_PI/4;
EXPECT(assert_equal(expected, R1.angle(R2), 1e-6));
pair<Unit3, double> actual = R1.axisAngle(R2);
EXPECT(assert_equal(expectedAxis, actual.first, 1e-6));
EXPECT(assert_equal(expectedAngle, actual.second, 1e-6));
}
/* ************************************************************************* */