Some new methods and improvements to Unit3 from Skydio

gtsam/geometry/Unit3.cpp

@@ -15,12 +15,14 @@
  * @author Can Erdogan
  * @author Frank Dellaert
  * @author Alex Trevor
- * @author Zhaoyang Lv
  * @brief The Unit3 class - basically a point on a unit sphere
  */
 
 #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__
@@ -32,13 +34,8 @@
 #  pragma clang diagnostic pop
 #endif
 
-#ifdef GTSAM_USE_TBB
-#include <tbb/mutex.h>
-#endif
-
 #include <boost/random/variate_generator.hpp>
 #include <iostream>
-#include <limits>
 
 using namespace std;
 
@@ -46,14 +43,12 @@ namespace gtsam {
 
 /* ************************************************************************* */
 Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
-  Unit3 direction(point);
+  // 3*3 Derivative of representation with respect to point is 3*3:
-  if (H) {
+  Matrix3 D_p_point;
-    // 3*3 Derivative of representation with respect to point is 3*3:
+  Unit3 direction;
-    Matrix3 D_p_point;
+  direction.p_ = point.normalize(H ? &D_p_point : 0);
-    point.normalize(D_p_point); // TODO, this calculates norm a second time :-(
+  if (H)
-    // Calculate the 2*3 Jacobian
     *H << direction.basis().transpose() * D_p_point;
-  }
   return direction;
 }
 
@@ -63,49 +58,105 @@ Unit3 Unit3::Random(boost::mt19937 & rng) {
   boost::uniform_on_sphere<double> randomDirection(3);
   // This variate_generator object is required for versions of boost somewhere
   // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng).
-  boost::variate_generator<boost::mt19937&, boost::uniform_on_sphere<double> > generator(
+  boost::variate_generator<boost::mt19937&, boost::uniform_on_sphere<double> >
-      rng, randomDirection);
+      generator(rng, randomDirection);
   vector<double> d = generator();
-  return Unit3(d[0], d[1], d[2]);
+  Unit3 result;
+  result.p_ = Point3(d[0], d[1], d[2]);
+  return result;
 }
 
-#ifdef GTSAM_USE_TBB
-tbb::mutex unit3BasisMutex;
-#endif
-
 /* ************************************************************************* */
-const Matrix32& Unit3::basis() const {
+const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
 #ifdef GTSAM_USE_TBB
-  tbb::mutex::scoped_lock lock(unit3BasisMutex);
+  // NOTE(hayk): At some point it seemed like this reproducably resulted in deadlock. However, I
+  // can't see the reason why and I can no longer reproduce it. It may have been a red herring, or
+  // there is still a latent bug to watch out for.
+  tbb::mutex::scoped_lock lock(B_mutex_);
 #endif
 
-  // Return cached version if exists
+  // Return cached basis if available and the Jacobian isn't needed.
-  if (B_) return *B_;
+  if (B_ && !H) {
+    return *B_;
+  }
+
+  // Return cached basis and derivatives if available.
+  if (B_ && H && H_B_) {
+    *H = *H_B_;
+    return *B_;
+  }
+
+  // Get the unit vector and derivative wrt this.
+  // NOTE(hayk): We can't call point3(), because it would recursively call basis().
+  const Point3& n = p_;
 
   // Get the axis of rotation with the minimum projected length of the point
-  Vector3 axis;
+  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 = Vector3(1.0, 0.0, 0.0);
+    axis = Point3(1.0, 0.0, 0.0);
-  else if ((my <= mx) && (my <= mz))
+  } else if ((my <= mx) && (my <= mz)) {
-    axis = Vector3(0.0, 1.0, 0.0);
+    axis = Point3(0.0, 1.0, 0.0);
-  else if ((mz <= mx) && (mz <= my))
+  } else if ((mz <= mx) && (mz <= my)) {
-    axis = Vector3(0.0, 0.0, 1.0);
+    axis = Point3(0.0, 0.0, 1.0);
-  else
+  } else {
     assert(false);
+  }
 
-  // Create the two basis vectors
+  // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with
-  Vector3 b1 = p_.cross(axis).normalized();
+  // the chosen axis.
-  Vector3 b2 = p_.cross(b1).normalized();
+  Matrix33 H_B1_n;
+  Point3 B1 = n.cross(axis, H ? &H_B1_n : nullptr);
 
-  // Create the basis matrix
+  // Normalize result to get a unit vector: b1 = B1 / |B1|.
+  Matrix33 H_b1_B1;
+  Point3 b1 = B1.normalize(H ? &H_b1_B1 : nullptr);
+
+  // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them.
+  // No need to normalize this, p and b1 are orthogonal unit vectors.
+  Matrix33 H_b2_n, H_b2_b1;
+  Point3 b2 = n.cross(b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr);
+
+  // Create the basis by stacking b1 and b2.
   B_.reset(Matrix32());
-  (*B_) << b1, b2;
+  (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
+
+  if (H) {
+    // Chain rule tomfoolery to compute the derivative.
+    const Matrix32& H_n_p = *B_;
+    Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p;
+    Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p;
+
+    // Cache the derivative and fill the result.
+    H_B_.reset(Matrix62());
+    (*H_B_) << H_b1_p, H_b2_p;
+    *H = *H_B_;
+  }
+
   return *B_;
 }
 
 /* ************************************************************************* */
-/// The print fuction
+const Point3& Unit3::point3(OptionalJacobian<3, 2> H) const {
+  if (H)
+    *H = basis();
+  return p_;
+}
+
+/* ************************************************************************* */
+Vector3 Unit3::unitVector(boost::optional<Matrix&> H) const {
+  if (H)
+    *H = basis();
+  return (p_.vector());
+}
+
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Unit3& pair) {
+  os << pair.p_ << endl;
+  return os;
+}
+
+/* ************************************************************************* */
 void Unit3::print(const std::string& s) const {
   cout << s << ":" << p_ << endl;
 }
@@ -116,11 +167,72 @@ Matrix3 Unit3::skew() const {
 }
 
 /* ************************************************************************* */
-Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) 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);
+
+  Matrix32 H_qn_q;
+  const Point3& qn = q.point3(H_q ? &H_qn_q : 0);
+
+  // Compute the dot product of the Point3s.
+  Matrix13 H_dot_pn, H_dot_qn;
+  double d = pn.dot(qn, H_p ? &H_dot_pn : nullptr, H_q ? &H_dot_qn : nullptr);
+
+  if (H_p) {
+    (*H_p) << H_dot_pn * H_pn_p;
+  }
+
+  if (H_q) {
+    (*H_q) = H_dot_qn * H_qn_q;
+  }
+
+  return d;
+}
+
+/* ************************************************************************* */
+Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const {
   // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
-  Vector2 xi = basis().transpose() * q.p_;
+  Matrix23 Bt = basis().transpose();
-  if (H)
+  Vector2 xi = Bt * q.p_.vector();
-    *H = basis().transpose() * q.basis();
+  if (H_q) {
+    *H_q = Bt * q.basis();
+  }
+  return xi;
+}
+
+/* ************************************************************************* */
+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);
+
+  // 2D error here is projecting q into the tangent plane of this (p).
+  Matrix62 H_B_p;
+  Matrix23 Bt = basis(H_p ? &H_B_p : nullptr).transpose();
+  Vector2 xi = Bt * qn.vector();
+
+  if (H_p) {
+    // Derivatives of each basis vector.
+    const Matrix32& H_b1_p = H_B_p.block<3, 2>(0, 0);
+    const Matrix32& H_b2_p = H_B_p.block<3, 2>(3, 0);
+
+    // Derivatives of the two entries of xi wrt the basis vectors.
+    Matrix13 H_xi1_b1 = qn.vector().transpose();
+    Matrix13 H_xi2_b2 = qn.vector().transpose();
+
+    // Assemble dxi/dp = dxi/dB * dB/dp.
+    Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p;
+    Matrix12 H_xi2_p = H_xi2_b2 * H_b2_p;
+    *H_p << H_xi1_p, H_xi2_p;
+  }
+
+  if (H_q) {
+    // dxi/dq is given by dxi/dqu * dqu/dq, where qu is the unit vector of q.
+    Matrix23 H_xi_qu = Bt;
+    *H_q = H_xi_qu * H_qn_q;
+  }
+
   return xi;
 }
 
@@ -136,39 +248,46 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const {
 
 /* ************************************************************************* */
 Unit3 Unit3::retract(const Vector2& v) const {
-  // Compute the 3D xi_hat vector
-  Vector3 xi_hat = basis() * v;
-  double theta = xi_hat.norm();
 
-  // Treat case of very small v differently
+  // Get the vector form of the point and the basis matrix
-  if (theta < std::numeric_limits<double>::epsilon()) {
+  Vector3 p = p_.vector();
-    return Unit3(cos(theta) * p_ + xi_hat);
+  Matrix32 B = basis();
+
+  // Compute the 3D xi_hat vector
+  Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
+
+  double xi_hat_norm = xi_hat.norm();
+
+  // Avoid nan
+  if (xi_hat_norm == 0.0) {
+    if (v.norm() == 0.0)
+      return Unit3(point3());
+    else
+      return Unit3(-point3());
   }
 
-  Vector3 exp_p_xi_hat =
+  Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p
-      cos(theta) * p_ + xi_hat * (sin(theta) / theta);
+      + sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
   return Unit3(exp_p_xi_hat);
 }
 
 /* ************************************************************************* */
-Vector2 Unit3::localCoordinates(const Unit3& other) const {
+Vector2 Unit3::localCoordinates(const Unit3& y) const {
-  const double x = p_.dot(other.p_);
+
-  // Crucial quantity here is y = theta/sin(theta) with theta=acos(x)
+  Vector3 p = p_.vector(), q = y.p_.vector();
-  // Now, y = acos(x) / sin(acos(x)) = acos(x)/sqrt(1-x^2)
+  double dot = p.dot(q);
-  // We treat the special case 1 and -1 below
+
-  const double x2 = x * x;
+  // Check for special cases
-  const double z = 1 - x2;
+  if (dot > 1.0 - 1e-16)
-  double y;
+    return Vector2(0, 0);
-  if (z < std::numeric_limits<double>::epsilon()) {
+  else if (dot < -1.0 + 1e-16)
-    if (x > 0)  // first order expansion at x=1
+    return Vector2(M_PI, 0);
-      y = 1.0 - (x - 1.0) / 3.0;
+  else {
-    else  // cop out
-      return Vector2(M_PI, 0.0);
-  } else {
     // no special case
-    y = acos(x) / sqrt(z);
+    double theta = acos(dot);
+    Vector3 result_hat = (theta / sin(theta)) * (q - p * dot);
+    return basis().transpose() * result_hat;
   }
-  return basis().transpose() * y * (other.p_ - x * p_);
 }
 /* ************************************************************************* */
 

gtsam/geometry/Unit3.h

@@ -20,16 +20,16 @@
 
 #pragma once
 
-#include <gtsam/geometry/Point3.h>
 #include <gtsam/base/Manifold.h>
-#include <gtsam/base/Matrix.h>
+#include <gtsam/geometry/Point2.h>
-#include <gtsam/dllexport.h>
+#include <gtsam/geometry/Point3.h>
 
-#include <boost/optional.hpp>
 #include <boost/random/mersenne_twister.hpp>
-#include <boost/serialization/nvp.hpp>
+#include <boost/optional.hpp>
 
-#include <string>
+#ifdef GTSAM_USE_TBB
+#include <tbb/mutex.h>
+#endif
 
 namespace gtsam {
 
@@ -38,8 +38,13 @@ class GTSAM_EXPORT Unit3 {
 
 private:
 
-  Vector3 p_; ///< The location of the point on the unit sphere
+  Point3 p_; ///< The location of the point on the unit sphere
   mutable boost::optional<Matrix32> B_; ///< Cached basis
+  mutable boost::optional<Matrix62> H_B_; ///< Cached basis derivative
+
+#ifdef GTSAM_USE_TBB
+  mutable tbb::mutex B_mutex_; ///< Mutex to protect the cached basis.
+#endif
 
 public:
 
@@ -57,18 +62,23 @@ public:
 
   /// Construct from point
   explicit Unit3(const Point3& p) :
-      p_(p.vector().normalized()) {
+      p_(p.normalize()) {
   }
 
   /// Construct from a vector3
-  explicit Unit3(const Vector3& p) :
+  explicit Unit3(const Vector3& v) :
-      p_(p.normalized()) {
+      p_(v / v.norm()) {
   }
 
   /// Construct from x,y,z
   Unit3(double x, double y, double z) :
-      p_(x, y, z) {
+      p_(Point3(x, y, z).normalize()) {
-    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_(Point3(p.x(), p.y(), f).normalize()) {
   }
 
   /// Named constructor from Point3 with optional Jacobian
@@ -83,12 +93,14 @@ public:
   /// @name Testable
   /// @{
 
+  friend std::ostream& operator<<(std::ostream& os, const Unit3& pair);
+
   /// The print fuction
   void print(const std::string& s = std::string()) const;
 
   /// The equals function with tolerance
   bool equals(const Unit3& s, double tol = 1e-9) const {
-    return equal_with_abs_tol(p_, s.p_, tol);
+    return p_.equals(s.p_, tol);
   }
   /// @}
 
@@ -99,37 +111,50 @@ public:
    * Returns the local coordinate frame to tangent plane
    * It is a 3*2 matrix [b1 b2] composed of two orthogonal directions
    * tangent to the sphere at the current direction.
+   * Provides derivatives of the basis with the two basis vectors stacked up as a 6x1.
    */
-  const Matrix32& basis() const;
+  const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const;
 
   /// Return skew-symmetric associated with 3D point on unit sphere
   Matrix3 skew() const;
 
   /// Return unit-norm Point3
-  Point3 point3(OptionalJacobian<3, 2> H = boost::none) const {
+  const Point3& point3(OptionalJacobian<3, 2> H = boost::none) const;
-    if (H)
-      *H = basis();
-    return Point3(p_);
-  }
 
   /// Return unit-norm Vector
-  const Vector3& unitVector(boost::optional<Matrix&> H = boost::none) const {
+  Vector3 unitVector(boost::optional<Matrix&> H = boost::none) const;
-    if (H)
-      *H = basis();
-    return p_;
-  }
 
   /// Return scaled direction as Point3
   friend Point3 operator*(double s, const Unit3& d) {
-    return Point3(s * d.p_);
+    return s * d.p_;
   }
 
+  /// Return dot product with q
+  double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, //
+                             OptionalJacobian<1,2> H2 = boost::none) const;
+
   /// Signed, vector-valued error between two directions
-  Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const;
+  /// @deprecated, errorVector has the proper derivatives, this confusingly has only the second.
+  Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const;
+
+  /// Signed, vector-valued error between two directions
+  /// NOTE(hayk): This method has zero derivatives if this (p) and q are orthogonal.
+  Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, //
+                      OptionalJacobian<2, 2> H_q = boost::none) const;
 
   /// Distance between two directions
   double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const;
 
+  /// Cross-product between two Unit3s
+  Unit3 cross(const Unit3& q) const {
+    return Unit3(p_.cross(q.p_));
+  }
+
+  /// Cross-product w Point3
+  Point3 cross(const Point3& q) const {
+    return Point3(p_.vector().cross(q.vector()));
+  }
+
   /// @}
 
   /// @name Manifold
@@ -147,7 +172,7 @@ public:
 
   enum CoordinatesMode {
     EXPMAP, ///< Use the exponential map to retract
-    RENORM ///< Retract with vector addition and renormalize.
+    RENORM ///< Retract with vector addtion and renormalize.
   };
 
   /// The retract function
@@ -167,6 +192,13 @@ private:
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     ar & BOOST_SERIALIZATION_NVP(p_);
+    // homebrew serialize Eigen Matrix
+    ar & boost::serialization::make_nvp("B11", (*B_)(0, 0));
+    ar & boost::serialization::make_nvp("B12", (*B_)(0, 1));
+    ar & boost::serialization::make_nvp("B21", (*B_)(1, 0));
+    ar & boost::serialization::make_nvp("B22", (*B_)(1, 1));
+    ar & boost::serialization::make_nvp("B31", (*B_)(2, 0));
+    ar & boost::serialization::make_nvp("B32", (*B_)(2, 1));
   }
 
   /// @}

gtsam/geometry/tests/testUnit3.cpp

@@ -22,19 +22,26 @@
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
-#include <gtsam/base/serializationTestHelpers.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>
+//#include <boost/thread.hpp>
 #include <boost/assign/std/vector.hpp>
 #include <cmath>
 
 using namespace boost::assign;
 using namespace gtsam;
 using namespace std;
+using gtsam::symbol_shorthand::U;
 
 GTSAM_CONCEPT_TESTABLE_INST(Unit3)
 GTSAM_CONCEPT_MANIFOLD_INST(Unit3)
@@ -43,7 +50,6 @@ 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)
@@ -69,7 +75,7 @@ TEST(Unit3, rotate) {
   Unit3 actual = R * p;
   EXPECT(assert_equal(expected, actual, 1e-8));
   Matrix actualH, expectedH;
-
+  // Use numerical derivatives to calculate the expected Jacobian
   {
     expectedH = numericalDerivative21(rotate_, R, p);
     R.rotate(p, actualH, boost::none);
@@ -93,8 +99,8 @@ 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
   {
     expectedH = numericalDerivative21(unrotate_, R, p);
     R.unrotate(p, actualH, boost::none);
@@ -107,6 +113,37 @@ TEST(Unit3, unrotate) {
   }
 }
 
+TEST(Unit3, dot) {
+  Unit3 p(1, 0.2, 0.3);
+  Unit3 q = p.retract(Vector2(0.5, 0));
+  Unit3 r = p.retract(Vector2(0.8, 0));
+  Unit3 t = p.retract(Vector2(0, 0.3));
+  EXPECT(assert_equal(1.0, p.dot(p), 1e-8));
+  EXPECT(assert_equal(0.877583, p.dot(q), 1e-5));
+  EXPECT(assert_equal(0.696707, p.dot(r), 1e-5));
+  EXPECT(assert_equal(0.955336, p.dot(t), 1e-5));
+
+  // Use numerical derivatives to calculate the expected Jacobians
+  Matrix H1, H2;
+  boost::function<double(const Unit3&, const Unit3&)> f = boost::bind(&Unit3::dot, _1, _2,  //
+                                                                      boost::none, boost::none);
+  {
+    p.dot(q, H1, H2);
+    EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, q), H1, 1e-9));
+    EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, q), H2, 1e-9));
+  }
+  {
+    p.dot(r, H1, H2);
+    EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, r), H1, 1e-9));
+    EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, r), H2, 1e-9));
+  }
+  {
+    p.dot(t, H1, H2);
+    EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, t), H1, 1e-9));
+    EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, t), H2, 1e-9));
+  }
+}
+
 //*******************************************************************************
 TEST(Unit3, error) {
   Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), //
@@ -116,6 +153,7 @@ TEST(Unit3, error) {
   EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5));
 
   Matrix actual, expected;
+  // Use numerical derivatives to calculate the expected Jacobian
   {
     expected = numericalDerivative11<Vector2,Unit3>(
         boost::bind(&Unit3::error, &p, _1, boost::none), q);
@@ -130,6 +168,45 @@ TEST(Unit3, error) {
   }
 }
 
+//*******************************************************************************
+TEST(Unit3, error2) {
+  Unit3 p(0.1, -0.2, 0.8);
+  Unit3 q = p.retract(Vector2(0.2, -0.1));
+  Unit3 r = p.retract(Vector2(0.8, 0));
+
+  // Hard-coded as simple regression values
+  EXPECT(assert_equal((Vector)(Vector2(0.0, 0.0)), p.errorVector(p), 1e-8));
+  EXPECT(assert_equal((Vector)(Vector2(0.198337495, -0.0991687475)), p.errorVector(q), 1e-5));
+  EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.errorVector(r), 1e-5));
+
+  Matrix actual, expected;
+  // Use numerical derivatives to calculate the expected Jacobian
+  {
+    expected = numericalDerivative21<Vector2, Unit3, Unit3>(
+        boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q);
+    p.errorVector(q, actual, boost::none);
+    EXPECT(assert_equal(expected, actual, 1e-9));
+  }
+  {
+    expected = numericalDerivative21<Vector2, Unit3, Unit3>(
+        boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r);
+    p.errorVector(r, actual, boost::none);
+    EXPECT(assert_equal(expected, actual, 1e-9));
+  }
+  {
+    expected = numericalDerivative22<Vector2, Unit3, Unit3>(
+        boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q);
+    p.errorVector(q, boost::none, actual);
+    EXPECT(assert_equal(expected, actual, 1e-9));
+  }
+  {
+    expected = numericalDerivative22<Vector2, Unit3, Unit3>(
+        boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r);
+    p.errorVector(r, boost::none, actual);
+    EXPECT(assert_equal(expected, actual, 1e-9));
+  }
+}
+
 //*******************************************************************************
 TEST(Unit3, distance) {
   Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), //
@@ -155,107 +232,211 @@ TEST(Unit3, distance) {
 }
 
 //*******************************************************************************
-
+TEST(Unit3, localCoordinates0) {
-TEST(Unit3, localCoordinates) {
+  Unit3 p;
-  {
+  Vector actual = p.localCoordinates(p);
-    Unit3 p, q;
+  EXPECT(assert_equal(zero(2), actual, 1e-8));
-    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);
-    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, 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;
-  {
-    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)
-  }
-  {
-    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)
-  }
 }
 
 //*******************************************************************************
+TEST(Unit3, localCoordinates1) {
+  Unit3 p, q(1, 6.12385e-21, 0);
+  Vector actual = p.localCoordinates(q);
+  CHECK(assert_equal(zero(2), actual, 1e-8));
+}
+
+//*******************************************************************************
+TEST(Unit3, localCoordinates2) {
+  Unit3 p, q(-1, 0, 0);
+  Vector expected = (Vector(2) << M_PI, 0).finished();
+  Vector actual = p.localCoordinates(q);
+  CHECK(assert_equal(expected, actual, 1e-8));
+}
+
+//*******************************************************************************
+// Wrapper to make basis return a vector6 so we can test numerical derivatives.
+Vector6 BasisTest(const Unit3& p, OptionalJacobian<6, 2> H) {
+  Matrix32 B = p.basis(H);
+  Vector6 B_vec;
+  B_vec << B;
+  return B_vec;
+}
+
 TEST(Unit3, basis) {
-  Unit3 p;
+  Unit3 p(0.1, -0.2, 0.9);
-  Matrix32 expected;
+
-  expected << 0, 0, 0, -1, 1, 0;
+  Matrix expected(3, 2);
-  Matrix actual = p.basis();
+  expected << 0.0, -0.994169047, 0.97618706,
-  EXPECT(assert_equal(expected, actual, 1e-8));
+             -0.0233922129, 0.216930458,  0.105264958;
+
+  Matrix62 actualH;
+  Matrix actual = p.basis(actualH);
+  EXPECT(assert_equal(expected, actual, 1e-6));
+
+  Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
+                         boost::bind(BasisTest, _1, boost::none), p);
+  EXPECT(assert_equal(expectedH, actualH, 1e-8));
+}
+
+//*******************************************************************************
+/// Check the basis derivatives of a bunch of random Unit3s.
+TEST(Unit3, basis_derivatives) {
+  int num_tests = 100;
+  boost::mt19937 rng(42);
+  for (int i = 0; i < num_tests; i++) {
+    Unit3 p = Unit3::Random(rng);
+
+    Matrix62 actualH;
+    p.basis(actualH);
+
+    Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
+                           boost::bind(BasisTest, _1, boost::none), p);
+    EXPECT(assert_equal(expectedH, actualH, 1e-8));
+  }
 }
 
 //*******************************************************************************
 TEST(Unit3, retract) {
-  {
+  Unit3 p;
-    Unit3 p;
+  Vector v(2);
-    Vector2 v(0.5, 0);
+  v << 0.5, 0;
-    Unit3 expected(0.877583, 0, 0.479426);
+  Unit3 expected(0.877583, 0, 0.479426);
-    Unit3 actual = p.retract(v);
+  Unit3 actual = p.retract(v);
-    EXPECT(assert_equal(expected, actual, 1e-6));
+  EXPECT(assert_equal(expected, actual, 1e-6));
-    EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
+  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;
-  Vector2 v((M_PI / 2.0), 0);
+  Vector v(2);
+  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++) {
+
+    // Sleep for the random number generator (TODO?: Better create all of them first).
+//    boost::this_thread::sleep(boost::posix_time::milliseconds(0));
+
+    // 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)));
+//      Unit3 s2 (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++) {
+
+    // Sleep for the random number generator (TODO?: Better create all of them first).
+//    boost::this_thread::sleep(boost::posix_time::milliseconds(0));
+
+    // Create the two Unit3s.
+    // Unlike the above case, we can use any two Unit3's.
+    Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
+//      Unit3 s2 (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( Pose2, between )
+//{
+//  // <
+//  //
+//  //       ^
+//  //
+//  // *--0--*--*
+//  Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
+//  Pose2 gT2(M_PI, Point2(-1,4));  // robot at (-1,4) loooking at negative x
+//
+//  Matrix actualH1,actualH2;
+//  Pose2 expected(M_PI/2.0, Point2(2,2));
+//  Pose2 actual1 = gT1.between(gT2);
+//  Pose2 actual2 = gT1.between(gT2,actualH1,actualH2);
+//  EXPECT(assert_equal(expected,actual1));
+//  EXPECT(assert_equal(expected,actual2));
+//
+//  Matrix expectedH1 = (Matrix(3,3) <<
+//      0.0,-1.0,-2.0,
+//      1.0, 0.0,-2.0,
+//      0.0, 0.0,-1.0
+//  );
+//  Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
+//  EXPECT(assert_equal(expectedH1,actualH1));
+//  EXPECT(assert_equal(numericalH1,actualH1));
+//  // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx
+//  EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1));
+//
+//  Matrix expectedH2 = (Matrix(3,3) <<
+//       1.0, 0.0, 0.0,
+//       0.0, 1.0, 0.0,
+//       0.0, 0.0, 1.0
+//  );
+//  Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
+//  EXPECT(assert_equal(expectedH2,actualH2));
+//  EXPECT(assert_equal(numericalH2,actualH2));
+//
+//}
+
 //*******************************************************************************
 TEST(Unit3, Random) {
   boost::mt19937 rng(42);
@@ -267,26 +448,6 @@ 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;
@@ -298,12 +459,42 @@ TEST (Unit3, FromPoint3) {
   EXPECT(assert_equal(expectedH, actualH, 1e-8));
 }
 
-/* ************************************************************************* */
+//*******************************************************************************
-TEST(actualH, Serialization) {
+TEST(Unit3, ErrorBetweenFactor) {
-  Unit3 p(0, 1, 0);
+  std::vector<Unit3> data = {Unit3(1.0, 0.0, 0.0), Unit3(0.0, 0.0, 1.0)};
-  EXPECT(serializationTestHelpers::equalsObj(p));
+
-  EXPECT(serializationTestHelpers::equalsXML(p));
+  NonlinearFactorGraph graph;
-  EXPECT(serializationTestHelpers::equalsBinary(p));
+  Values initial_values;
+
+  // Add prior factors.
+  SharedNoiseModel R_prior = noiseModel::Unit::Create(2);
+  for (int i = 0; i < data.size(); i++) {
+    graph.add(PriorFactor<Unit3>(U(i), data[i], R_prior));
+  }
+
+  // Add process factors using the dot product error function.
+  SharedNoiseModel R_process = noiseModel::Isotropic::Sigma(2, 0.01);
+  for (int i = 0; i < data.size() - 1; i++) {
+    Expression<Vector2> exp(Expression<Unit3>(U(i)), &Unit3::errorVector, Expression<Unit3>(U(i + 1)));
+    graph.addExpressionFactor<Vector2>(R_process, Vector2::Zero(), exp);
+  }
+
+  // Add initial values. Since there is no identity, just pick something.
+  for (int i = 0; i < data.size(); i++) {
+    initial_values.insert(U(i), Unit3(0.0, 1.0, 0.0));
+  }
+
+  Values values = GaussNewtonOptimizer(graph, initial_values).optimize();
+
+  // Check that the y-value is very small for each.
+  for (int i = 0; i < data.size(); i++) {
+    EXPECT(assert_equal(0.0, values.at<Unit3>(U(i)).unitVector().y(), 1e-3));
+  }
+
+  // Check that the dot product between variables is close to 1.
+  for (int i = 0; i < data.size() - 1; i++) {
+    EXPECT(assert_equal(1.0, values.at<Unit3>(U(i)).dot(values.at<Unit3>(U(i + 1))), 1e-2));
+  }
 }
 
 /* ************************************************************************* */