Revived Sphere2, the S^2 manifold that can be used for directions in 3D space

.cproject

@@ -904,6 +904,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testEssentialMatrix.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testEssentialMatrix.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1540,6 +1548,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testParticleFactor.run" path="build/gtsam_unstable/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testParticleFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1892,6 +1908,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testSphere2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testSphere2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -2132,6 +2156,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testSampler.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testSampler.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>

gtsam/geometry/Sphere2.cpp

@@ -0,0 +1,110 @@
+/* ----------------------------------------------------------------------------
+
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/*
+ * @file Sphere2.h
+ * @date Feb 02, 2011
+ * @author Can Erdogan
+ * @brief Develop a Sphere2 class - basically a point on a unit sphere
+ */
+
+#include <gtsam/geometry/Sphere2.h>
+#include <gtsam/geometry/Point2.h>
+
+using namespace std;
+
+namespace gtsam {
+
+Sphere2::~Sphere2() {
+}
+
+Sphere2 Sphere2::retract(const Vector& v) const {
+
+  // Get the vector form of the point and the basis matrix
+  Vector p = Point3::Logmap(p_);
+  Vector axis;
+  Matrix B = getBasis(&axis);
+
+  // Compute the 3D ξ^ vector
+  Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
+  Vector newPoint = p + xi_hat;
+
+  // Project onto the manifold, i.e. the closest point on the circle to the new location; same as
+  // putting it onto the unit circle
+  Vector projected = newPoint / newPoint.norm();
+
+#ifdef DEBUG_SPHERE2_RETRACT
+  cout << "retract output for Matlab visualization (copy/paste =/): \n";
+  cout << "p = [" << p.transpose() << "];\n";
+  cout << "b1 = [" << B.col(0).transpose() << "];\n";
+  cout << "b2 = [" << B.col(1).transpose() << "];\n";
+  cout << "axis = [" << axis.transpose() << "];\n";
+  cout << "xi_hat = [" << xi_hat.transpose() << "];\n";
+  cout << "newPoint = [" << newPoint.transpose() << "];\n";
+  cout << "projected = [" << projected.transpose() << "];\n";
+#endif
+
+  Sphere2 result(Point3::Expmap(projected));
+  return result;
+}
+
+Vector Sphere2::localCoordinates(const Sphere2& y) const {
+
+  // Make sure that the angle different between x and y is less than 90. Otherwise,
+  // we can project x + ξ^ from the tangent space at x to y.
+  double cosAngle = y.p_.dot(p_);
+  assert(cosAngle > 0.0 && "Can not retract from x to y in the first place.");
+
+  // Get the basis matrix
+  Matrix B = getBasis();
+
+  // Create the vector forms of p and q (the Point3 of y).
+  Vector p = Point3::Logmap(p_);
+  Vector q = Point3::Logmap(y.p_);
+
+  // Compute the basis coefficients [ξ1,ξ2] = (B'q)/(p'q).
+  double alpha = p.transpose() * q;
+  assert(alpha != 0.0);
+  Matrix coeffs = (B.transpose() * q) / alpha;
+  Vector result = Vector_(2, coeffs(0, 0), coeffs(1, 0));
+  return result;
+}
+
+Matrix Sphere2::getBasis(Vector* axisOutput) const {
+
+  // Get the axis of rotation with the minimum projected length of the point
+  Point3 axis;
+  double mx = fabs(p_.x()), my = fabs(p_.y()), mz = fabs(p_.z());
+  if ((mx <= my) && (mx <= mz))
+    axis = Point3(1.0, 0.0, 0.0);
+  else if ((my <= mx) && (my <= mz))
+    axis = Point3(0.0, 1.0, 0.0);
+  else if ((mz <= mx) && (mz <= my))
+    axis = Point3(0.0, 0.0, 1.0);
+  else
+    assert(false);
+
+  // Create the two basis vectors
+  Point3 b1 = p_.cross(axis);
+  b1 = b1 / b1.norm();
+  Point3 b2 = p_.cross(b1);
+  b2 = b2 / b2.norm();
+
+  // Create the basis matrix
+  Matrix basis = Matrix_(3, 2, b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z());
+
+  // Return the axis if requested
+  if (axisOutput != NULL)
+    *axisOutput = Point3::Logmap(axis);
+  return basis;
+}
+
+}

gtsam/geometry/Sphere2.h

@@ -0,0 +1,90 @@
+/* ----------------------------------------------------------------------------
+
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/*
+ * @file Sphere2.h
+ * @date Feb 02, 2011
+ * @author Can Erdogan
+ * @brief Develop a Sphere2 class - basically a point on a unit sphere
+ */
+
+#pragma once
+
+#include <gtsam/geometry/Point3.h>
+
+namespace gtsam {
+
+/// Represents a 3D point on a unit sphere. The Sphere2 with the 3D ξ^ variable and two
+/// coefficients ξ_1 and ξ_2 that scale the 3D basis vectors of the tangent space.
+struct Sphere2 {
+
+  gtsam::Point3 p_; ///< The location of the point on the unit sphere
+
+  /// The constructors
+  Sphere2() :
+      p_(gtsam::Point3(1.0, 0.0, 0.0)) {
+  }
+
+  /// Copy constructor
+  Sphere2(const Sphere2& s) {
+    p_ = s.p_ / s.p_.norm();
+  }
+
+  /// Destructor
+  ~Sphere2();
+
+  /// Field constructor
+  Sphere2(const gtsam::Point3& p) {
+    p_ = p / p.norm();
+  }
+
+  /// @name Testable
+  /// @{
+
+  /// The print fuction
+  void print(const std::string& s = std::string()) const {
+    printf("%s(x, y, z): (%.3lf, %.3lf, %.3lf)\n", s.c_str(), p_.x(), p_.y(),
+        p_.z());
+  }
+
+  /// The equals function with tolerance
+  bool equals(const Sphere2& s, double tol = 1e-9) const {
+    return p_.equals(s.p_, tol);
+  }
+  /// @}
+
+  /// @name Manifold
+  /// @{
+
+  /// Dimensionality of tangent space = 2 DOF
+  inline static size_t Dim() {
+    return 2;
+  }
+
+  /// Dimensionality of tangent space = 2 DOF
+  inline size_t dim() const {
+    return 2;
+  }
+
+  /// The retract function
+  Sphere2 retract(const gtsam::Vector& v) const;
+
+  /// The local coordinates function
+  gtsam::Vector localCoordinates(const Sphere2& s) const;
+
+  /// @}
+
+  /// Returns the axis of rotations
+  gtsam::Matrix getBasis(gtsam::Vector* axisOutput = NULL) const;
+};
+
+} // namespace gtsam
+

gtsam/geometry/tests/testSphere2.cpp

@@ -0,0 +1,81 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/*
+ * @file testSphere2.cpp
+ * @date Feb 03, 2012
+ * @author Can Erdogan
+ * @brief Tests the Sphere2 class
+ */
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/geometry/Sphere2.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace gtsam;
+using namespace std;
+
+GTSAM_CONCEPT_TESTABLE_INST(Sphere2)
+GTSAM_CONCEPT_MANIFOLD_INST(Sphere2)
+
+/// Returns a random vector
+inline static Vector randomVector(const Vector& minLimits,
+    const Vector& maxLimits) {
+
+  // Get the number of dimensions and create the return vector
+  size_t numDims = dim(minLimits);
+  Vector vector = zero(numDims);
+
+  // Create the random vector
+  for (size_t i = 0; i < numDims; i++) {
+    double range = maxLimits(i) - minLimits(i);
+    vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i);
+  }
+  return vector;
+}
+
+/* ************************************************************************* */
+// Let x and y be two Sphere2's.
+// The equality x.localCoordinates(x.retract(v)) == v should hold.
+TEST(Sphere2, localCoordinates_retract) {
+
+  size_t numIterations = 10000;
+  Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit =
+      Vector_(3, 1.0, 1.0, 1.0);
+  Vector minXiLimit = Vector_(2, -1.0, -1.0), maxXiLimit = Vector_(2, 1.0, 1.0);
+  for (size_t i = 0; i < numIterations; i++) {
+
+    // Sleep for the random number generator (TODO?: Better create all of them first).
+    sleep(0);
+
+    // Create the two Sphere2s.
+    // NOTE: You can not create two totally random Sphere2's because you cannot always compute
+    // between two any Sphere2's. (For instance, they might be at the different sides of the circle).
+    Sphere2 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
+//		Sphere2 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit)));
+    Vector v12 = randomVector(minXiLimit, maxXiLimit);
+    Sphere2 s2 = s1.retract(v12);
+
+    // Check if the local coordinates and retract return the same results.
+    Vector actual_v12 = s1.localCoordinates(s2);
+    EXPECT(assert_equal(v12, actual_v12, 1e-3));
+    Sphere2 actual_s2 = s1.retract(actual_v12);
+    EXPECT(assert_equal(s2, actual_s2, 1e-3));
+  }
+}
+
+/* ************************************************************************* */
+int main() {
+  srand(time(NULL));
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */