Renamed rodriguez variants to preoper naming convention (uppercase for static) and spelling (Rodrigues). Also, now call Expmap in either SO3 or Quaternion.

gtsam/geometry/Rot3.cpp

@@ -33,30 +33,13 @@ void Rot3::print(const std::string& s) const {
   gtsam::print((Matrix)matrix(), s);
 }
 
-/* ************************************************************************* */
-Rot3 Rot3::rodriguez(const Point3& w, double theta) {
-  return rodriguez((Vector)w.vector(),theta);
-}
-
-/* ************************************************************************* */
-Rot3 Rot3::rodriguez(const Unit3& w, double theta) {
-  return rodriguez(w.point3(),theta);
-}
-
 /* ************************************************************************* */
 Rot3 Rot3::Random(boost::mt19937 & rng) {
   // TODO allow any engine without including all of boost :-(
   Unit3 w = Unit3::Random(rng);
   boost::uniform_real<double> randomAngle(-M_PI,M_PI);
   double angle = randomAngle(rng);
-  return rodriguez(w,angle);
+  return Rodrigues(w,angle);
-}
-
-/* ************************************************************************* */
-Rot3 Rot3::rodriguez(const Vector3& w) {
-  double t = w.norm();
-  if (t < 1e-10) return Rot3();
-  return rodriguez(w/t, t);
 }
 
 /* ************************************************************************* */

gtsam/geometry/Rot3.h

@@ -22,8 +22,9 @@
 
 #pragma once
 
-#include <gtsam/geometry/Quaternion.h>
 #include <gtsam/geometry/Unit3.h>
+#include <gtsam/geometry/Quaternion.h>
+#include <gtsam/geometry/SO3.h>
 #include <gtsam/base/concepts.h>
 #include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
 
@@ -149,45 +150,58 @@ namespace gtsam {
     }
 
     /**
-     * Rodriguez' formula to compute an incremental rotation matrix
+     * Rodrigues' formula to compute an incremental rotation matrix
      * @param   w is the rotation axis, unit length
-     * @param   theta rotation angle
+     * @param   angle rotation angle
      * @return incremental rotation matrix
      */
-    static Rot3 rodriguez(const Vector3& w, double theta);
+    static Rot3 Rodrigues(const Vector3& axis, double angle) {
+#ifdef GTSAM_USE_QUATERNIONS
+      return Quaternion(Eigen::AngleAxis<double>(angle, axis));
+#else
+      return SO3::Rodrigues(axis,angle);
+#endif
+      }
 
     /**
-     * Rodriguez' formula to compute an incremental rotation matrix
+     * Rodrigues' formula to compute an incremental rotation matrix
      * @param   w is the rotation axis, unit length
-     * @param   theta rotation angle
+     * @param   angle rotation angle
      * @return incremental rotation matrix
      */
-    static Rot3 rodriguez(const Point3& w, double theta);
+    static Rot3 Rodrigues(const Point3& axis, double angle) {
+      return Rodrigues(axis.vector(),angle);
+    }
 
     /**
-     * Rodriguez' formula to compute an incremental rotation matrix
+     * Rodrigues' formula to compute an incremental rotation matrix
      * @param   w is the rotation axis
-     * @param   theta rotation angle
+     * @param   angle rotation angle
      * @return incremental rotation matrix
      */
-    static Rot3 rodriguez(const Unit3& w, double theta);
+    static Rot3 Rodrigues(const Unit3& axis, double angle) {
+      return Rodrigues(axis.unitVector(),angle);
+    }
 
     /**
-     * Rodriguez' formula to compute an incremental rotation matrix
+     * Rodrigues' formula to compute an incremental rotation matrix
-     * @param v a vector of incremental roll,pitch,yaw
+     * @param w a vector of incremental roll,pitch,yaw
      * @return incremental rotation matrix
      */
-    static Rot3 rodriguez(const Vector3& v);
+    static Rot3 Rodrigues(const Vector3& w) {
+      return Rot3::Expmap(w);
+    }
 
     /**
-     * Rodriguez' formula to compute an incremental rotation matrix
+     * Rodrigues' formula to compute an incremental rotation matrix
      * @param wx Incremental roll (about X)
      * @param wy Incremental pitch (about Y)
      * @param wz Incremental yaw (about Z)
      * @return incremental rotation matrix
      */
-    static Rot3 rodriguez(double wx, double wy, double wz)
+    static Rot3 Rodrigues(double wx, double wy, double wz) {
-      { return rodriguez(Vector3(wx, wy, wz));}
+      return Rodrigues(Vector3(wx, wy, wz));
+    }
 
     /// @}
     /// @name Testable
@@ -272,11 +286,15 @@ namespace gtsam {
 
     /**
      * Exponential map at identity - create a rotation from canonical coordinates
-     * \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula
+     * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
      */
     static Rot3 Expmap(const Vector& v, OptionalJacobian<3,3> H = boost::none) {
       if(H) *H = Rot3::ExpmapDerivative(v);
-      if (zero(v)) return Rot3(); else return rodriguez(v);
+#ifdef GTSAM_USE_QUATERNIONS
+      return traits<Quaternion>::Expmap(v);
+#else
+      return traits<SO3>::Expmap(v);
+#endif
     }
 
     /**
@@ -422,6 +440,14 @@ namespace gtsam {
     GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
 
     /// @}
+    /// @name Deprecated
+    /// @{
+    static Rot3 rodriguez(const Vector3& axis, double angle) { return Rodrigues(axis, angle); }
+    static Rot3 rodriguez(const Point3&  axis, double angle) { return Rodrigues(axis, angle); }
+    static Rot3 rodriguez(const Unit3&   axis, double angle) { return Rodrigues(axis, angle); }
+    static Rot3 rodriguez(const Vector3& w)                  { return Rodrigues(w); }
+    static Rot3 rodriguez(double wx, double wy, double wz)   { return Rodrigues(wx, wy, wz); }
+    /// @}
 
   private:
 

gtsam/geometry/Rot3M.cpp

@@ -117,11 +117,6 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) {
   );
 }
 
-/* ************************************************************************* */
-Rot3 Rot3::rodriguez(const Vector3& w, double theta) {
-  return SO3::Rodrigues(w,theta);
-}
-
 /* ************************************************************************* */
 Rot3 Rot3::operator*(const Rot3& R2) const {
   return Rot3(Matrix3(rot_*R2.rot_));

gtsam/geometry/Rot3Q.cpp

@@ -83,10 +83,6 @@ namespace gtsam {
       Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX())));
   }
 
-  /* ************************************************************************* */
-  Rot3 Rot3::rodriguez(const Vector3& w, double theta) {
-    return Quaternion(Eigen::AngleAxis<double>(theta, w));
-  }
   /* ************************************************************************* */
   Rot3 Rot3::operator*(const Rot3& R2) const {
     return Rot3(quaternion_ * R2.quaternion_);