From 9636ace3bbed904f2eb4d9b820830cc8948a4a62 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 21 Dec 2013 04:13:58 +0000
Subject: [PATCH] Added more rodriguez variants -- although some should be
 deprecated (e.g., Point and Vector versions, for which it is possible to give
 incorrect input)

git-svn-id: https://svn.cc.gatech.edu/borg/gtsam/trunk@20419 898a188c-9671-0410-8e00-e3fd810bbb7f
---
 gtsam/geometry/Rot3.cpp | 10 ++++++++++
 gtsam/geometry/Rot3.h   | 21 ++++++++++++++++++++-
 2 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp
index 34b86cd80..0a3b4fd9f 100644
--- a/gtsam/geometry/Rot3.cpp
+++ b/gtsam/geometry/Rot3.cpp
@@ -33,6 +33,16 @@ 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 Sphere2& w, double theta) {
+  return rodriguez(w.point3(),theta);
+}
+
 /* ************************************************************************* */
 Rot3 Rot3::rodriguez(const Vector& w) {
   double t = w.norm();
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index b86a3e899..5bb382a3e 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -150,7 +150,10 @@ namespace gtsam {
     static Rot3 ypr  (double y, double p, double r) { return RzRyRx(r,p,y);}
 
     /** Create from Quaternion coefficients */
-    static Rot3 quaternion(double w, double x, double y, double z) { Quaternion q(w, x, y, z); return Rot3(q); }
+    static Rot3 quaternion(double w, double x, double y, double z) {
+      Quaternion q(w, x, y, z);
+      return Rot3(q);
+    }
 
     /**
      * Rodriguez' formula to compute an incremental rotation matrix
@@ -160,6 +163,22 @@ namespace gtsam {
      */
     static Rot3 rodriguez(const Vector& w, double theta);
 
+    /**
+     * Rodriguez' formula to compute an incremental rotation matrix
+     * @param   w is the rotation axis, unit length
+     * @param   theta rotation angle
+     * @return incremental rotation matrix
+     */
+    static Rot3 rodriguez(const Point3& w, double theta);
+
+    /**
+     * Rodriguez' formula to compute an incremental rotation matrix
+     * @param   w is the rotation axis
+     * @param   theta rotation angle
+     * @return incremental rotation matrix
+     */
+    static Rot3 rodriguez(const Sphere2& w, double theta);
+
     /**
      * Rodriguez' formula to compute an incremental rotation matrix
      * @param v a vector of incremental roll,pitch,yaw