Merge pull request #251 from borglab/fix/rotations

Bunch of fixes for Rot3

gtsam/geometry/Rot3.cpp

@@ -37,7 +37,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

@@ -200,10 +200,12 @@ namespace gtsam {
      * @return incremental rotation
      */
     static Rot3 AxisAngle(const Point3& 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
     }
 
@@ -269,9 +271,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 Rot3(quaternion_.inverse());
+#else
+      return Rot3(rot_.matrix().transpose());
+#endif
     }
 
     /**
@@ -407,7 +413,6 @@ namespace gtsam {
      * Return 3*3 transpose (inverse) rotation matrix
      */
     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;
@@ -440,7 +445,7 @@ namespace gtsam {
      * 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
@@ -448,7 +453,7 @@ namespace gtsam {
      * 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
@@ -456,7 +461,7 @@ namespace gtsam {
      * 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,7 +110,6 @@ Rot3 Rot3::operator*(const Rot3& R2) const {
 }
 
 /* ************************************************************************* */
-// TODO const Eigen::Transpose<const Matrix3> Rot3::transpose() const {
 Matrix3 Rot3::transpose() const {
   return rot_.matrix().transpose();
 }

gtsam/geometry/Rot3Q.cpp

@@ -19,8 +19,8 @@
 
 #ifdef GTSAM_USE_QUATERNIONS
 
-#include <boost/math/constants/constants.hpp>
 #include <gtsam/geometry/Rot3.h>
+#include <boost/math/constants/constants.hpp>
 #include <cmath>
 
 using namespace std;
@@ -79,9 +79,13 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  // TODO: Could we do this? It works in Rot3M but not here, probably because
+  // TODO: Maybe use return type `const Eigen::Transpose<const Matrix3>`?
-  // here we create an intermediate value by calling matrix()
+  // It works in Rot3M but not here, because of some weird behavior
-  // const Eigen::Transpose<const Matrix3> Rot3::transpose() const {
+  // due to Eigen's expression templates which needs more investigation.
+  // For example, if we use matrix(), the value returned has a 1e-10,
+  // and if we use quaternion_.toRotationMatrix(), the matrix is arbitrary.
+  // Using eval() here doesn't help, it only helps if we use it in
+  // the downstream code.
   Matrix3 Rot3::transpose() const {
     return matrix().transpose();
   }

gtsam/geometry/tests/testRot3.cpp

@@ -116,6 +116,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));
 }
 
 /* ************************************************************************* */
@@ -377,7 +381,7 @@ TEST( Rot3, inverse )
   Rot3 actual = R.inverse(actualH);
   CHECK(assert_equal(I,R*actual));
   CHECK(assert_equal(I,actual*R));
-  CHECK(assert_equal((Matrix)actual.matrix(), R.transpose()));
+  CHECK(assert_equal(actual.matrix(), R.transpose()));
 
   Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, R);
   CHECK(assert_equal(numericalH,actualH));

gtsam/navigation/tests/testAHRSFactor.cpp

@@ -15,6 +15,7 @@
  * @author Krunal Chande
  * @author Luca Carlone
  * @author Frank Dellaert
+ * @author Varun Agrawal
  */
 
 #include <gtsam/navigation/AHRSFactor.h>
@@ -200,7 +201,7 @@ TEST(AHRSFactor, Error) {
   // 1e-5 needs to be added only when using quaternions for rotations
 
   EXPECT(assert_equal(H3e, H3a, 1e-5));
-  // FIXME!! DOes not work. Different matrix dimensions.
+  // 1e-5 needs to be added only when using quaternions for rotations
 }
 
 /* ************************************************************************* */