new constructor, getBasis->basis, rotate and distance

gtsam.h

@@ -574,7 +574,7 @@ virtual class Sphere2 : gtsam::Value {
   bool equals(const gtsam::Sphere2& pose, double tol) const;
 
   // Other functionality
-  Matrix getBasis() const;
+  Matrix basis() const;
   Matrix skew() const;
 
   // Manifold

gtsam/geometry/EssentialMatrix.h

@@ -118,7 +118,7 @@ public:
       // See math.lyx
       Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB);
       Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
-          * aTb_.getBasis();
+          * aTb_.basis();
       *H << HR, HD;
     }
     return dot(vA, E_ * vB);

gtsam/geometry/Sphere2.cpp

@@ -14,7 +14,7 @@
  * @date Feb 02, 2011
  * @author Can Erdogan
  * @author Frank Dellaert
- * @brief Develop a Sphere2 class - basically a point on a unit sphere
+ * @brief The Sphere2 class - basically a point on a unit sphere
  */
 
 #include <gtsam/geometry/Sphere2.h>
@@ -26,7 +26,11 @@ using namespace std;
 namespace gtsam {
 
 /* ************************************************************************* */
-Matrix Sphere2::getBasis() const {
+Matrix Sphere2::basis() const {
+
+  // Return cached version if exists
+  if (B_.rows() == 3)
+    return B_;
 
   // Get the axis of rotation with the minimum projected length of the point
   Point3 axis;
@@ -47,9 +51,9 @@ Matrix Sphere2::getBasis() const {
   b2 = b2 / b2.norm();
 
   // Create the basis matrix
-  Matrix B(3, 2);
+  B_ = Matrix(3, 2);
-  B << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
+  B_ << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
-  return B;
+  return B_;
 }
 
 /* ************************************************************************* */
@@ -63,12 +67,39 @@ 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);
+}
+
+/* ************************************************************************* */
+double Sphere2::distance(const Sphere2& other,
+    boost::optional<Matrix&> H) const {
+  Matrix Bt = basis().transpose();
+  Vector xi = Bt * other.p_.vector();
+  double theta = xi.norm();
+  if (H)
+    *H = (xi.transpose() / theta) * Bt * other.basis();
+  return theta;
+}
+
 /* ************************************************************************* */
 Sphere2 Sphere2::retract(const Vector& v) const {
 
   // Get the vector form of the point and the basis matrix
   Vector p = Point3::Logmap(p_);
-  Matrix B = getBasis();
+  Matrix B = basis();
 
   // Compute the 3D xi_hat vector
   Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
@@ -90,7 +121,7 @@ Vector Sphere2::localCoordinates(const Sphere2& y) const {
   assert(cosAngle > 0.0 && "Can not retract from x to y.");
 
   // Get the basis matrix
-  Matrix B = getBasis();
+  Matrix B = basis();
 
   // Create the vector forms of p and q (the Point3 of y).
   Vector p = Point3::Logmap(p_);

gtsam/geometry/Sphere2.h

@@ -19,7 +19,7 @@
 
 #pragma once
 
-#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Rot3.h>
 #include <gtsam/base/DerivedValue.h>
 
 namespace gtsam {
@@ -30,6 +30,7 @@ class Sphere2 : public DerivedValue<Sphere2> {
 private:
 
   Point3 p_; ///< The location of the point on the unit sphere
+  mutable Matrix B_; ///< Cached basis
 
 public:
 
@@ -45,6 +46,13 @@ public:
   Sphere2(const Point3& p) :
       p_(p / p.norm()) {
   }
+
+  /// Construct from x,y,z
+  Sphere2(double x, double y, double z) :
+      p_(x, y, z) {
+    p_ = p_ / p_.norm();
+  }
+
   /// @}
 
   /// @name Testable
@@ -63,11 +71,23 @@ public:
   /// @{
 
   /// Returns the local coordinate frame to tangent plane
-  Matrix getBasis() const;
+  Matrix basis() const;
 
   /// Return skew-symmetric associated with 3D point on unit sphere
   Matrix skew() const;
 
+  /// 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);
+
+  /// Distance between two directions
+  double distance(const Sphere2& other,
+      boost::optional<Matrix&> H = boost::none) const;
+
   /// @}
 
   /// @name Manifold

gtsam/geometry/tests/testSphere2.cpp

@@ -19,6 +19,8 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/geometry/Sphere2.h>
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <boost/bind.hpp>
 
 using namespace gtsam;
 using namespace std;
@@ -26,6 +28,82 @@ using namespace std;
 GTSAM_CONCEPT_TESTABLE_INST(Sphere2)
 GTSAM_CONCEPT_MANIFOLD_INST(Sphere2)
 
+//*******************************************************************************
+TEST(Sphere2, rotate) {
+  Rot3 R = Rot3::yaw(0.5);
+  Sphere2 p(1, 0, 0);
+  Sphere2 expected = Sphere2(R.column(0));
+  Sphere2 actual = R * p;
+  EXPECT(assert_equal(expected, actual, 1e-8));
+  Matrix actualH, expectedH;
+  // Use numerical derivatives to calculate the expected Jacobian
+  {
+    expectedH = numericalDerivative11<Sphere2, Rot3>(
+        boost::bind(&Sphere2::Rotate, _1, p, boost::none, boost::none), R);
+    Sphere2::Rotate(R, p, actualH, boost::none);
+    EXPECT(assert_equal(expectedH, actualH, 1e-9));
+  }
+  {
+    expectedH = numericalDerivative11<Sphere2, Sphere2>(
+        boost::bind(&Sphere2::Rotate, R, _1, boost::none, boost::none), p);
+    Sphere2::Rotate(R, p, boost::none, actualH);
+    EXPECT(assert_equal(expectedH, actualH, 1e-9));
+  }
+}
+
+//*******************************************************************************
+TEST(Sphere2, distance) {
+  Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
+  r = p.retract((Vector(2) << 0.8, 0));
+  EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8);
+  EXPECT_DOUBLES_EQUAL(0.44721359549995798, p.distance(q), 1e-8);
+  EXPECT_DOUBLES_EQUAL(0.6246950475544244, p.distance(r), 1e-8);
+
+  Matrix actual, expected;
+  // Use numerical derivatives to calculate the expected Jacobian
+  {
+    expected = numericalGradient<Sphere2>(
+        boost::bind(&Sphere2::distance, &p, _1, boost::none), q);
+    p.distance(q, actual);
+    EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
+  }
+  {
+    expected = numericalGradient<Sphere2>(
+        boost::bind(&Sphere2::distance, &p, _1, boost::none), r);
+    p.distance(r, actual);
+    EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
+  }
+}
+
+//*******************************************************************************
+TEST(Sphere2, localCoordinates0) {
+  Sphere2 p;
+  Vector expected = zero(2);
+  Vector actual = p.localCoordinates(p);
+  EXPECT(assert_equal(expected, actual, 1e-8));
+}
+
+//*******************************************************************************
+TEST(Sphere2, basis) {
+  Sphere2 p;
+  Matrix expected(3, 2);
+  expected << 0, 0, 0, -1, 1, 0;
+  Matrix actual = p.basis();
+  EXPECT(assert_equal(expected, actual, 1e-8));
+}
+
+//*******************************************************************************
+TEST(Sphere2, retract) {
+  Sphere2 p;
+  Vector v(2);
+  v << 0.5, 0;
+  Sphere2 expected(Point3(1, 0, 0.5));
+  Sphere2 actual = p.retract(v);
+  EXPECT(assert_equal(expected, actual, 1e-8));
+  EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
+}
+
+//*******************************************************************************
 /// Returns a random vector
 inline static Vector randomVector(const Vector& minLimits,
     const Vector& maxLimits) {
@@ -42,7 +120,7 @@ inline static Vector randomVector(const Vector& minLimits,
   return vector;
 }
 
-/* ************************************************************************* */
+//*******************************************************************************
 // Let x and y be two Sphere2's.
 // The equality x.localCoordinates(x.retract(v)) == v should hold.
 TEST(Sphere2, localCoordinates_retract) {
@@ -72,7 +150,7 @@ TEST(Sphere2, localCoordinates_retract) {
   }
 }
 
-///* ************************************************************************* */
+//*******************************************************************************
 //TEST( Pose2, between )
 //{
 //  // <
@@ -111,7 +189,7 @@ TEST(Sphere2, localCoordinates_retract) {
 //  EXPECT(assert_equal(numericalH2,actualH2));
 //
 //}
-//
+
 /* ************************************************************************* */
 int main() {
   srand(time(NULL));

gtsam/slam/EssentialMatrixFactor.h

@@ -34,8 +34,8 @@ public:
   }
 
   /// print
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
+  virtual void print(const std::string& s = "",
-      DefaultKeyFormatter) const {
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
     Base::print(s);
     std::cout << "  EssentialMatrixFactor with measurements\n  ("
         << pA_.vector().transpose() << ")' and (" << pB_.vector().transpose()

gtsam/slam/tests/testEssentialMatrixFactor.cpp

@@ -98,7 +98,7 @@ TEST (EssentialMatrix, factor) {
             boost::none), trueE);
 
     // Verify the Jacobian is correct
-    CHECK(assert_equal(HExpected, HActual, 1e-9));
+    EXPECT(assert_equal(HExpected, HActual, 1e-9));
   }
 }