From 02a6a8caad29d4346c57e9d9a76ac675f7b9dafe Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Sat, 27 Aug 2011 00:58:43 +0000
Subject: [PATCH] Added derivatives to Point3::between and added several
 missing unit tests

---
 gtsam/geometry/Point3.h             | 10 ++++++++--
 gtsam/geometry/tests/testPoint3.cpp | 18 ++++++++++++++++++
 2 files changed, 26 insertions(+), 2 deletions(-)

diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index cccd59c5d..59a23bdd3 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -67,7 +67,7 @@ namespace gtsam {
     		boost::optional<Matrix&> H2=boost::none) const {
   	  if (H1) *H1 = eye(3);
   	  if (H2) *H2 = eye(3);
-  	  return *this+p2;
+  	  return *this + p2;
     }
 
     /** Exponential map at identity - just create a Point3 from x,y,z */
@@ -81,7 +81,13 @@ namespace gtsam {
     inline Vector logmap(const Point3& p2) const { return gtsam::logmap_default(*this, p2);}
 
     /** Between using the default implementation */
-    inline Point3 between(const Point3& p2) const { return between_default(*this, p2); }
+    inline Point3 between(const Point3& p2,
+        boost::optional<Matrix&> H1=boost::none,
+        boost::optional<Matrix&> H2=boost::none) const {
+      if(H1) *H1 = -eye(3);
+      if(H2) *H2 = eye(3);
+      return p2 - *this;
+    }
 
     /** return vectorized form (column-wise)*/
     Vector vector() const {
diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp
index e59b4e93a..67d35986d 100644
--- a/gtsam/geometry/tests/testPoint3.cpp
+++ b/gtsam/geometry/tests/testPoint3.cpp
@@ -21,6 +21,24 @@ using namespace gtsam;
 
 Point3 P(0.2,0.7,-2);
 
+/* ************************************************************************* */
+TEST(Point3, Lie) {
+  Point3 p1(1,2,3);
+  Point3 p2(4,5,6);
+  Matrix H1, H2;
+
+  EXPECT(assert_equal(Point3(5,7,9), p1.compose(p2, H1, H2)));
+  EXPECT(assert_equal(eye(3), H1));
+  EXPECT(assert_equal(eye(3), H2));
+
+  EXPECT(assert_equal(Point3(3,3,3), p1.between(p2, H1, H2)));
+  EXPECT(assert_equal(-eye(3), H1));
+  EXPECT(assert_equal(eye(3), H2));
+
+  EXPECT(assert_equal(Point3(5,7,9), p1.expmap(Vector_(3, 4.,5.,6.))));
+  EXPECT(assert_equal(Vector_(3, 3.,3.,3.), p1.logmap(p2)));
+}
+
 /* ************************************************************************* */
 TEST( Point3, arithmetic)
 {