Lots of improvements to Rot3

- Ensure axis in AxisAngle constructor is a unit vector. Added test.
- Improved rotation inverse with support for quaternions.
- Use Eigen::Transpose for transpose return type.
- Roll/Pitch/Yaw is more efficient.
- Fix/Remove old TODOs.

gtsam/geometry/Rot3.cpp

@@ -36,7 +36,6 @@ void Rot3::print(const std::string& s) const {
 
 /* ************************************************************************* */
 Rot3 Rot3::Random(std::mt19937& rng) {
-  // TODO allow any engine without including all of boost :-(
   Unit3 axis = Unit3::Random(rng);
   uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
   double angle = randomAngle(rng);

gtsam/geometry/Rot3.h

@@ -194,15 +194,17 @@ 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
      */
-    static Rot3 AxisAngle(const Point3& axis, double angle) {
+    static Rot3 AxisAngle(const Vector3& axis, double angle) {
+      // Convert to unit vector.
+      Vector3 unitAxis = Unit3(axis).unitVector();
 #ifdef GTSAM_USE_QUATERNIONS
-      return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, axis));
+      return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, unitAxis));
 #else
-      return Rot3(SO3::AxisAngle(axis,angle));
+      return Rot3(SO3::AxisAngle(unitAxis,angle));
 #endif
     }
 
@@ -268,9 +270,13 @@ namespace gtsam {
     /// Syntatic sugar for composing two rotations
     Rot3 operator*(const Rot3& R2) const;
 
-    /// inverse of a rotation, TODO should be different for M/Q
+    /// inverse of a rotation
     Rot3 inverse() const {
-      return Rot3(Matrix3(transpose()));
+#ifdef GTSAM_USE_QUATERNIONS
+      return quaternion_.inverse();
+#else
+      return Rot3(transpose());
+#endif
     }
 
     /**
@@ -405,8 +411,7 @@ namespace gtsam {
     /**
      * Return 3*3 transpose (inverse) rotation matrix
      */
-    Matrix3 transpose() const;
+    const Eigen::Transpose<const Matrix3> transpose() const;
-    // TODO: const Eigen::Transpose<const Matrix3> transpose() const;
 
     /// @deprecated, this is base 1, and was just confusing
     Point3 column(int index) const;
@@ -435,27 +440,18 @@ namespace gtsam {
 
     /**
      * Accessor to get to component of angle representations
-     * NOTE: these are not efficient to get to multiple separate parts,
-     * you should instead use xyz() or ypr()
-     * TODO: make this more efficient
      */
-    inline double roll() const  { return ypr()(2); }
+    inline double roll() const  { return xyz()(0); }
 
     /**
      * Accessor to get to component of angle representations
-     * NOTE: these are not efficient to get to multiple separate parts,
-     * you should instead use xyz() or ypr()
-     * TODO: make this more efficient
      */
-    inline double pitch() const { return ypr()(1); }
+    inline double pitch() const { return xyz()(1); }
 
     /**
      * Accessor to get to component of angle representations
-     * NOTE: these are not efficient to get to multiple separate parts,
-     * you should instead use xyz() or ypr()
-     * TODO: make this more efficient
      */
-    inline double yaw() const   { return ypr()(0); }
+    inline double yaw() const   { return xyz()(2); }
 
     /// @}
     /// @name Advanced Interface

gtsam/geometry/Rot3M.cpp

@@ -110,8 +110,7 @@ Rot3 Rot3::operator*(const Rot3& R2) const {
 }
 
 /* ************************************************************************* */
-// TODO const Eigen::Transpose<const Matrix3> Rot3::transpose() const {
+const Eigen::Transpose<const Matrix3> Rot3::transpose() const {
-Matrix3 Rot3::transpose() const {
   return rot_.matrix().transpose();
 }
 

gtsam/geometry/tests/testRot3.cpp

@@ -115,6 +115,10 @@ TEST( Rot3, AxisAngle)
   CHECK(assert_equal(expected,actual,1e-5));
   Rot3 actual2 = Rot3::AxisAngle(axis, angle-2*M_PI);
   CHECK(assert_equal(expected,actual2,1e-5));
+
+  axis = Vector3(0, 50, 0);
+  Rot3 actual3 = Rot3::AxisAngle(axis, angle);
+  CHECK(assert_equal(expected,actual3,1e-5));
 }
 
 /* ************************************************************************* */