Moved rotate(Rot3,Sphere2) to Rot3, makes more sense

git-svn-id: https://svn.cc.gatech.edu/borg/gtsam/trunk@20415 898a188c-9671-0410-8e00-e3fd810bbb7f

gtsam/geometry/Rot3.h

@@ -42,6 +42,7 @@
 #include <gtsam/base/DerivedValue.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Sphere2.h>
 
 namespace gtsam {
 
@@ -293,6 +294,17 @@ namespace gtsam {
     Point3 unrotate(const Point3& p, boost::optional<Matrix&> H1 = boost::none,
         boost::optional<Matrix&> H2 = boost::none) const;
 
+    /// @}
+    /// @name Group Action on Sphere2
+    /// @{
+
+    /// rotate 3D direction from rotated coordinate frame to world frame
+    Sphere2 rotate(const Sphere2& p, boost::optional<Matrix&> HR = boost::none,
+        boost::optional<Matrix&> Hp = boost::none) const;
+
+    /// rotate 3D direction from rotated coordinate frame to world frame
+    Sphere2 operator*(const Sphere2& p) const;
+
     /// @}
     /// @name Standard Interface
     /// @{
@@ -303,11 +315,12 @@ namespace gtsam {
     /** return 3*3 transpose (inverse) rotation matrix   */
     Matrix3 transpose() const;
 
-    /** returns column vector specified by index */
+    /// @deprecated, this is base 1, and was just confusing
     Point3 column(int index) const;
-    Point3 r1() const;
+
-    Point3 r2() const;
+    Point3 r1() const; ///< first column
-    Point3 r3() const;
+    Point3 r2() const; ///< second column
+    Point3 r3() const; ///< third column
 
     /**
      * Use RQ to calculate xyz angle representation

gtsam/geometry/Rot3M.cpp

@@ -201,6 +201,22 @@ Point3 Rot3::rotate(const Point3& p,
   return Point3(rot_ * p.vector());
 }
 
+/* ************************************************************************* */
+Sphere2 Rot3::rotate(const Sphere2& p,
+    boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
+  Sphere2 q(rotate(p.point3(Hp)));
+  if (Hp)
+    (*Hp) = q.basis().transpose() * matrix() * (*Hp);
+  if (HR)
+    (*HR) = -q.basis().transpose() * matrix() * p.skew();
+  return q;
+}
+
+/* ************************************************************************* */
+Sphere2 Rot3::operator*(const Sphere2& p) const {
+  return rotate(p);
+}
+
 /* ************************************************************************* */
 // see doc/math.lyx, SO(3) section
 Point3 Rot3::unrotate(const Point3& p,
@@ -346,7 +362,14 @@ Matrix3 Rot3::transpose() const {
 
 /* ************************************************************************* */
 Point3 Rot3::column(int index) const{
-  return Point3(rot_.col(index));
+  if(index == 3)
+    return r3();
+  else if(index == 2)
+    return r2();
+  else if(index == 1)
+    return r1(); // default returns r1
+  else
+    throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
 }
 
 /* ************************************************************************* */

gtsam/geometry/Sphere2.cpp

@@ -67,22 +67,6 @@ Matrix Sphere2::skew() const {
   return skewSymmetric(p_.x(), p_.y(), p_.z());
 }
 
-/* ************************************************************************* */
-Sphere2 Sphere2::Rotate(const Rot3& R, const Sphere2& p,
-    boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) {
-  Sphere2 q(R * p.p_);
-  if (Hp)
-    (*Hp) = q.basis().transpose() * R.matrix() * p.basis();
-  if (HR)
-    (*HR) = -q.basis().transpose() * R.matrix() * p.skew();
-  return q;
-}
-
-/* ************************************************************************* */
-Sphere2 operator*(const Rot3& R, const Sphere2& p) {
-  return Sphere2::Rotate(R, p);
-}
-
 /* ************************************************************************* */
 Vector Sphere2::error(const Sphere2& q, boost::optional<Matrix&> H) const {
   Matrix Bt = basis().transpose();

gtsam/geometry/Sphere2.h

@@ -19,7 +19,7 @@
 
 #pragma once
 
-#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/Point3.h>
 #include <gtsam/base/DerivedValue.h>
 
 namespace gtsam {
@@ -83,14 +83,6 @@ public:
     return p_;
   }
 
-  /// Rotate
-  static Sphere2 Rotate(const Rot3& R, const Sphere2& p,
-      boost::optional<Matrix&> HR = boost::none, boost::optional<Matrix&> Hp =
-          boost::none);
-
-  /// Rotate sugar
-  friend Sphere2 operator*(const Rot3& R, const Sphere2& p);
-
   /// Signed, vector-valued error between two directions
   Vector error(const Sphere2& q,
       boost::optional<Matrix&> H = boost::none) const;

gtsam/geometry/tests/testSphere2.cpp

@@ -17,6 +17,7 @@
  */
 
 #include <gtsam/geometry/Sphere2.h>
+#include <gtsam/geometry/Rot3.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
@@ -36,8 +37,9 @@ Point3 point3_(const Sphere2& p) {
   return p.point3();
 }
 TEST(Sphere2, point3) {
-  vector < Point3 > ps;
+  vector<Point3> ps;
-  ps += Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), Point3(1, 1, 0)/sqrt(2);
+  ps += Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), Point3(1, 1, 0)
+      / sqrt(2);
   Matrix actualH, expectedH;
   BOOST_FOREACH(Point3 p,ps) {
     Sphere2 s(p);
@@ -48,6 +50,10 @@ TEST(Sphere2, point3) {
 }
 
 //*******************************************************************************
+static Sphere2 rotate_(const Rot3& R, const Sphere2& p) {
+  return R * p;
+}
+
 TEST(Sphere2, rotate) {
   Rot3 R = Rot3::yaw(0.5);
   Sphere2 p(1, 0, 0);
@@ -57,15 +63,13 @@ TEST(Sphere2, rotate) {
   Matrix actualH, expectedH;
   // Use numerical derivatives to calculate the expected Jacobian
   {
-    expectedH = numericalDerivative11<Sphere2, Rot3>(
+    expectedH = numericalDerivative21(rotate_,R,p);
-        boost::bind(&Sphere2::Rotate, _1, p, boost::none, boost::none), R);
+    R.rotate(p, actualH, boost::none);
-    Sphere2::Rotate(R, p, actualH, boost::none);
     EXPECT(assert_equal(expectedH, actualH, 1e-9));
   }
   {
-    expectedH = numericalDerivative11<Sphere2, Sphere2>(
+    expectedH = numericalDerivative22(rotate_,R,p);
-        boost::bind(&Sphere2::Rotate, R, _1, boost::none, boost::none), p);
+    R.rotate(p, boost::none, actualH);
-    Sphere2::Rotate(R, p, boost::none, actualH);
     EXPECT(assert_equal(expectedH, actualH, 1e-9));
   }
 }

gtsam/slam/RotateFactor.h

@@ -8,7 +8,7 @@
 #pragma once
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/geometry/Sphere2.h>
+#include <gtsam/geometry/Rot3.h>
 
 namespace gtsam {
 
@@ -44,7 +44,7 @@ public:
     Vector e = p_.error(q, H);
     if (H) {
       Matrix DR;
-      Sphere2::Rotate(R, z_, DR);
+      R.rotate(z_, DR);
       *H = (*H) * DR;
     }
     return e;
@@ -57,7 +57,7 @@ public:
     double e = p_.distance(q, H);
     if (H) {
       Matrix DR;
-      Sphere2::Rotate(R, z_, DR);
+      R.rotate(z_, DR);
       *H = (*H) * DR;
     }
     return (Vector(1) << e);

gtsam/slam/tests/testRotateFactor.cpp

@@ -28,6 +28,7 @@ Point3 cameraX(0, 1, 0), cameraY(0, 0, 1), cameraZ(1, 0, 0);
 Rot3 iRc(cameraX, cameraY, cameraZ);
 // Now, let's create some rotations around IMU frame
 Sphere2 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1);
+Rot3 gRi1 = Rot3::Expmap(Vector3(1, 0, 0));
 // The corresponding rotations in the camera frame
 Sphere2 z1 = iRc.inverse() * p1, z2 = iRc.inverse() * p2, //
 z3 = iRc.inverse() * p3;