Undid overzealous merge

gtsam/geometry/Unit3.cpp

@@ -21,9 +21,6 @@
 
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/geometry/Point2.h>
-#include <gtsam/config.h> // GTSAM_USE_TBB
-
-#include <boost/random/mersenne_twister.hpp>
 #include <gtsam/config.h> // for GTSAM_USE_TBB
 
 #ifdef __clang__
@@ -45,14 +42,13 @@ namespace gtsam {
 
 /* ************************************************************************* */
 Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
-  Unit3 direction(point);
-  if (H) {
   // 3*3 Derivative of representation with respect to point is 3*3:
   Matrix3 D_p_point;
-    point.normalize(D_p_point); // TODO, this calculates norm a second time :-(
+  Point3 normalized = point.normalize(H ? &D_p_point : 0);
-    // Calculate the 2*3 Jacobian
+  Unit3 direction;
+  direction.p_ = normalized.vector();
+  if (H)
     *H << direction.basis().transpose() * D_p_point;
-  }
   return direction;
 }
 
@@ -90,19 +86,20 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
 
   // Get the unit vector and derivative wrt this.
   // NOTE(hayk): We can't call point3(), because it would recursively call basis().
-  Point3 n(p_);
+  const Point3 n(p_);
 
   // 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());
+  double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z());
-  if ((mx <= my) && (mx <= mz))
+  if ((mx <= my) && (mx <= mz)) {
     axis = Point3(1.0, 0.0, 0.0);
-  else if ((my <= mx) && (my <= mz))
+  } else if ((my <= mx) && (my <= mz)) {
     axis = Point3(0.0, 1.0, 0.0);
-  else if ((mz <= mx) && (mz <= my))
+  } else if ((mz <= mx) && (mz <= my)) {
     axis = Point3(0.0, 0.0, 1.0);
-  else
+  } else {
     assert(false);
+  }
 
   // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with
   // the chosen axis.
@@ -148,7 +145,7 @@ Point3 Unit3::point3(OptionalJacobian<3, 2> H) const {
 Vector3 Unit3::unitVector(OptionalJacobian<3, 2> H) const {
   if (H)
     *H = basis();
-  return (p_);
+  return p_;
 }
 
 /* ************************************************************************* */
@@ -171,10 +168,10 @@ Matrix3 Unit3::skew() const {
 double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,2> H_q) const {
   // Get the unit vectors of each, and the derivative.
   Matrix32 H_pn_p;
-  const Point3& pn = point3(H_p ? &H_pn_p : 0);
+  Point3 pn = point3(H_p ? &H_pn_p : 0);
 
   Matrix32 H_qn_q;
-  const Point3& qn = q.point3(H_q ? &H_qn_q : 0);
+  Point3 qn = q.point3(H_q ? &H_qn_q : 0);
 
   // Compute the dot product of the Point3s.
   Matrix13 H_dot_pn, H_dot_qn;
@@ -206,7 +203,7 @@ Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const {
 Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJacobian<2, 2> H_q) const {
   // Get the point3 of this, and the derivative.
   Matrix32 H_qn_q;
-  const Point3& qn = q.point3(H_q ? &H_qn_q : 0);
+  Point3 qn = q.point3(H_q ? &H_qn_q : 0);
 
   // 2D error here is projecting q into the tangent plane of this (p).
   Matrix62 H_B_p;

gtsam/geometry/Unit3.h

@@ -20,12 +20,17 @@
 
 #pragma once
 
-#include <gtsam/base/Manifold.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
+#include <gtsam/base/Manifold.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/dllexport.h>
 
-#include <boost/random/mersenne_twister.hpp>
 #include <boost/optional.hpp>
+#include <boost/random/mersenne_twister.hpp>
+#include <boost/serialization/nvp.hpp>
+
+#include <string>
 
 #ifdef GTSAM_USE_TBB
 #include <tbb/mutex.h>
@@ -76,6 +81,12 @@ public:
     p_.normalize();
   }
 
+  /// Construct from 2D point in plane at focal length f
+  /// Unit3(p,1) can be viewed as normalized homogeneous coordinates of 2D point
+  explicit Unit3(const Point2& p, double f = 1.0) : p_(p.x(), p.y(), f) {
+    p_.normalize();
+  }
+
   /// Named constructor from Point3 with optional Jacobian
   static Unit3 FromPoint3(const Point3& point, //
       OptionalJacobian<2, 3> H = boost::none);

gtsam/geometry/tests/testUnit3.cpp

@@ -18,19 +18,20 @@
  * @brief Tests the Unit3 class
  */
 
-#include <gtsam/geometry/Unit3.h>
-#include <gtsam/geometry/Rot3.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/serializationTestHelpers.h>
+#include <gtsam/geometry/Unit3.h>
+#include <gtsam/geometry/Rot3.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/ExpressionFactor.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/slam/expressions.h>
 #include <gtsam/slam/PriorFactor.h>
 
+
 #include <CppUnitLite/TestHarness.h>
+
 #include <boost/bind.hpp>
 #include <boost/foreach.hpp>
 #include <boost/random.hpp>
@@ -49,6 +50,7 @@ GTSAM_CONCEPT_MANIFOLD_INST(Unit3)
 Point3 point3_(const Unit3& p) {
   return p.point3();
 }
+
 TEST(Unit3, point3) {
   vector<Point3> ps;
   ps += Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), Point3(1, 1, 0)
@@ -98,6 +100,7 @@ TEST(Unit3, unrotate) {
   Unit3 expected = Unit3(1, 1, 0);
   Unit3 actual = R.unrotate(p);
   EXPECT(assert_equal(expected, actual, 1e-8));
+
   Matrix actualH, expectedH;
   // Use numerical derivatives to calculate the expected Jacobian
   {
@@ -237,19 +240,66 @@ TEST(Unit3, localCoordinates0) {
   EXPECT(assert_equal(zero(2), actual, 1e-8));
 }
 
-//*******************************************************************************
+TEST(Unit3, localCoordinates) {
-TEST(Unit3, localCoordinates1) {
+  {
+    Unit3 p, q;
+    Vector2 expected = Vector2::Zero();
+    Vector2 actual = p.localCoordinates(q);
+    EXPECT(assert_equal(zero(2), actual, 1e-8));
+    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
+  }
+  {
     Unit3 p, q(1, 6.12385e-21, 0);
-  Vector actual = p.localCoordinates(q);
+    Vector2 expected = Vector2::Zero();
-  CHECK(assert_equal(zero(2), actual, 1e-8));
+    Vector2 actual = p.localCoordinates(q);
+    EXPECT(assert_equal(zero(2), actual, 1e-8));
+    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
+  }
+  {
+    Unit3 p, q(-1, 0, 0);
+    Vector2 expected(M_PI, 0);
+    Vector2 actual = p.localCoordinates(q);
+    EXPECT(assert_equal(expected, actual, 1e-8));
+    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
+  }
+  {
+    Unit3 p, q(0, 1, 0);
+    Vector2 expected(0,-M_PI_2);
+    Vector2 actual = p.localCoordinates(q);
+    EXPECT(assert_equal(expected, actual, 1e-8));
+    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
+  }
+  {
+    Unit3 p, q(0, -1, 0);
+    Vector2 expected(0, M_PI_2);
+    Vector2 actual = p.localCoordinates(q);
+    EXPECT(assert_equal(expected, actual, 1e-8));
+    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
+  }
+  {
+    Unit3 p(0,1,0), q(0,-1,0);
+    Vector2 actual = p.localCoordinates(q);
+    EXPECT(assert_equal(q, p.retract(actual), 1e-8));
+  }
+  {
+    Unit3 p(0,0,1), q(0,0,-1);
+    Vector2 actual = p.localCoordinates(q);
+    EXPECT(assert_equal(q, p.retract(actual), 1e-8));
   }
 
-//*******************************************************************************
+  double twist = 1e-4;
-TEST(Unit3, localCoordinates2) {
+  {
-  Unit3 p, q(-1, 0, 0);
+    Unit3 p(0, 1, 0), q(0 - twist, -1 + twist, 0);
-  Vector expected = (Vector(2) << M_PI, 0).finished();
+    Vector2 actual = p.localCoordinates(q);
-  Vector actual = p.localCoordinates(q);
+    EXPECT(actual(0) < 1e-2);
-  CHECK(assert_equal(expected, actual, 1e-8));
+    EXPECT(actual(1) > M_PI - 1e-2)
+  }
+  {
+    Unit3 p(0, 1, 0), q(0 + twist, -1 - twist, 0);
+    Vector2 actual = p.localCoordinates(q);
+    EXPECT(actual(0) < 1e-2);
+    EXPECT(actual(1) < -M_PI + 1e-2)
+  }
 }
 
 //*******************************************************************************
@@ -296,98 +346,33 @@ TEST(Unit3, basis_derivatives) {
 
 //*******************************************************************************
 TEST(Unit3, retract) {
+  {
     Unit3 p;
-  Vector v(2);
+    Vector2 v(0.5, 0);
-  v << 0.5, 0;
     Unit3 expected(0.877583, 0, 0.479426);
     Unit3 actual = p.retract(v);
     EXPECT(assert_equal(expected, actual, 1e-6));
     EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
   }
+  {
+    Unit3 p;
+    Vector2 v(0, 0);
+    Unit3 actual = p.retract(v);
+    EXPECT(assert_equal(p, actual, 1e-6));
+    EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
+  }
+}
 
 //*******************************************************************************
 TEST(Unit3, retract_expmap) {
   Unit3 p;
-  Vector v(2);
+  Vector2 v((M_PI / 2.0), 0);
-  v << (M_PI / 2.0), 0;
   Unit3 expected(Point3(0, 0, 1));
   Unit3 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) {
-
-  // 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 Unit3's.
-// The equality x.localCoordinates(x.retract(v)) == v should hold.
-TEST(Unit3, localCoordinates_retract) {
-
-  size_t numIterations = 10000;
-  Vector minSphereLimit = Vector3(-1.0, -1.0, -1.0), maxSphereLimit =
-      Vector3(1.0, 1.0, 1.0);
-  Vector minXiLimit = Vector2(-1.0, -1.0), maxXiLimit = Vector2(1.0, 1.0);
-  for (size_t i = 0; i < numIterations; i++) {
-
-    // Create the two Unit3s.
-    // NOTE: You can not create two totally random Unit3's because you cannot always compute
-    // between two any Unit3's. (For instance, they might be at the different sides of the circle).
-    Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
-    Vector v12 = randomVector(minXiLimit, maxXiLimit);
-    Unit3 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));
-    Unit3 actual_s2 = s1.retract(actual_v12);
-    EXPECT(assert_equal(s2, actual_s2, 1e-3));
-  }
-}
-
-//*******************************************************************************
-// Let x and y be two Unit3's.
-// The equality x.localCoordinates(x.retract(v)) == v should hold.
-TEST(Unit3, localCoordinates_retract_expmap) {
-
-  size_t numIterations = 10000;
-  Vector minSphereLimit = Vector3(-1.0, -1.0, -1.0), maxSphereLimit =
-      Vector3(1.0, 1.0, 1.0);
-  Vector minXiLimit = (Vector(2) << -M_PI, -M_PI).finished(), maxXiLimit = (Vector(2) << M_PI, M_PI).finished();
-  for (size_t i = 0; i < numIterations; i++) {
-
-    // Create the two Unit3s.
-    // Unlike the above case, we can use any two Unit3's.
-    Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
-    Vector v12 = randomVector(minXiLimit, maxXiLimit);
-
-    // Magnitude of the rotation can be at most pi
-    if (v12.norm() > M_PI)
-      v12 = v12 / M_PI;
-    Unit3 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));
-    Unit3 actual_s2 = s1.retract(actual_v12);
-    EXPECT(assert_equal(s2, actual_s2, 1e-3));
-  }
-}
-
 //*******************************************************************************
 TEST(Unit3, Random) {
   boost::mt19937 rng(42);
@@ -399,6 +384,26 @@ TEST(Unit3, Random) {
   EXPECT(assert_equal(expectedMean,actualMean,0.1));
 }
 
+//*******************************************************************************
+// New test that uses Unit3::Random
+TEST(Unit3, localCoordinates_retract) {
+  boost::mt19937 rng(42);
+  size_t numIterations = 10000;
+
+  for (size_t i = 0; i < numIterations; i++) {
+    // Create two random Unit3s
+    const Unit3 s1 = Unit3::Random(rng);
+    const Unit3 s2 = Unit3::Random(rng);
+    // Check that they are not at opposite ends of the sphere, which is ill defined
+    if (s1.unitVector().dot(s2.unitVector())<-0.9) continue;
+
+    // Check if the local coordinates and retract return consistent results.
+    Vector v12 = s1.localCoordinates(s2);
+    Unit3 actual_s2 = s1.retract(v12);
+    EXPECT(assert_equal(s2, actual_s2, 1e-9));
+  }
+}
+
 //*************************************************************************
 TEST (Unit3, FromPoint3) {
   Matrix actualH;
@@ -450,6 +455,14 @@ TEST(Unit3, ErrorBetweenFactor) {
   }
 }
 
+/* ************************************************************************* */
+TEST(actualH, Serialization) {
+  Unit3 p(0, 1, 0);
+  EXPECT(serializationTestHelpers::equalsObj(p));
+  EXPECT(serializationTestHelpers::equalsXML(p));
+  EXPECT(serializationTestHelpers::equalsBinary(p));
+}
+
 /* ************************************************************************* */
 int main() {
   srand(time(NULL));