Merged in feature/SoundSlam (pull request #85)

Sound Expressions, including ExpressionFactorGraph...

.cproject

@@ -526,6 +526,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testEvent.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testEvent.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="check" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -840,6 +848,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testTOAFactor.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testTOAFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testGaussianFactorGraph.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>

examples/Pose2SLAMExampleExpressions.cpp

@@ -19,7 +19,7 @@
 
 // The two new headers that allow using our Automatic Differentiation Expression framework
 #include <gtsam/slam/expressions.h>
-#include <gtsam/nonlinear/ExpressionFactor.h>
+#include <gtsam/nonlinear/ExpressionFactorGraph.h>
 
 // Header order is close to far
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@@ -35,28 +35,26 @@ using namespace gtsam;
 int main(int argc, char** argv) {
 
   // 1. Create a factor graph container and add factors to it
-  NonlinearFactorGraph graph;
+  ExpressionFactorGraph graph;
 
   // Create Expressions for unknowns
   Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5);
 
   // 2a. Add a prior on the first pose, setting it to the origin
   noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-  graph.push_back(ExpressionFactor<Pose2>(priorNoise, Pose2(0, 0, 0), x1));
+  graph.addExpressionFactor(x1, Pose2(0, 0, 0), priorNoise);
 
   // For simplicity, we will use the same noise model for odometry and loop closures
   noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
 
   // 2b. Add odometry factors
-  BinaryExpression<Pose2, Pose2, Pose2>::Function f = traits<Pose2>::Between;
+  graph.addExpressionFactor(between(x1,x2), Pose2(2, 0, 0     ), model);
-  Expression<Pose2>(traits<Pose2>::Between, x1, x2);
+  graph.addExpressionFactor(between(x2,x3), Pose2(2, 0, M_PI_2), model);
-  graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, 0     ), between(x1,x2)));
+  graph.addExpressionFactor(between(x3,x4), Pose2(2, 0, M_PI_2), model);
-  graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x2,x3)));
+  graph.addExpressionFactor(between(x4,x5), Pose2(2, 0, M_PI_2), model);
-  graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x3,x4)));
-  graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x4,x5)));
 
   // 2c. Add the loop closure constraint
-  graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x5,x2)));
+  graph.addExpressionFactor(between(x5,x2), Pose2(2, 0, M_PI_2), model);
   graph.print("\nFactor Graph:\n"); // print
 
   // 3. Create the data structure to hold the initialEstimate estimate to the solution

examples/SFMExampleExpressions.cpp

@@ -24,14 +24,11 @@
 
 // The two new headers that allow using our Automatic Differentiation Expression framework
 #include <gtsam/slam/expressions.h>
-#include <gtsam/nonlinear/ExpressionFactor.h>
+#include <gtsam/nonlinear/ExpressionFactorGraph.h>
 
 // Header order is close to far
 #include <examples/SFMdata.h>
-#include <gtsam/slam/PriorFactor.h>
-#include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam/geometry/Point2.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/DoglegOptimizer.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/inference/Symbol.h>
@@ -40,27 +37,29 @@
 
 using namespace std;
 using namespace gtsam;
+using namespace noiseModel;
 
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
 
   Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
-  noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
+  Isotropic::shared_ptr measurementNoise = Isotropic::Sigma(2, 1.0); // one pixel in u and v
 
   // Create the set of ground-truth landmarks and poses
   vector<Point3> points = createPoints();
   vector<Pose3> poses = createPoses();
 
   // Create a factor graph
-  NonlinearFactorGraph graph;
+  ExpressionFactorGraph graph;
 
   // Specify uncertainty on first pose prior
-  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
+  Vector6 sigmas; sigmas << Vector3(0.3,0.3,0.3), Vector3(0.1,0.1,0.1);
+  Diagonal::shared_ptr poseNoise = Diagonal::Sigmas(sigmas);
 
   // Here we don't use a PriorFactor but directly the ExpressionFactor class
-  // The object x0 is an Expression, and we create a factor wanting it to be equal to poses[0]
+  // x0 is an Expression, and we create a factor wanting it to be equal to poses[0]
   Pose3_ x0('x',0);
-  graph.push_back(ExpressionFactor<Pose3>(poseNoise, poses[0], x0));
+  graph.addExpressionFactor(x0, poses[0], poseNoise);
 
   // We create a constant Expression for the calibration here
   Cal3_S2_ cK(K);
@@ -74,25 +73,26 @@ int main(int argc, char* argv[]) {
       // Below an expression for the prediction of the measurement:
       Point3_ p('l', j);
       Point2_ prediction = uncalibrate(cK, project(transform_to(x, p)));
-      // Again, here we use a ExpressionFactor
+      // Again, here we use an ExpressionFactor
-      graph.push_back(ExpressionFactor<Point2>(measurementNoise, measurement, prediction));
+      graph.addExpressionFactor(prediction, measurement, measurementNoise);
     }
   }
 
   // Add prior on first point to constrain scale, again with ExpressionFactor
-  noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
+  Isotropic::shared_ptr pointNoise = Isotropic::Sigma(3, 0.1);
-  graph.push_back(ExpressionFactor<Point3>(pointNoise, points[0], Point3_('l', 0)));
+  graph.addExpressionFactor(Point3_('l', 0), points[0], pointNoise);
 
   // Create perturbed initial
-  Values initialEstimate;
+  Values initial;
+  Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
   for (size_t i = 0; i < poses.size(); ++i)
-    initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
+    initial.insert(Symbol('x', i), poses[i].compose(delta));
   for (size_t j = 0; j < points.size(); ++j)
-    initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
+    initial.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
-  cout << "initial error = " << graph.error(initialEstimate) << endl;
+  cout << "initial error = " << graph.error(initial) << endl;
 
   /* Optimize the graph and print results */
-  Values result = DoglegOptimizer(graph, initialEstimate).optimize();
+  Values result = DoglegOptimizer(graph, initial).optimize();
   cout << "final error = " << graph.error(result) << endl;
 
   return 0;

gtsam/base/Lie.h

@@ -110,7 +110,7 @@ struct LieGroup {
   Class retract(const TangentVector& v, //
       ChartJacobian H1, ChartJacobian H2 = boost::none) const {
     Jacobian D_g_v;
-    Class g = Class::ChartAtOrigin::Retract(v,D_g_v);
+    Class g = Class::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0);
     Class h = compose(g,H1,H2);
     if (H2) *H2 = (*H2) * D_g_v;
     return h;
@@ -120,7 +120,7 @@ struct LieGroup {
       ChartJacobian H1, ChartJacobian H2 = boost::none) const {
     Class h = between(g,H1,H2);
     Jacobian D_v_h;
-    TangentVector v = Class::ChartAtOrigin::Local(h, D_v_h);
+    TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0);
     if (H1) *H1 = D_v_h * (*H1);
     if (H2) *H2 = D_v_h * (*H2);
     return v;

gtsam/base/numericalDerivative.h

@@ -296,6 +296,48 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (*
       x2, x3, delta);
 }
 
+/**
+ * Compute numerical derivative in argument 1 of 4-argument function
+ * @param h ternary function yielding m-vector
+ * @param x1 n-dimensional first argument value
+ * @param x2 second argument value
+ * @param x3 third argument value
+ * @param x4 fourth argument value
+ * @param delta increment for numerical derivative
+ * @return m*n Jacobian computed via central differencing
+ */
+template<class Y, class X1, class X2, class X3, class X4>
+typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
+    boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
+    const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
+      "Template argument Y must be a manifold type.");
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
+      "Template argument X1 must be a manifold type.");
+  return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2, x3, x4), x1, delta);
+}
+
+/**
+ * Compute numerical derivative in argument 2 of 4-argument function
+ * @param h ternary function yielding m-vector
+ * @param x1 n-dimensional first argument value
+ * @param x2 second argument value
+ * @param x3 third argument value
+ * @param x4 fourth argument value
+ * @param delta increment for numerical derivative
+ * @return m*n Jacobian computed via central differencing
+ */
+template<class Y, class X1, class X2, class X3, class X4>
+typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
+    boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
+    const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
+      "Template argument Y must be a manifold type.");
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
+      "Template argument X1 must be a manifold type.");
+  return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1, x3, x4), x2, delta);
+}
+
 /**
  * Compute numerical Hessian matrix.  Requires a single-argument Lie->scalar
  * function.  This is implemented simply as the derivative of the gradient.

gtsam/base/testLie.h

@@ -34,23 +34,25 @@ void testLieGroupDerivatives(TestResult& result_, const std::string& name_,
 
   Matrix H1, H2;
   typedef traits<G> T;
+  typedef OptionalJacobian<T::dimension,T::dimension> OJ;
 
   // Inverse
+  OJ none;
   EXPECT(assert_equal(t1.inverse(),T::Inverse(t1, H1)));
-  EXPECT(assert_equal(numericalDerivative11(T::Inverse, t1),H1));
+  EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t1, none),H1));
 
   EXPECT(assert_equal(t2.inverse(),T::Inverse(t2, H1)));
-  EXPECT(assert_equal(numericalDerivative11(T::Inverse, t2),H1));
+  EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t2, none),H1));
 
   // Compose
   EXPECT(assert_equal(t1 * t2,T::Compose(t1, t2, H1, H2)));
-  EXPECT(assert_equal(numericalDerivative21(T::Compose, t1, t2), H1));
+  EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H1));
-  EXPECT(assert_equal(numericalDerivative22(T::Compose, t1, t2), H2));
+  EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H2));
 
   // Between
   EXPECT(assert_equal(t1.inverse() * t2,T::Between(t1, t2, H1, H2)));
-  EXPECT(assert_equal(numericalDerivative21(T::Between, t1, t2), H1));
+  EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H1));
-  EXPECT(assert_equal(numericalDerivative22(T::Between, t1, t2), H2));
+  EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H2));
 }
 // Do a comprehensive test of Lie Group Chart derivatives
 template<typename G>
@@ -59,17 +61,20 @@ void testChartDerivatives(TestResult& result_, const std::string& name_,
 
   Matrix H1, H2;
   typedef traits<G> T;
+  typedef typename G::TangentVector V;
+  typedef OptionalJacobian<T::dimension,T::dimension> OJ;
 
   // Retract
-  typename G::TangentVector w12 = T::Local(t1, t2);
+  OJ none;
+  V w12 = T::Local(t1, t2);
   EXPECT(assert_equal(t2, T::Retract(t1,w12, H1, H2)));
-  EXPECT(assert_equal(numericalDerivative21(T::Retract, t1, w12), H1));
+  EXPECT(assert_equal(numericalDerivative41<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H1));
-  EXPECT(assert_equal(numericalDerivative22(T::Retract, t1, w12), H2));
+  EXPECT(assert_equal(numericalDerivative42<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H2));
 
   // Local
   EXPECT(assert_equal(w12, t1.localCoordinates(t2, H1, H2)));
-  EXPECT(assert_equal(numericalDerivative21(T::Local, t1, t2), H1));
+  EXPECT(assert_equal(numericalDerivative41<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H1));
-  EXPECT(assert_equal(numericalDerivative22(T::Local, t1, t2), H2));
+  EXPECT(assert_equal(numericalDerivative42<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H2));
 }
 } // namespace gtsam
 

gtsam/geometry/Point2.h

@@ -165,12 +165,12 @@ public:
   /// @{
   inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;}
   inline void operator *= (double s) {x_*=s;y_*=s;}
-  Point2 inverse() { return -(*this);}
+  Point2 inverse() const { return -(*this);}
-  Point2 compose(const Point2& q) { return (*this)+q;}
+  Point2 compose(const Point2& q) const { return (*this)+q;}
-  Point2 between(const Point2& q) { return q-(*this);}
+  Point2 between(const Point2& q) const { return q-(*this);}
-  Vector2 localCoordinates(const Point2& q) { return between(q).vector();}
+  Vector2 localCoordinates(const Point2& q) const { return between(q).vector();}
-  Point2 retract(const Vector2& v) {return compose(Point2(v));}
+  Point2 retract(const Vector2& v) const { return compose(Point2(v));}
-  static Vector2 Logmap(const Point2& p) {return p.vector();}
+  static Vector2 Logmap(const Point2& p) { return p.vector();}
   static Point2 Expmap(const Vector2& v) { return Point2(v);}
   /// @}
 

gtsam/geometry/Point3.h

@@ -164,12 +164,12 @@ namespace gtsam {
 
     /// @name Deprecated
     /// @{
-    Point3 inverse() { return -(*this);}
+    Point3 inverse() const { return -(*this);}
-    Point3 compose(const Point3& q) { return (*this)+q;}
+    Point3 compose(const Point3& q) const { return (*this)+q;}
-    Point3 between(const Point3& q) { return q-(*this);}
+    Point3 between(const Point3& q) const { return q-(*this);}
-    Vector3 localCoordinates(const Point3& q) { return between(q).vector();}
+    Vector3 localCoordinates(const Point3& q) const { return between(q).vector();}
-    Point3 retract(const Vector3& v) {return compose(Point3(v));}
+    Point3 retract(const Vector3& v) const { return compose(Point3(v));}
-    static Vector3 Logmap(const Point3& p) {return p.vector();}
+    static Vector3 Logmap(const Point3& p) { return p.vector();}
     static Point3 Expmap(const Vector3& v) { return Point3(v);}
     /// @}
 

gtsam/geometry/tests/testSO3.cpp

@@ -55,46 +55,6 @@ TEST(SO3 , Retract) {
   EXPECT(actual.isApprox(R2));
 }
 
-//******************************************************************************
-TEST(SO3 , Compose) {
-  SO3 expected = R1 * R2;
-  Matrix actualH1, actualH2;
-  SO3 actual = traits<SO3>::Compose(R1, R2, actualH1, actualH2);
-  EXPECT(traits<SO3>::Equals(expected,actual));
-
-  Matrix numericalH1 = numericalDerivative21(traits<SO3>::Compose, R1, R2);
-  EXPECT(assert_equal(numericalH1,actualH1));
-
-  Matrix numericalH2 = numericalDerivative22(traits<SO3>::Compose, R1, R2);
-  EXPECT(assert_equal(numericalH2,actualH2));
-}
-
-//******************************************************************************
-TEST(SO3 , Between) {
-  SO3 expected = R1.inverse() * R2;
-  Matrix actualH1, actualH2;
-  SO3 actual = traits<SO3>::Between(R1, R2, actualH1, actualH2);
-  EXPECT(traits<SO3>::Equals(expected,actual));
-
-  Matrix numericalH1 = numericalDerivative21(traits<SO3>::Between, R1, R2);
-  EXPECT(assert_equal(numericalH1,actualH1));
-
-  Matrix numericalH2 = numericalDerivative22(traits<SO3>::Between, R1, R2);
-  EXPECT(assert_equal(numericalH2,actualH2));
-}
-
-//******************************************************************************
-TEST(SO3 , Inverse) {
-  SO3 expected(Eigen::AngleAxisd(-0.1, z_axis));
-
-  Matrix actualH;
-  SO3 actual = traits<SO3>::Inverse(R1, actualH);
-  EXPECT(traits<SO3>::Equals(expected,actual));
-
-  Matrix numericalH = numericalDerivative11(traits<SO3>::Inverse, R1);
-  EXPECT(assert_equal(numericalH,actualH));
-}
-
 //******************************************************************************
 TEST(SO3 , Invariants) {
   check_group_invariants(id,id);

gtsam/nonlinear/Expression-inl.h

@@ -32,6 +32,7 @@
 #include <boost/mpl/fold.hpp>
 namespace MPL = boost::mpl::placeholders;
 
+#include <typeinfo>       // operator typeid
 #include <map>
 
 class ExpressionFactorBinaryTest;
@@ -245,6 +246,15 @@ public:
   virtual ~ExpressionNode() {
   }
 
+  /// Streaming
+  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os,
+      const ExpressionNode& node) {
+    os << "Expression of type " << typeid(T).name();
+    if (node.traceSize_>0) os << ", trace size = " << node.traceSize_;
+    os << "\n";
+    return os;
+  }
+
   /// Return keys that play in this expression as a set
   virtual std::set<Key> keys() const {
     std::set<Key> keys;
@@ -621,10 +631,11 @@ struct FunctionalNode {
 template<class T, class A1>
 class UnaryExpression: public FunctionalNode<T, boost::mpl::vector<A1> >::type {
 
+  typedef typename MakeOptionalJacobian<T, A1>::type OJ1;
+
 public:
 
-  typedef boost::function<
+  typedef boost::function<T(const A1&, OJ1)> Function;
-      T(const A1&, typename MakeOptionalJacobian<T, A1>::type)> Function;
   typedef typename FunctionalNode<T, boost::mpl::vector<A1> >::type Base;
   typedef typename Base::Record Record;
 
@@ -664,13 +675,15 @@ public:
 /// Binary Expression
 
 template<class T, class A1, class A2>
-class BinaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2> >::type {
+class BinaryExpression:
+    public FunctionalNode<T, boost::mpl::vector<A1, A2> >::type {
+
+  typedef typename MakeOptionalJacobian<T, A1>::type OJ1;
+  typedef typename MakeOptionalJacobian<T, A2>::type OJ2;
 
 public:
 
-  typedef boost::function<
+  typedef boost::function<T(const A1&, const A2&, OJ1, OJ2)> Function;
-      T(const A1&, const A2&, typename MakeOptionalJacobian<T, A1>::type,
-          typename MakeOptionalJacobian<T, A2>::type)> Function;
   typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2> >::type Base;
   typedef typename Base::Record Record;
 
@@ -718,15 +731,16 @@ public:
 /// Ternary Expression
 
 template<class T, class A1, class A2, class A3>
-class TernaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type {
+class TernaryExpression:
+    public FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type {
+
+  typedef typename MakeOptionalJacobian<T, A1>::type OJ1;
+  typedef typename MakeOptionalJacobian<T, A2>::type OJ2;
+  typedef typename MakeOptionalJacobian<T, A3>::type OJ3;
 
 public:
 
-  typedef boost::function<
+  typedef boost::function<T(const A1&, const A2&, const A3&, OJ1, OJ2, OJ3)> Function;
-      T(const A1&, const A2&, const A3&,
-          typename MakeOptionalJacobian<T, A1>::type,
-          typename MakeOptionalJacobian<T, A2>::type,
-          typename MakeOptionalJacobian<T, A3>::type)> Function;
   typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type Base;
   typedef typename Base::Record Record;
 

gtsam/nonlinear/Expression.h

@@ -52,6 +52,11 @@ private:
 
 public:
 
+  /// Print
+  void print(const std::string& s) const {
+    std::cout << s << *root_ << std::endl;
+  }
+
   // Construct a constant expression
   Expression(const T& value) :
       root_(new ConstantExpression<T>(value)) {
@@ -104,6 +109,20 @@ public:
       root_(new BinaryExpression<T, A1, A2>(function, expression1, expression2)) {
   }
 
+  /// Construct a binary method expression
+  template<typename A1, typename A2, typename A3>
+  Expression(const Expression<A1>& expression1,
+      T (A1::*method)(const A2&, const A3&,
+          typename TernaryExpression<T, A1, A2, A3>::OJ1,
+          typename TernaryExpression<T, A1, A2, A3>::OJ2,
+          typename TernaryExpression<T, A1, A2, A3>::OJ3) const,
+      const Expression<A2>& expression2, const Expression<A3>& expression3) :
+      root_(
+          new TernaryExpression<T, A1, A2, A3>(
+              boost::bind(method, _1, _2, _3, _4, _5, _6), expression1,
+              expression2, expression3)) {
+  }
+
   /// Construct a ternary function expression
   template<typename A1, typename A2, typename A3>
   Expression(typename TernaryExpression<T, A1, A2, A3>::Function function,

gtsam/nonlinear/ExpressionFactor.h

@@ -109,5 +109,5 @@ public:
 };
 // ExpressionFactor
 
-}
+} // \ namespace gtsam
 

gtsam/nonlinear/ExpressionFactorGraph.h

@@ -0,0 +1,51 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  ExpressionFactorGraph.h
+ *  @brief Factor graph that supports adding ExpressionFactors directly
+ *  @author Frank Dellaert
+ *  @date December 2014
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/ExpressionFactor.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+
+namespace gtsam {
+
+/**
+ * Factor graph that supports adding ExpressionFactors directly
+ */
+class ExpressionFactorGraph: public NonlinearFactorGraph {
+
+public:
+
+  /// @name Adding Factors
+  /// @{
+
+  /**
+   * Directly add ExpressionFactor that implements |h(x)-z|^2_R
+   * @param h expression that implements measurement function
+   * @param z measurement
+   * @param R model
+   */
+  template<typename T>
+  void addExpressionFactor(const Expression<T>& h, const T& z,
+      const SharedNoiseModel& R) {
+    push_back(boost::make_shared<ExpressionFactor<T> >(R, z, h));
+  }
+
+  /// @}
+};
+
+}

gtsam/nonlinear/tests/testExpression.cpp

@@ -21,7 +21,6 @@
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/base/Testable.h>
-#include <gtsam/base/LieScalar.h>
 
 #include <CppUnitLite/TestHarness.h>
 
@@ -78,9 +77,6 @@ namespace unary {
 Point2 f0(const Point3& p, OptionalJacobian<2,3> H) {
   return Point2();
 }
-LieScalar f1(const Point3& p, OptionalJacobian<1, 3> H) {
-  return LieScalar(0.0);
-}
 double f2(const Point3& p, OptionalJacobian<1, 3> H) {
   return 0.0;
 }
@@ -92,11 +88,6 @@ TEST(Expression, Unary0) {
   Expression<Point2> e(f0, p);
   EXPECT(expected == e.keys());
 }
-TEST(Expression, Unary1) {
-  using namespace unary;
-  Expression<double> e(f1, p);
-  EXPECT(expected == e.keys());
-}
 TEST(Expression, Unary2) {
   using namespace unary;
   Expression<double> e(f2, p);

gtsam_unstable/geometry/Event.h

@@ -0,0 +1,109 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  Event
+ *  @brief Space-time event
+ *  @author Frank Dellaert
+ *  @author Jay Chakravarty
+ *  @date December 2014
+ */
+
+#pragma once
+
+#include <gtsam/geometry/Point3.h>
+#include <cmath>
+
+namespace gtsam {
+
+/// A space-time event
+class Event {
+
+  double time_; ///< Time event was generated
+  Point3 location_; ///< Location at time event was generated
+
+public:
+  enum { dimension = 4 };
+
+  /// Speed of sound
+  static const double Speed;
+  static const Matrix14 JacobianZ;
+
+  /// Default Constructor
+  Event() :
+      time_(0) {
+  }
+
+  /// Constructor from time and location
+  Event(double t, const Point3& p) :
+      time_(t), location_(p) {
+  }
+
+  /// Constructor with doubles
+  Event(double t, double x, double y, double z) :
+      time_(t), location_(x, y, z) {
+  }
+
+  double time() const { return time_;}
+  Point3 location() const { return location_;}
+
+  // TODO we really have to think of a better way to do linear arguments
+  double height(OptionalJacobian<1,4> H = boost::none) const {
+    if (H) *H = JacobianZ;
+    return location_.z();
+  }
+
+  /** print with optional string */
+  void print(const std::string& s = "") const {
+    std::cout << s << "time = " << time_;
+    location_.print("; location");
+  }
+
+  /** equals with an tolerance */
+  bool equals(const Event& other, double tol = 1e-9) const {
+    return std::abs(time_ - other.time_) < tol
+        && location_.equals(other.location_, tol);
+  }
+
+  /// Updates a with tangent space delta
+  inline Event retract(const Vector4& v) const {
+    return Event(time_ + v[0], location_.retract(v.tail(3)));
+  }
+
+  /// Returns inverse retraction
+  inline Vector4 localCoordinates(const Event& q) const {
+    return Vector4::Zero(); // TODO
+  }
+
+  /// Time of arrival to given microphone
+  double toa(const Point3& microphone, //
+      OptionalJacobian<1, 4> H1 = boost::none, //
+      OptionalJacobian<1, 3> H2 = boost::none) const {
+    Matrix13 D1, D2;
+    double distance = location_.distance(microphone, D1, D2);
+    if (H1)
+      // derivative of toa with respect to event
+      *H1 << 1.0, D1 / Speed;
+    if (H2)
+      // derivative of toa with respect to microphone location
+      *H2 << D2 / Speed;
+    return time_ + distance / Speed;
+  }
+};
+
+const double Event::Speed = 330;
+const Matrix14 Event::JacobianZ = (Matrix14() << 0,0,0,1).finished();
+
+// Define GTSAM traits
+template<>
+struct GTSAM_EXPORT traits<Event> : internal::Manifold<Event> {};
+
+} //\ namespace gtsam

gtsam_unstable/geometry/tests/testEvent.cpp

@@ -0,0 +1,100 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  testEvent.cpp
+ *  @brief Unit tests for space time "Event"
+ *  @author Frank Dellaert
+ *  @author Jay Chakravarty
+ *  @date December 2014
+ */
+
+#include <gtsam_unstable/geometry/Event.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/nonlinear/Expression.h>
+#include <CppUnitLite/TestHarness.h>
+#include <boost/bind.hpp>
+
+using namespace std;
+using namespace gtsam;
+
+// Create a noise model for the TOA error
+static const double ms = 1e-3;
+static const double cm = 1e-2;
+typedef Eigen::Matrix<double, 1, 1> Vector1;
+static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1,0.5*ms));
+
+static const double timeOfEvent = 25;
+static const Event exampleEvent(timeOfEvent, 1, 0, 0);
+static const Point3 microphoneAt0;
+
+//*****************************************************************************
+TEST( Event, Constructor ) {
+  const double t = 0;
+  Event actual(t, 201.5 * cm, 201.5 * cm, (212 - 45) * cm);
+}
+
+//*****************************************************************************
+TEST( Event, Toa1 ) {
+  Event event(0, 1, 0, 0);
+  double expected = 1 / Event::Speed;
+  EXPECT_DOUBLES_EQUAL(expected, event.toa(microphoneAt0), 1e-9);
+}
+
+//*****************************************************************************
+TEST( Event, Toa2 ) {
+  double expectedTOA = timeOfEvent + 1 / Event::Speed;
+  EXPECT_DOUBLES_EQUAL(expectedTOA, exampleEvent.toa(microphoneAt0), 1e-9);
+}
+
+//*************************************************************************
+TEST (Event, Derivatives) {
+  Matrix14 actualH1;
+  Matrix13 actualH2;
+  exampleEvent.toa(microphoneAt0, actualH1, actualH2);
+  Matrix expectedH1 = numericalDerivative11<double, Event>(
+      boost::bind(&Event::toa, _1, microphoneAt0, boost::none, boost::none),
+      exampleEvent);
+  EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
+  Matrix expectedH2 = numericalDerivative11<double, Point3>(
+      boost::bind(&Event::toa, exampleEvent, _1, boost::none, boost::none),
+      microphoneAt0);
+  EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
+}
+
+//*****************************************************************************
+TEST( Event, Expression ) {
+  Key key = 12;
+  Expression<Event> event_(key);
+  Expression<Point3> knownMicrophone_(microphoneAt0); // constant expression
+  Expression<double> expression(&Event::toa, event_, knownMicrophone_);
+
+  Values values;
+  values.insert(key, exampleEvent);
+  double expectedTOA = timeOfEvent + 1 / Event::Speed;
+  EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9);
+}
+
+//*****************************************************************************
+TEST(Event, Retract) {
+  Event event, expected(1, 2, 3, 4);
+  Vector4 v;
+  v << 1, 2, 3, 4;
+  EXPECT(assert_equal(expected, event.retract(v)));
+}
+
+//*****************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//*****************************************************************************
+

gtsam_unstable/slam/TOAFactor.h

@@ -0,0 +1,49 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  TOAFactor.h
+ *  @brief "Time of Arrival" factor
+ *  @author Frank Dellaert
+ *  @author Jay Chakravarty
+ *  @date December 2014
+ */
+
+#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
+#include <gtsam_unstable/geometry/Event.h>
+
+namespace gtsam {
+
+/// A "Time of Arrival" factor - so little code seems hardly worth it :-)
+class TOAFactor: public ExpressionFactor<double> {
+
+  typedef Expression<double> double_;
+
+public:
+
+  /**
+   * Constructor
+   * @param some expression yielding an event
+   * @param microphone_ expression yielding a microphone location
+   * @param toaMeasurement time of arrival at microphone
+   * @param model noise model
+   */
+  TOAFactor(const Expression<Event>& eventExpression,
+      const Expression<Point3>& microphone_, double toaMeasurement,
+      const SharedNoiseModel& model) :
+      ExpressionFactor<double>(model, toaMeasurement,
+          double_(&Event::toa, eventExpression, microphone_)) {
+  }
+
+};
+
+} //\ namespace gtsam
+

gtsam_unstable/slam/tests/testTOAFactor.cpp

@@ -0,0 +1,133 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  testTOAFactor.cpp
+ *  @brief Unit tests for "Time of Arrival" factor
+ *  @author Frank Dellaert
+ *  @author Jay Chakravarty
+ *  @date December 2014
+ */
+
+#include <gtsam_unstable/geometry/Event.h>
+#include <gtsam/nonlinear/ExpressionFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/base/numericalDerivative.h>
+
+#include <CppUnitLite/TestHarness.h>
+#include <boost/format.hpp>
+#include <boost/bind.hpp>
+
+using namespace std;
+using namespace gtsam;
+
+// typedefs
+typedef Eigen::Matrix<double, 1, 1> Vector1;
+typedef Expression<double> Double_;
+typedef Expression<Point3> Point3_;
+typedef Expression<Event> Event_;
+
+// units
+static const double ms = 1e-3;
+static const double cm = 1e-2;
+
+// Create a noise model for the TOA error
+static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms));
+
+static const double timeOfEvent = 25;
+static const Event exampleEvent(timeOfEvent, 1, 0, 0);
+static const Point3 microphoneAt0;
+
+//*****************************************************************************
+TEST( TOAFactor, NewWay ) {
+  Key key = 12;
+  Event_ eventExpression(key);
+  Point3_ microphoneConstant(microphoneAt0); // constant expression
+  double measurement = 7;
+  Double_ expression(&Event::toa, eventExpression, microphoneConstant);
+  ExpressionFactor<double> factor(model, measurement, expression);
+}
+
+//*****************************************************************************
+TEST( TOAFactor, WholeEnchilada ) {
+
+  static const bool verbose = false;
+
+  // Create microphones
+  const double height = 0.5;
+  vector<Point3> microphones;
+  microphones.push_back(Point3(0, 0, height));
+  microphones.push_back(Point3(403 * cm, 0, height));
+  microphones.push_back(Point3(403 * cm, 403 * cm, height));
+  microphones.push_back(Point3(0, 403 * cm, 2 * height));
+  EXPECT_LONGS_EQUAL(4, microphones.size());
+//  microphones.push_back(Point3(200 * cm, 200 * cm, height));
+
+  // Create a ground truth point
+  const double timeOfEvent = 0;
+  Event groundTruthEvent(timeOfEvent, 245 * cm, 201.5 * cm, (212 - 45) * cm);
+
+  // Simulate simulatedTOA
+  size_t K = microphones.size();
+  vector<double> simulatedTOA(K);
+  for (size_t i = 0; i < K; i++) {
+    simulatedTOA[i] = groundTruthEvent.toa(microphones[i]);
+    if (verbose) {
+      cout << "mic" << i << " = " << microphones[i] << endl;
+      cout << "z" << i << " = " << simulatedTOA[i] / ms << endl;
+    }
+  }
+
+  // Now, estimate using non-linear optimization
+  ExpressionFactorGraph graph;
+  Key key = 12;
+  Event_ eventExpression(key);
+  for (size_t i = 0; i < K; i++) {
+    Point3_ microphone_i(microphones[i]); // constant expression
+    Double_ predictTOA(&Event::toa, eventExpression, microphone_i);
+    graph.addExpressionFactor(predictTOA, simulatedTOA[i], model);
+  }
+
+  /// Print the graph
+  if (verbose)
+    GTSAM_PRINT(graph);
+
+  // Create initial estimate
+  Values initialEstimate;
+  //Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm);
+  Vector4 delta;
+  delta << 0.1, 0.1, -0.1, 0.1;
+  Event estimatedEvent = groundTruthEvent.retract(delta);
+  initialEstimate.insert(key, estimatedEvent);
+
+  // Print
+  if (verbose)
+    initialEstimate.print("Initial Estimate:\n");
+
+  // Optimize using Levenberg-Marquardt optimization.
+  LevenbergMarquardtParams params;
+  params.setAbsoluteErrorTol(1e-10);
+  if (verbose)
+    params.setVerbosity("ERROR");
+  LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
+  Values result = optimizer.optimize();
+  if (verbose)
+    result.print("Final Result:\n");
+
+  EXPECT(assert_equal(groundTruthEvent, result.at<Event>(key), 1e-6));
+}
+//*****************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//*****************************************************************************
+

matlab/+gtsam/plot3DTrajectory.m

@@ -33,12 +33,12 @@ for i = 0:keys.size-1
                 end
                 gtsam.plotPose3(lastPose, P, scale);
             catch err
-                warning(['no Pose3 at ' lastKey]);
+                % warning(['no Pose3 at ' lastKey]);
+            end
         end
         lastIndex = i;
-        end
     catch
-        warning(['no Pose3 at ' key]);
+        % warning(['no Pose3 at ' key]);
     end
     
     % Draw final pose
@@ -53,7 +53,7 @@ for i = 0:keys.size-1
             end
             gtsam.plotPose3(lastPose, P, scale);
         catch
-            warning(['no Pose3 at ' lastIndex]);
+            % warning(['no Pose3 at ' lastIndex]);
         end
     end