error is a signed, vector-valued error signal

gtsam/geometry/Sphere2.cpp

@@ -80,17 +80,24 @@ Sphere2 Sphere2::Rotate(const Rot3& R, const Sphere2& p,
 
 /* ************************************************************************* */
 Sphere2 operator*(const Rot3& R, const Sphere2& p) {
-  return Sphere2::Rotate(R,p);
+  return Sphere2::Rotate(R, p);
 }
 
 /* ************************************************************************* */
-double Sphere2::distance(const Sphere2& other,
+Vector Sphere2::error(const Sphere2& q, boost::optional<Matrix&> H) const {
-    boost::optional<Matrix&> H) const {
   Matrix Bt = basis().transpose();
-  Vector xi = Bt * other.p_.vector();
+  Vector xi = Bt * q.p_.vector();
+  if (H)
+    *H = Bt * q.basis();
+  return xi;
+}
+
+/* ************************************************************************* */
+double Sphere2::distance(const Sphere2& q, boost::optional<Matrix&> H) const {
+  Vector xi = error(q,H);
   double theta = xi.norm();
   if (H)
-    *H = (xi.transpose() / theta) * Bt * other.basis();
+    *H = (xi.transpose() / theta) * (*H);
   return theta;
 }
 

gtsam/geometry/Sphere2.h

@@ -84,8 +84,12 @@ public:
   /// 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;
+
   /// Distance between two directions
-  double distance(const Sphere2& other,
+  double distance(const Sphere2& q,
       boost::optional<Matrix&> H = boost::none) const;
 
   /// @}

gtsam/geometry/tests/testSphere2.cpp

@@ -51,6 +51,30 @@ TEST(Sphere2, rotate) {
   }
 }
 
+//*******************************************************************************
+TEST(Sphere2, error) {
+  Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
+  r = p.retract((Vector(2) << 0.8, 0));
+  EXPECT(assert_equal((Vector(2) << 0, 0), p.error(p), 1e-8));
+  EXPECT(assert_equal((Vector(2) << 0.447214, 0), p.error(q), 1e-5));
+  EXPECT(assert_equal((Vector(2) << 0.624695, 0), p.error(r), 1e-5));
+
+  Matrix actual, expected;
+  // Use numerical derivatives to calculate the expected Jacobian
+  {
+    expected = numericalDerivative11<Sphere2>(
+        boost::bind(&Sphere2::error, &p, _1, boost::none), q);
+    p.error(q, actual);
+    EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
+  }
+  {
+    expected = numericalDerivative11<Sphere2>(
+        boost::bind(&Sphere2::error, &p, _1, boost::none), r);
+    p.error(r, actual);
+    EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
+  }
+}
+
 //*******************************************************************************
 TEST(Sphere2, distance) {
   Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //