From e5d7e3ce8aa8e5fb54947f8d0e05e3f835faf746 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 19 Dec 2013 04:05:13 +0000
Subject: [PATCH] point3

---
 gtsam/geometry/Sphere2.h             |  7 +++++++
 gtsam/geometry/tests/testSphere2.cpp | 23 +++++++++++++++++++++--
 2 files changed, 28 insertions(+), 2 deletions(-)

diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h
index 4a06f052c..29decde1c 100644
--- a/gtsam/geometry/Sphere2.h
+++ b/gtsam/geometry/Sphere2.h
@@ -76,6 +76,13 @@ public:
   /// Return skew-symmetric associated with 3D point on unit sphere
   Matrix skew() const;
 
+  /// Return unit-norm Point3
+  Point3 point3(boost::optional<Matrix&> H = boost::none) const {
+    if (H)
+      *H = basis();
+    return p_;
+  }
+
   /// Rotate
   static Sphere2 Rotate(const Rot3& R, const Sphere2& p,
       boost::optional<Matrix&> HR = boost::none, boost::optional<Matrix&> Hp =
diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp
index 5802d48a2..b31b624cb 100644
--- a/gtsam/geometry/tests/testSphere2.cpp
+++ b/gtsam/geometry/tests/testSphere2.cpp
@@ -16,18 +16,37 @@
  * @brief Tests the Sphere2 class
  */
 
-#include <gtsam/base/Testable.h>
 #include <gtsam/geometry/Sphere2.h>
-#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <CppUnitLite/TestHarness.h>
 #include <boost/bind.hpp>
+#include <boost/foreach.hpp>
+#include <boost/assign/std/vector.hpp>
 
+using namespace boost::assign;
 using namespace gtsam;
 using namespace std;
 
 GTSAM_CONCEPT_TESTABLE_INST(Sphere2)
 GTSAM_CONCEPT_MANIFOLD_INST(Sphere2)
 
+//*******************************************************************************
+Point3 point3_(const Sphere2& p) {
+  return p.point3();
+}
+TEST(Sphere2, point3) {
+  vector < Point3 > ps;
+  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);
+    expectedH = numericalDerivative11<Point3, Sphere2>(point3_, s);
+    EXPECT(assert_equal(p, s.point3(actualH), 1e-8));
+    EXPECT(assert_equal(expectedH, actualH, 1e-9));
+  }
+}
+
 //*******************************************************************************
 TEST(Sphere2, rotate) {
   Rot3 R = Rot3::yaw(0.5);