merge in changes in develop branch recently

.gitignore

@@ -1,2 +1,3 @@
 /build*
 *.pyc
+*.DS_Store

gtsam/geometry/EssentialMatrix.cpp

@@ -0,0 +1,152 @@
+/*
+ * @file EssentialMatrix.cpp
+ * @brief EssentialMatrix class
+ * @author Frank Dellaert
+ * @date December 5, 2014
+ */
+
+#include <gtsam/geometry/EssentialMatrix.h>
+#include <iostream>
+
+using namespace std;
+
+namespace gtsam {
+
+/* ************************************************************************* */
+EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
+    boost::optional<Matrix&> H) {
+  const Rot3& _1R2_ = _1P2_.rotation();
+  const Point3& _1T2_ = _1P2_.translation();
+  if (!H) {
+    // just make a direction out of translation and create E
+    Sphere2 direction(_1T2_);
+    return EssentialMatrix(_1R2_, direction);
+  } else {
+    // Calculate the 5*6 Jacobian H = D_E_1P2
+    // D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation
+    // First get 2*3 derivative from Sphere2::FromPoint3
+    Matrix D_direction_1T2;
+    Sphere2 direction = Sphere2::FromPoint3(_1T2_, D_direction_1T2);
+    H->resize(5, 6);
+    H->block<3, 3>(0, 0) << Matrix::Identity(3, 3); // upper left
+    H->block<2, 3>(3, 0) << Matrix::Zero(2, 3); // lower left
+    H->block<3, 3>(0, 3) << Matrix::Zero(3, 3); // upper right
+    H->block<2, 3>(3, 3) << D_direction_1T2 * _1R2_.matrix(); // lower right
+    return EssentialMatrix(_1R2_, direction);
+  }
+}
+
+/* ************************************************************************* */
+void EssentialMatrix::print(const string& s) const {
+  cout << s;
+  aRb_.print("R:\n");
+  aTb_.print("d: ");
+}
+
+/* ************************************************************************* */
+EssentialMatrix EssentialMatrix::retract(const Vector& xi) const {
+  assert(xi.size() == 5);
+  Vector3 omega(sub(xi, 0, 3));
+  Vector2 z(sub(xi, 3, 5));
+  Rot3 R = aRb_.retract(omega);
+  Sphere2 t = aTb_.retract(z);
+  return EssentialMatrix(R, t);
+}
+
+/* ************************************************************************* */
+Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
+  return Vector(5) << //
+      aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_);
+}
+
+/* ************************************************************************* */
+Point3 EssentialMatrix::transform_to(const Point3& p,
+    boost::optional<Matrix&> DE, boost::optional<Matrix&> Dpoint) const {
+  Pose3 pose(aRb_, aTb_.point3());
+  Point3 q = pose.transform_to(p, DE, Dpoint);
+  if (DE) {
+    // DE returned by pose.transform_to is 3*6, but we need it to be 3*5
+    // The last 3 columns are derivative with respect to change in translation
+    // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
+    // Duy made an educated guess that this needs to be rotated to the local frame
+    Matrix H(3, 5);
+    H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis();
+    *DE = H;
+  }
+  return q;
+}
+
+/* ************************************************************************* */
+EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
+    boost::optional<Matrix&> HE, boost::optional<Matrix&> HR) const {
+
+  // The rotation must be conjugated to act in the camera frame
+  Rot3 c1Rc2 = aRb_.conjugate(cRb);
+
+  if (!HE && !HR) {
+    // Rotate translation direction and return
+    Sphere2 c1Tc2 = cRb * aTb_;
+    return EssentialMatrix(c1Rc2, c1Tc2);
+  } else {
+    // Calculate derivatives
+    Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2
+    Sphere2 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
+    if (HE) {
+      *HE = zeros(5, 5);
+      HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2
+      HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2)
+    }
+    if (HR) {
+      throw runtime_error(
+          "EssentialMatrix::rotate: derivative HR not implemented yet");
+      /*
+       HR->resize(5, 3);
+       HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ?
+       HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ?
+       */
+    }
+    return EssentialMatrix(c1Rc2, c1Tc2);
+  }
+}
+
+/* ************************************************************************* */
+double EssentialMatrix::error(const Vector& vA, const Vector& vB, //
+    boost::optional<Matrix&> H) const {
+  if (H) {
+    H->resize(1, 5);
+    // See math.lyx
+    Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB);
+    Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
+        * aTb_.basis();
+    *H << HR, HD;
+  }
+  return dot(vA, E_ * vB);
+}
+
+/* ************************************************************************* */
+ostream& operator <<(ostream& os, const EssentialMatrix& E) {
+  Rot3 R = E.rotation();
+  Sphere2 d = E.direction();
+  os.precision(10);
+  os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " ";
+  return os;
+}
+
+/* ************************************************************************* */
+istream& operator >>(istream& is, EssentialMatrix& E) {
+  double rx, ry, rz, dx, dy, dz;
+  is >> rx >> ry >> rz; // Read the rotation rxyz
+  is >> dx >> dy >> dz; // Read the translation dxyz
+
+  // Create EssentialMatrix from rotation and translation
+  Rot3 rot = Rot3::RzRyRx(rx, ry, rz);
+  Sphere2 dt = Sphere2(dx, dy, dz);
+  E = EssentialMatrix(rot, dt);
+
+  return is;
+}
+
+/* ************************************************************************* */
+
+} // gtsam
+

gtsam/geometry/EssentialMatrix.h

@@ -26,7 +26,7 @@ private:
 
   Rot3 aRb_; ///< Rotation between a and b
   Sphere2 aTb_; ///< translation direction from a to b
-  Matrix E_; ///< Essential matrix
+  Matrix3 E_; ///< Essential matrix
 
 public:
 
@@ -48,6 +48,10 @@ public:
       aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) {
   }
 
+  /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
+  static EssentialMatrix FromPose3(const Pose3& _1P2_,
+      boost::optional<Matrix&> H = boost::none);
+
   /// Random, using Rot3::Random and Sphere2::Random
   template<typename Engine>
   static EssentialMatrix Random(Engine & rng) {
@@ -60,11 +64,7 @@ public:
   /// @{
 
   /// print with optional string
-  void print(const std::string& s = "") const {
+  void print(const std::string& s = "") const;
-    std::cout << s;
-    aRb_.print("R:\n");
-    aTb_.print("d: ");
-  }
 
   /// assert equality up to a tolerance
   bool equals(const EssentialMatrix& other, double tol = 1e-8) const {
@@ -87,20 +87,10 @@ public:
   }
 
   /// Retract delta to manifold
-  virtual EssentialMatrix retract(const Vector& xi) const {
+  virtual EssentialMatrix retract(const Vector& xi) const;
-    assert(xi.size() == 5);
-    Vector3 omega(sub(xi, 0, 3));
-    Vector2 z(sub(xi, 3, 5));
-    Rot3 R = aRb_.retract(omega);
-    Sphere2 t = aTb_.retract(z);
-    return EssentialMatrix(R, t);
-  }
 
   /// Compute the coordinates in the tangent space
-  virtual Vector localCoordinates(const EssentialMatrix& other) const {
+  virtual Vector localCoordinates(const EssentialMatrix& other) const;
-    return Vector(5) << //
-        aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_);
-  }
 
   /// @}
 
@@ -108,17 +98,17 @@ public:
   /// @{
 
   /// Rotation
-  const Rot3& rotation() const {
+  inline const Rot3& rotation() const {
     return aRb_;
   }
 
   /// Direction
-  const Sphere2& direction() const {
+  inline const Sphere2& direction() const {
     return aTb_;
   }
 
   /// Return 3*3 matrix representation
-  const Matrix& matrix() const {
+  inline const Matrix3& matrix() const {
     return E_;
   }
 
@@ -131,20 +121,7 @@ public:
    */
   Point3 transform_to(const Point3& p,
       boost::optional<Matrix&> DE = boost::none,
-      boost::optional<Matrix&> Dpoint = boost::none) const {
+      boost::optional<Matrix&> Dpoint = boost::none) const;
-    Pose3 pose(aRb_, aTb_.point3());
-    Point3 q = pose.transform_to(p, DE, Dpoint);
-    if (DE) {
-      // DE returned by pose.transform_to is 3*6, but we need it to be 3*5
-      // The last 3 columns are derivative with respect to change in translation
-      // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
-      // Duy made an educated guess that this needs to be rotated to the local frame
-      Matrix H(3, 5);
-      H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis();
-      *DE = H;
-    }
-    return q;
-  }
 
   /**
    * Given essential matrix E in camera frame B, convert to body frame C
@@ -152,36 +129,7 @@ public:
    * @param E essential matrix E in camera frame C
    */
   EssentialMatrix rotate(const Rot3& cRb, boost::optional<Matrix&> HE =
-      boost::none, boost::optional<Matrix&> HR = boost::none) const {
+      boost::none, boost::optional<Matrix&> HR = boost::none) const;
-
-    // The rotation must be conjugated to act in the camera frame
-    Rot3 c1Rc2 = aRb_.conjugate(cRb);
-
-    if (!HE && !HR) {
-      // Rotate translation direction and return
-      Sphere2 c1Tc2 = cRb * aTb_;
-      return EssentialMatrix(c1Rc2, c1Tc2);
-    } else {
-      // Calculate derivatives
-      Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2
-      Sphere2 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
-      if (HE) {
-        *HE = zeros(5, 5);
-        HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2
-        HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2)
-      }
-      if (HR) {
-        throw std::runtime_error(
-            "EssentialMatrix::rotate: derivative HR not implemented yet");
-        /*
-         HR->resize(5, 3);
-         HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ?
-         HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ?
-         */
-      }
-      return EssentialMatrix(c1Rc2, c1Tc2);
-    }
-  }
 
   /**
    * Given essential matrix E in camera frame B, convert to body frame C
@@ -194,17 +142,18 @@ public:
 
   /// epipolar error, algebraic
   double error(const Vector& vA, const Vector& vB, //
-      boost::optional<Matrix&> H = boost::none) const {
+      boost::optional<Matrix&> H = boost::none) const;
-    if (H) {
+
-      H->resize(1, 5);
+  /// @}
-      // See math.lyx
+
-      Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB);
+  /// @name Streaming operators
-      Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
+  /// @{
-          * aTb_.basis();
+
-      *H << HR, HD;
+  /// stream to stream
-    }
+  friend std::ostream& operator <<(std::ostream& os, const EssentialMatrix& E);
-    return dot(vA, E_ * vB);
+
-  }
+  /// stream from stream
+  friend std::istream& operator >>(std::istream& is, EssentialMatrix& E);
 
   /// @}
 

gtsam/geometry/Point3.cpp

@@ -26,7 +26,8 @@ INSTANTIATE_LIE(Point3);
 
 /* ************************************************************************* */
 bool Point3::equals(const Point3 & q, double tol) const {
-  return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol);
+  return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol
+      && fabs(z_ - q.z()) < tol);
 }
 
 /* ************************************************************************* */
@@ -37,18 +38,18 @@ void Point3::print(const string& s) const {
 
 /* ************************************************************************* */
 
-bool Point3::operator== (const Point3& q) const {
+bool Point3::operator==(const Point3& q) const {
   return x_ == q.x_ && y_ == q.y_ && z_ == q.z_;
 }
 
 /* ************************************************************************* */
 Point3 Point3::operator+(const Point3& q) const {
-  return Point3( x_ + q.x_, y_ + q.y_, z_ + q.z_ );
+  return Point3(x_ + q.x_, y_ + q.y_, z_ + q.z_);
 }
 
 /* ************************************************************************* */
-Point3 Point3::operator- (const Point3& q) const {
+Point3 Point3::operator-(const Point3& q) const {
-  return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ );
+  return Point3(x_ - q.x_, y_ - q.y_, z_ - q.z_);
 }
 
 /* ************************************************************************* */
@@ -62,36 +63,53 @@ Point3 Point3::operator/(double s) const {
 }
 
 /* ************************************************************************* */
-Point3 Point3::add(const Point3 &q,
+Point3 Point3::add(const Point3 &q, boost::optional<Matrix&> H1,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+    boost::optional<Matrix&> H2) const {
-  if (H1) *H1 = eye(3,3);
+  if (H1)
-  if (H2) *H2 = eye(3,3);
+    *H1 = eye(3, 3);
+  if (H2)
+    *H2 = eye(3, 3);
   return *this + q;
 }
 
 /* ************************************************************************* */
-Point3 Point3::sub(const Point3 &q,
+Point3 Point3::sub(const Point3 &q, boost::optional<Matrix&> H1,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+    boost::optional<Matrix&> H2) const {
-  if (H1) *H1 = eye(3,3);
+  if (H1)
-  if (H2) *H2 = -eye(3,3);
+    *H1 = eye(3, 3);
+  if (H2)
+    *H2 = -eye(3, 3);
   return *this - q;
 }
 
 /* ************************************************************************* */
 Point3 Point3::cross(const Point3 &q) const {
-  return Point3( y_*q.z_ - z_*q.y_,
+  return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_,
-      z_*q.x_ - x_*q.z_,
+      x_ * q.y_ - y_ * q.x_);
-      x_*q.y_ - y_*q.x_ );
 }
 
 /* ************************************************************************* */
 double Point3::dot(const Point3 &q) const {
-  return ( x_*q.x_ + y_*q.y_ + z_*q.z_ );
+  return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_);
 }
 
 /* ************************************************************************* */
 double Point3::norm() const {
-  return sqrt( x_*x_ + y_*y_ + z_*z_ );
+  return sqrt(x_ * x_ + y_ * y_ + z_ * z_);
+}
+
+/* ************************************************************************* */
+Point3 Point3::normalize(boost::optional<Matrix&> H) const {
+  Point3 normalized = *this / norm();
+  if (H) {
+    // 3*3 Derivative
+    double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_;
+    double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_;
+    H->resize(3, 3);
+    *H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2;
+    *H /= pow(x2 + y2 + z2, 1.5);
+  }
+  return normalized;
 }
 
 /* ************************************************************************* */

gtsam/geometry/Point3.h

@@ -176,6 +176,9 @@ namespace gtsam {
     /** Distance of the point from the origin */
     double norm() const;
 
+    /** normalize, with optional Jacobian */
+    Point3 normalize(boost::optional<Matrix&> H = boost::none) const;
+
     /** cross product @return this x q */
     Point3 cross(const Point3 &q) const;
 

gtsam/geometry/Sphere2.cpp

@@ -28,6 +28,21 @@ using namespace std;
 
 namespace gtsam {
 
+/* ************************************************************************* */
+Sphere2 Sphere2::FromPoint3(const Point3& point,
+    boost::optional<Matrix&> H) {
+  Sphere2 direction(point);
+  if (H) {
+    // 3*3 Derivative of representation with respect to point is 3*3:
+    Matrix D_p_point;
+    point.normalize(D_p_point); // TODO, this calculates norm a second time :-(
+    // Calculate the 2*3 Jacobian
+    H->resize(2, 3);
+    *H << direction.basis().transpose() * D_p_point;
+  }
+  return direction;
+}
+
 /* ************************************************************************* */
 Sphere2 Sphere2::Random(boost::random::mt19937 & rng) {
   // TODO allow any engine without including all of boost :-(

gtsam/geometry/Sphere2.h

@@ -70,6 +70,10 @@ public:
     p_ = p_ / p_.norm();
   }
 
+  /// Named constructor from Point3 with optional Jacobian
+  static Sphere2 FromPoint3(const Point3& point,
+      boost::optional<Matrix&> H = boost::none);
+
   /// Random direction, using boost::uniform_on_sphere
   static Sphere2 Random(boost::random::mt19937 & rng);
 
@@ -90,7 +94,11 @@ public:
   /// @name Other functionality
   /// @{
 
-  /// Returns the local coordinate frame to tangent plane
+  /**
+   * 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.
+   */
   Matrix basis() const;
 
   /// Return skew-symmetric associated with 3D point on unit sphere

gtsam/geometry/tests/testEssentialMatrix.cpp

@@ -10,6 +10,7 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/Testable.h>
 #include <CppUnitLite/TestHarness.h>
+#include <sstream>
 
 using namespace std;
 using namespace gtsam;
@@ -95,6 +96,38 @@ TEST (EssentialMatrix, rotate) {
   // Does not work yet EXPECT(assert_equal(expH2, actH2, 1e-8));
 }
 
+//*************************************************************************
+TEST (EssentialMatrix, FromPose3_a) {
+  Matrix actualH;
+  Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
+  EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
+  Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
+      boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
+  EXPECT(assert_equal(expectedH, actualH, 1e-8));
+}
+
+//*************************************************************************
+TEST (EssentialMatrix, FromPose3_b) {
+  Matrix actualH;
+  Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3);
+  Point3 c1Tc2(0.4, 0.5, 0.6);
+  EssentialMatrix trueE(c1Rc2, c1Tc2);
+  Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
+  EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
+  Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
+      boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
+  EXPECT(assert_equal(expectedH, actualH, 1e-8));
+}
+
+//*************************************************************************
+TEST (EssentialMatrix, streaming) {
+  EssentialMatrix expected(c1Rc2, c1Tc2), actual;
+  stringstream ss;
+  ss << expected;
+  ss >> actual;
+  EXPECT(assert_equal(expected, actual));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/geometry/tests/testPoint3.cpp

@@ -14,73 +14,84 @@
  * @brief  Unit tests for Point3 class
  */
 
-#include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/Testable.h>
 #include <gtsam/geometry/Point3.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <CppUnitLite/TestHarness.h>
 
 using namespace gtsam;
 
 GTSAM_CONCEPT_TESTABLE_INST(Point3)
 GTSAM_CONCEPT_LIE_INST(Point3)
 
-static Point3 P(0.2,0.7,-2);
+static Point3 P(0.2, 0.7, -2);
 
 /* ************************************************************************* */
 TEST(Point3, Lie) {
-  Point3 p1(1,2,3);
+  Point3 p1(1, 2, 3);
-  Point3 p2(4,5,6);
+  Point3 p2(4, 5, 6);
   Matrix H1, H2;
 
-  EXPECT(assert_equal(Point3(5,7,9), p1.compose(p2, 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(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.retract((Vector(3) << 4., 5., 6.))));
+  EXPECT(assert_equal(Point3(5, 7, 9), p1.retract((Vector(3) << 4., 5., 6.))));
   EXPECT(assert_equal((Vector)(Vector(3) << 3.,3.,3.), p1.localCoordinates(p2)));
 }
 
 /* ************************************************************************* */
-TEST( Point3, arithmetic)
+TEST( Point3, arithmetic) {
-{
+  CHECK(P * 3 == 3 * P);
-  CHECK(P*3==3*P);
+  CHECK(assert_equal(Point3(-1, -5, -6), -Point3(1, 5, 6)));
-  CHECK(assert_equal( Point3(-1,-5,-6), -Point3(1,5,6) ));
+  CHECK(assert_equal(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1)));
-  CHECK(assert_equal( Point3(2,5,6), Point3(1,4,5)+Point3(1,1,1)));
+  CHECK(assert_equal(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1)));
-  CHECK(assert_equal( Point3(0,3,4), Point3(1,4,5)-Point3(1,1,1)));
+  CHECK(assert_equal(Point3(2, 8, 6), Point3(1, 4, 3) * 2));
-  CHECK(assert_equal( Point3(2,8,6), Point3(1,4,3)*2));
+  CHECK(assert_equal(Point3(2, 2, 6), 2 * Point3(1, 1, 3)));
-  CHECK(assert_equal( Point3(2,2,6), 2*Point3(1,1,3)));
+  CHECK(assert_equal(Point3(1, 2, 3), Point3(2, 4, 6) / 2));
-  CHECK(assert_equal( Point3(1,2,3), Point3(2,4,6)/2));
 }
 
 /* ************************************************************************* */
-TEST( Point3, equals)
+TEST( Point3, equals) {
-{
   CHECK(P.equals(P));
   Point3 Q;
   CHECK(!P.equals(Q));
 }
 
 /* ************************************************************************* */
-TEST( Point3, dot)
+TEST( Point3, dot) {
-{
+  Point3 origin, ones(1, 1, 1);
-  Point3 origin, ones(1,1,1);
+  CHECK(origin.dot(Point3(1, 1, 0)) == 0);
-  CHECK(origin.dot(Point3(1,1,0)) == 0);
+  CHECK(ones.dot(Point3(1, 1, 0)) == 2);
-  CHECK(ones.dot(Point3(1,1,0)) == 2);
 }
 
 /* ************************************************************************* */
-TEST( Point3, stream)
+TEST( Point3, stream) {
-{
+  Point3 p(1, 2, -3);
-  Point3 p(1,2, -3);
   std::ostringstream os;
   os << p;
   EXPECT(os.str() == "[1, 2, -3]';");
 }
 
+//*************************************************************************
+TEST (Point3, normalize) {
+  Matrix actualH;
+  Point3 point(1, -2, 3); // arbitrary point
+  Point3 expected(point / sqrt(14));
+  EXPECT(assert_equal(expected, point.normalize(actualH), 1e-8));
+  Matrix expectedH = numericalDerivative11<Point3, Point3>(
+      boost::bind(&Point3::normalize, _1, boost::none), point);
+  EXPECT(assert_equal(expectedH, actualH, 1e-8));
+}
+
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */
 

gtsam/geometry/tests/testSphere2.cpp

@@ -27,6 +27,7 @@
 #include <boost/foreach.hpp>
 #include <boost/random.hpp>
 #include <boost/assign/std/vector.hpp>
+#include <cmath>
 
 using namespace boost::assign;
 using namespace gtsam;
@@ -324,6 +325,17 @@ TEST(Sphere2, Random) {
   EXPECT(assert_equal(expectedMean,actualMean,0.1));
 }
 
+//*************************************************************************
+TEST (Sphere2, FromPoint3) {
+  Matrix actualH;
+  Point3 point(1, -2, 3); // arbitrary point
+  Sphere2 expected(point);
+  EXPECT(assert_equal(expected, Sphere2::FromPoint3(point, actualH), 1e-8));
+  Matrix expectedH = numericalDerivative11<Sphere2, Point3>(
+      boost::bind(Sphere2::FromPoint3, _1, boost::none), point);
+  EXPECT(assert_equal(expectedH, actualH, 1e-8));
+}
+
 /* ************************************************************************* */
 int main() {
   srand(time(NULL));

gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp

@@ -74,6 +74,16 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::linearize() const {
   return graph_.linearize(state_.values);
 }
 
+/* ************************************************************************* */
+void LevenbergMarquardtOptimizer::increaseLambda(){
+  state_.lambda *= params_.lambdaFactor;
+}
+
+/* ************************************************************************* */
+void LevenbergMarquardtOptimizer::decreaseLambda(){
+  state_.lambda /= params_.lambdaFactor;
+}
+
 /* ************************************************************************* */
 void LevenbergMarquardtOptimizer::iterate() {
 
@@ -146,7 +156,7 @@ void LevenbergMarquardtOptimizer::iterate() {
       if (error <= state_.error) {
         state_.values.swap(newValues);
         state_.error = error;
-        state_.lambda /= params_.lambdaFactor;
+        decreaseLambda();
         break;
       } else {
         // Either we're not cautious, or the same lambda was worse than the current error.
@@ -157,7 +167,10 @@ void LevenbergMarquardtOptimizer::iterate() {
             cout << "Warning:  Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl;
           break;
         } else {
-          state_.lambda *= params_.lambdaFactor;
+          if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
+            cout << "increasing lambda: old error (" << state_.error << ") new error (" << error << ")"  << endl;
+
+          increaseLambda();
         }
       }
     } catch (IndeterminantLinearSystemException& e) {
@@ -172,7 +185,7 @@ void LevenbergMarquardtOptimizer::iterate() {
           cout << "Warning:  Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl;
         break;
       } else {
-        state_.lambda *= params_.lambdaFactor;
+        increaseLambda();
       }
     }
 // Frank asks: why would we do that?

gtsam/nonlinear/LevenbergMarquardtOptimizer.h

@@ -174,6 +174,12 @@ public:
     return state_.lambda;
   }
 
+  // Apply policy to increase lambda if the current update was successful
+  virtual void increaseLambda();
+
+  // Apply policy to decrease lambda if the current update was NOT successful
+  virtual void decreaseLambda();
+
   /// Access the current number of inner iterations
   int getInnerIterations() const {
     return state_.totalNumberInnerIterations;

gtsam/slam/EssentialMatrixConstraint.cpp

@@ -0,0 +1,75 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2014, 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   EssentialMatrixConstraint.cpp
+ *  @author Frank Dellaert
+ *  @author Pablo Alcantarilla
+ *  @date   Jan 5, 2014
+ **/
+
+#include <gtsam/slam/EssentialMatrixConstraint.h>
+//#include <gtsam/linear/GaussianFactor.h>
+//#include <gtsam/base/Testable.h>
+
+#include <ostream>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+void EssentialMatrixConstraint::print(const std::string& s,
+    const KeyFormatter& keyFormatter) const {
+  std::cout << s << "EssentialMatrixConstraint(" << keyFormatter(this->key1())
+      << "," << keyFormatter(this->key2()) << ")\n";
+  measuredE_.print("  measured: ");
+  this->noiseModel_->print("  noise model: ");
+}
+
+/* ************************************************************************* */
+bool EssentialMatrixConstraint::equals(const NonlinearFactor& expected,
+    double tol) const {
+  const This *e = dynamic_cast<const This*>(&expected);
+  return e != NULL && Base::equals(*e, tol)
+      && this->measuredE_.equals(e->measuredE_, tol);
+}
+
+/* ************************************************************************* */
+Vector EssentialMatrixConstraint::evaluateError(const Pose3& p1,
+    const Pose3& p2, boost::optional<Matrix&> Hp1,
+    boost::optional<Matrix&> Hp2) const {
+
+  // compute relative Pose3 between p1 and p2
+  Pose3 _1P2_ = p1.between(p2, Hp1, Hp2);
+
+  // convert to EssentialMatrix
+  Matrix D_hx_1P2;
+  EssentialMatrix hx = EssentialMatrix::FromPose3(_1P2_,
+      (Hp1 || Hp2) ? boost::optional<Matrix&>(D_hx_1P2) : boost::none);
+
+  // Calculate derivatives if needed
+  if (Hp1) {
+    // Hp1 will already contain the 6*6 derivative D_1P2_p1
+    const Matrix& D_1P2_p1 = *Hp1;
+    // The 5*6 derivative is obtained by chaining with 5*6 D_hx_1P2:
+    *Hp1 = D_hx_1P2 * D_1P2_p1;
+  }
+  if (Hp2) {
+    // Hp2 will already contain the 6*6 derivative D_1P2_p1
+    const Matrix& D_1P2_p2 = *Hp2;
+    // The 5*6 derivative is obtained by chaining with 5*6 D_hx_1P2:
+    *Hp2 = D_hx_1P2 * D_1P2_p2;
+  }
+
+  // manifold equivalent of h(x)-z -> log(z,h(x))
+  return measuredE_.localCoordinates(hx); // 5D error
+}
+
+} /// namespace gtsam

gtsam/slam/EssentialMatrixConstraint.h

@@ -0,0 +1,109 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2014, 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   EssentialMatrixConstraint.h
+ *  @author Frank Dellaert
+ *  @author Pablo Alcantarilla
+ *  @date   Jan 5, 2014
+ **/
+
+#pragma once
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/geometry/EssentialMatrix.h>
+
+namespace gtsam {
+
+/**
+ * Binary factor between two Pose3 variables induced by an EssentialMatrix measurement
+ * @addtogroup SLAM
+ */
+class EssentialMatrixConstraint: public NoiseModelFactor2<Pose3, Pose3> {
+
+private:
+
+  typedef EssentialMatrixConstraint This;
+  typedef NoiseModelFactor2<Pose3, Pose3> Base;
+
+  EssentialMatrix measuredE_; /** The measurement is an essential matrix */
+
+public:
+
+  // shorthand for a smart pointer to a factor
+  typedef boost::shared_ptr<EssentialMatrixConstraint> shared_ptr;
+
+  /** default constructor - only use for serialization */
+  EssentialMatrixConstraint() {
+  }
+
+  /**
+   *  Constructor
+   *  @param key1 key for first pose
+   *  @param key2 key for second pose
+   *  @param measuredE measured EssentialMatrix
+   *  @param model noise model, 5D !
+   */
+  EssentialMatrixConstraint(Key key1, Key key2,
+      const EssentialMatrix& measuredE, const SharedNoiseModel& model) :
+      Base(model, key1, key2), measuredE_(measuredE) {
+  }
+
+  virtual ~EssentialMatrixConstraint() {
+  }
+
+  /// @return a deep copy of this factor
+  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
+
+  /** implement functions needed for Testable */
+
+  /** print */
+  virtual void print(const std::string& s = "",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+
+  /** equals */
+  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+
+  /** implement functions needed to derive from Factor */
+
+  /** vector of errors */
+  virtual Vector evaluateError(const Pose3& p1, const Pose3& p2,
+      boost::optional<Matrix&> Hp1 = boost::none, //
+      boost::optional<Matrix&> Hp2 = boost::none) const;
+
+  /** return the measured */
+  const EssentialMatrix& measured() const {
+    return measuredE_;
+  }
+
+  /** number of variables attached to this factor */
+  std::size_t size() const {
+    return 2;
+  }
+
+private:
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar
+        & boost::serialization::make_nvp("NoiseModelFactor2",
+            boost::serialization::base_object<Base>(*this));
+    ar & BOOST_SERIALIZATION_NVP(measuredE_);
+  }
+};
+// \class EssentialMatrixConstraint
+
+}/// namespace gtsam

gtsam/slam/dataset.cpp

@@ -229,7 +229,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
   }
 
   cout << "load2D read a graph file with " << initial->size()
-                    << " vertices and " << graph->nrFactors() << " factors" << endl;
+                        << " vertices and " << graph->nrFactors() << " factors" << endl;
 
   return make_pair(graph, initial);
 }
@@ -403,7 +403,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
   }
 
   cout << "load2D read a graph file with " << initial->size()
-                    << " vertices and " << graph->nrFactors() << " factors" << endl;
+                        << " vertices and " << graph->nrFactors() << " factors" << endl;
 
   return make_pair(graph, initial);
 }
@@ -671,33 +671,36 @@ bool writeBAL(const string& filename, SfM_data &data)
 
 bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
 
-  // CHECKS
+  // Store poses or cameras in SfM_data
   Values valuesPoses = values.filter<Pose3>();
-  if( valuesPoses.size() != data.number_cameras()){
+  if( valuesPoses.size() == data.number_cameras() ){ // we only estimated camera poses
-    cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras()
+    for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera
-        <<") and values (#cameras " << valuesPoses.size() << ")!!" << endl;
+      Key poseKey = symbol('x',i);
-    return false;
+      Pose3 pose = values.at<Pose3>(poseKey);
+      Cal3Bundler K = data.cameras[i].calibration();
+      PinholeCamera<Cal3Bundler> camera(pose, K);
+      data.cameras[i] = camera;
+    }
+  } else {
+    Values valuesCameras = values.filter< PinholeCamera<Cal3Bundler> >();
+    if ( valuesCameras.size() == data.number_cameras() ){  // we only estimated camera poses and calibration
+      for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera
+        Key cameraKey = symbol('c',i);
+        PinholeCamera<Cal3Bundler> camera = values.at<PinholeCamera<Cal3Bundler> >(cameraKey);
+        data.cameras[i] = camera;
+      }
+    }else{
+      cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras()
+              <<") and values (#cameras " << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" << endl;
+      return false;
+    }
   }
+
+  // Store 3D points in SfM_data
   Values valuesPoints = values.filter<Point3>();
   if( valuesPoints.size() != data.number_tracks()){
     cout << "writeBALfromValues: different number of points in SfM_data (#points= " << data.number_tracks()
-        <<") and values (#points " << valuesPoints.size() << ")!!" << endl;
+            <<") and values (#points " << valuesPoints.size() << ")!!" << endl;
-  }
-  if(valuesPoints.size() + valuesPoses.size() != values.size()){
-    cout << "writeBALfromValues write only poses and points values!!" << endl;
-    return false;
-  }
-  if(valuesPoints.size()==0 || valuesPoses.size()==0){
-    cout << "writeBALfromValues: No point or pose in values!!" << endl;
-    return false;
-  }
-
-  for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera
-    Key poseKey = symbol('x',i);
-    Pose3 pose = values.at<Pose3>(poseKey);
-    Cal3Bundler K = data.cameras[i].calibration();
-    PinholeCamera<Cal3Bundler> camera(pose, K);
-    data.cameras[i] = camera;
   }
 
   for (size_t j = 0; j < data.number_tracks(); j++){ // for each point
@@ -713,8 +716,8 @@ bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
     }
   }
 
+  // Write SfM_data to file
   return writeBAL(filename, data);
 }
 
-
 } // \namespace gtsam

gtsam/slam/dataset.h

@@ -141,7 +141,9 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data);
  * while camera poses and values are read from Values)
  * @param filename The name of the BAL file to write
  * @param data SfM structure where the data is stored
- * @param values structure where the graph values are stored
+ * @param values structure where the graph values are stored (values can be either Pose3 or PinholeCamera<Cal3Bundler> for the
+ * cameras, and should be Point3 for the 3D points). Note that the current version
+ * assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1
  * @return true if the parsing was successful, false otherwise
  */
 GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, SfM_data &data, Values& values);

gtsam/slam/tests/testEssentialMatrixConstraint.cpp

@@ -0,0 +1,76 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  testEssentialMatrixConstraint.cpp
+ *  @brief Unit tests for EssentialMatrixConstraint Class
+ *  @author Frank Dellaert
+ *  @author Pablo Alcantarilla
+ *  @date Jan 5, 2014
+ */
+
+#include <gtsam/slam/EssentialMatrixConstraint.h>
+#include <gtsam/nonlinear/Symbol.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST( EssentialMatrixConstraint, test ) {
+  // Create a factor
+  Key poseKey1(1);
+  Key poseKey2(2);
+  Rot3 trueRotation = Rot3::RzRyRx(0.15, 0.15, -0.20);
+  Point3 trueTranslation(+0.5, -1.0, +1.0);
+  Sphere2 trueDirection(trueTranslation);
+  EssentialMatrix measurement(trueRotation, trueDirection);
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 0.25);
+  EssentialMatrixConstraint factor(poseKey1, poseKey2, measurement, model);
+
+  // Create a linearization point at the zero-error point
+  Pose3 pose1(Rot3::RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0));
+  Pose3 pose2(
+      Rot3::RzRyRx(0.179693265735950, 0.002945368776519, 0.102274823253840),
+      Point3(-3.37493895, 6.14660244, -8.93650986));
+
+  Vector expected = zero(5);
+  Vector actual = factor.evaluateError(pose1,pose2);
+  CHECK(assert_equal(expected, actual, 1e-8));
+
+  // Calculate numerical derivatives
+  Matrix expectedH1 = numericalDerivative11<Pose3>(
+      boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2,
+          boost::none, boost::none), pose1);
+  Matrix expectedH2 = numericalDerivative11<Pose3>(
+      boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1,
+          boost::none, boost::none), pose2);
+
+  // Use the factor to calculate the derivative
+  Matrix actualH1;
+  Matrix actualH2;
+  factor.evaluateError(pose1, pose2, actualH1, actualH2);
+
+  // Verify we get the expected error
+  CHECK(assert_equal(expectedH1, actualH1, 1e-9));
+  CHECK(assert_equal(expectedH2, actualH2, 1e-9));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+

gtsam/slam/tests/testEssentialMatrixFactor.cpp

@@ -154,8 +154,8 @@ TEST (EssentialMatrixFactor2, factor) {
     EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2);
 
     // Check evaluation
-    Point3 P1 = data.tracks[i].p;
+    Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1);
-    const Point2 pi = camera2.project(P1);
+    const Point2 pi = SimpleCamera::project_to_camera(P2);
     Point2 reprojectionError(pi - pB(i));
     Vector expected = reprojectionError.vector();
 
@@ -269,6 +269,21 @@ TEST (EssentialMatrixFactor3, minimization) {
     truth.insert(i, LieScalar(baseline / P1.z()));
   }
   EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
+
+  // Optimize
+  LevenbergMarquardtParams parameters;
+  // parameters.setVerbosity("ERROR");
+  LevenbergMarquardtOptimizer optimizer(graph, truth, parameters);
+  Values result = optimizer.optimize();
+
+  // Check result
+  EssentialMatrix actual = result.at<EssentialMatrix>(100);
+  EXPECT(assert_equal(bodyE, actual, 1e-1));
+  for (size_t i = 0; i < 5; i++)
+    EXPECT(assert_equal(truth.at<LieScalar>(i), result.at<LieScalar>(i), 1e-1));
+
+  // Check error at result
+  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
 }
 
 } // namespace example1