static function & method for axis-angle with improved tests and fix for sign ambiguity
							parent
							
								
									7019d6f4b8
								
							
						
					
					
						commit
						0fbe63aa17
					
				
							
								
								
									
										5
									
								
								gtsam.h
								
								
								
								
							
							
						
						
									
										5
									
								
								gtsam.h
								
								
								
								
							|  | @ -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;
 | ||||
|  |  | |||
|  | @ -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); | ||||
|  |  | |||
|  | @ -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 | ||||
|      */ | ||||
|  |  | |||
|  | @ -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); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue