Implemented value and now testBADFactor also runs

gtsam_unstable/nonlinear/Expression-inl.h

@@ -21,6 +21,7 @@
 
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/Testable.h>
 #include <boost/foreach.hpp>
 
 namespace gtsam {
@@ -44,6 +45,17 @@ private:
 
   typedef std::pair<Key, Matrix> Pair;
 
+  /// Insert terms into jacobians_, premultiplying by H, adding if already exists
+  void add(const JacobianMap& terms) {
+    BOOST_FOREACH(const Pair& term, terms) {
+      JacobianMap::iterator it = jacobians_.find(term.first);
+      if (it != jacobians_.end())
+        it->second += term.second;
+      else
+        jacobians_[term.first] = term.second;
+    }
+  }
+
   /// Insert terms into jacobians_, premultiplying by H, adding if already exists
   void add(const Matrix& H, const JacobianMap& terms) {
     BOOST_FOREACH(const Pair& term, terms) {
@@ -90,7 +102,7 @@ public:
   Augmented(const T& t, const JacobianMap& jacobians1,
       const JacobianMap& jacobians2) :
       value_(t), jacobians_(jacobians1) {
-    jacobians_.insert(jacobians2.begin(), jacobians2.end());
+    add(jacobians2);
   }
 
   /// Construct value, pre-multiply jacobians by H
@@ -143,8 +155,9 @@ protected:
 public:
 
   struct Trace {
+    T t;
     T value() const {
-      return T();
+      return t;
     }
     virtual Augmented<T> augmented(const Matrix& H) const = 0;
   };
@@ -208,11 +221,10 @@ public:
   /// Trace structure for reverse AD
   typedef typename ExpressionNode<T>::Trace BaseTrace;
   struct Trace: public BaseTrace {
-    T t;
     /// Return value and derivatives
     virtual Augmented<T> augmented(const Matrix& H) const {
       // Base case: just return value and empty map
-      return Augmented<T>(t);
+      return Augmented<T>(this->t);
     }
   };
 
@@ -220,6 +232,8 @@ public:
   virtual boost::shared_ptr<BaseTrace> reverse(const Values& values) const {
     boost::shared_ptr<Trace> trace = boost::make_shared<Trace>();
     trace->t = constant_;
+    std::cout << "constant\n:";
+    GTSAM_PRINT(trace->t);
     return trace;
   }
 };
@@ -266,14 +280,12 @@ public:
   /// Trace structure for reverse AD
   typedef typename ExpressionNode<T>::Trace BaseTrace;
   struct Trace: public BaseTrace {
-    T t;
     Key key;
     /// Return value and derivatives
     virtual Augmented<T> augmented(const Matrix& H) const {
-      // This is a top-down calculation
+      // Base case: just insert H in the JacobianMap with correct key
-      // The end-result needs Jacobians to all leaf nodes.
+      std::cout << "Inserting Jacobian " << DefaultKeyFormatter(key) << "\n";
-      // Since this is a leaf node, we are done and just insert H in the JacobianMap
+      return Augmented<T>(this->t, key, H);
-      return Augmented<T>(t, key, H);
     }
   };
 
@@ -282,6 +294,8 @@ public:
     boost::shared_ptr<Trace> trace = boost::make_shared<Trace>();
     trace->t = value(values);
     trace->key = key_;
+    std::cout << "Leaf(" << DefaultKeyFormatter(key_) << "):\n";
+    GTSAM_PRINT(trace->t);
     return trace;
   }
 
@@ -339,14 +353,13 @@ public:
   struct Trace: public BaseTrace {
     boost::shared_ptr<typename ExpressionNode<A>::Trace> trace1;
     Matrix H1;
-    T t;
     /// Return value and derivatives
     virtual Augmented<T> augmented(const Matrix& H) const {
       // This is a top-down calculation
       // The end-result needs Jacobians to all leaf nodes.
       // Since this is not a leaf node, we compute what is needed for leaf nodes here
       Augmented<A> augmented1 = trace1->augmented(H * H1);
-      return Augmented<T>(t, augmented1.jacobians());
+      return Augmented<T>(this->t, augmented1.jacobians());
     }
   };
 
@@ -354,7 +367,10 @@ public:
   virtual boost::shared_ptr<BaseTrace> reverse(const Values& values) const {
     boost::shared_ptr<Trace> trace = boost::make_shared<Trace>();
     trace->trace1 = this->expressionA_->reverse(values);
+    std::cout << "Unary:\n";
+    GTSAM_PRINT(trace->trace1->value());
     trace->t = function_(trace->trace1->value(), trace->H1);
+    GTSAM_PRINT(trace->t);
     return trace;
   }
 };
@@ -424,7 +440,6 @@ public:
     boost::shared_ptr<typename ExpressionNode<A1>::Trace> trace1;
     boost::shared_ptr<typename ExpressionNode<A2>::Trace> trace2;
     Matrix H1, H2;
-    T t;
     /// Return value and derivatives
     virtual Augmented<T> augmented(const Matrix& H) const {
       // This is a top-down calculation
@@ -432,8 +447,9 @@ public:
       // Since this is not a leaf node, we compute what is needed for leaf nodes here
       // The binary node represents a fork in the tree, and hence we will get two Augmented maps
       Augmented<A1> augmented1 = trace1->augmented(H * H1);
-      Augmented<A1> augmented2 = trace1->augmented(H * H2);
+      Augmented<A2> augmented2 = trace2->augmented(H * H2);
-      return Augmented<T>(t, augmented1.jacobians(), augmented2.jacobians());
+      return Augmented<T>(this->t, augmented1.jacobians(),
+          augmented2.jacobians());
     }
   };
 
@@ -442,8 +458,12 @@ public:
     boost::shared_ptr<Trace> trace = boost::make_shared<Trace>();
     trace->trace1 = this->expressionA1_->reverse(values);
     trace->trace2 = this->expressionA2_->reverse(values);
+    std::cout << "Binary:\n";
+    GTSAM_PRINT(trace->trace1->value());
+    GTSAM_PRINT(trace->trace2->value());
     trace->t = function_(trace->trace1->value(), trace->trace2->value(),
         trace->H1, trace->H2);
+    GTSAM_PRINT(trace->t);
     return trace;
   }
 

gtsam_unstable/nonlinear/Expression.h

@@ -103,10 +103,14 @@ public:
 
   /// Return value and derivatives
   Augmented<T> augmented(const Values& values) const {
+#define REVERSE_AD
+#ifdef REVERSE_AD
     boost::shared_ptr<typename ExpressionNode<T>::Trace> trace = root_->reverse(values);
     size_t n = T::Dim();
     return trace->augmented(Eigen::MatrixXd::Identity(n, n));
-//    return root_->forward(values);
+#else
+    return root_->forward(values);
+#endif
   }
 
   const boost::shared_ptr<ExpressionNode<T> >& root() const {

gtsam_unstable/nonlinear/tests/testBADFactor.cpp

@@ -20,6 +20,7 @@
 #include <gtsam_unstable/slam/expressions.h>
 #include <gtsam_unstable/nonlinear/BADFactor.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
+#include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/base/Testable.h>
@@ -29,10 +30,64 @@
 using namespace std;
 using namespace gtsam;
 
-/* ************************************************************************* */
+Point2 measured(-17, 30);
+SharedNoiseModel model = noiseModel::Unit::Create(2);
 
+/* ************************************************************************* */
+// Unary(Leaf))
 TEST(BADFactor, test) {
 
+  // Create some values
+  Values values;
+  values.insert(2, Point3(0, 0, 1));
+
+  JacobianFactor expected( //
+      2, (Matrix(2, 3) << 1, 0, 0, 0, 1, 0), //
+      (Vector(2) << -17, 30));
+
+  // Create leaves
+  Point3_ p(2);
+
+  // Try concise version
+  BADFactor<Point2> f(model, measured, project(p));
+  EXPECT_LONGS_EQUAL(2, f.dim());
+  boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
+  boost::shared_ptr<JacobianFactor> jf = //
+      boost::dynamic_pointer_cast<JacobianFactor>(gf);
+  EXPECT( assert_equal(expected, *jf, 1e-9));
+}
+
+/* ************************************************************************* */
+ // Unary(Binary(Leaf,Leaf))
+ TEST(BADFactor, test1) {
+
+ // Create some values
+ Values values;
+ values.insert(1, Pose3());
+ values.insert(2, Point3(0, 0, 1));
+
+ // Create old-style factor to create expected value and derivatives
+ GenericProjectionFactor<Pose3, Point3> old(measured, model, 1, 2,
+ boost::make_shared<Cal3_S2>());
+ double expected_error = old.error(values);
+ GaussianFactor::shared_ptr expected = old.linearize(values);
+
+ // Create leaves
+ Pose3_ x(1);
+ Point3_ p(2);
+
+ // Try concise version
+ BADFactor<Point2> f2(model, measured, project(transform_to(x, p)));
+ EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
+ EXPECT_LONGS_EQUAL(2, f2.dim());
+ boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
+ EXPECT( assert_equal(*expected, *gf2, 1e-9));
+ }
+
+ /* ************************************************************************* */
+ // Binary(Leaf,Unary(Binary(Leaf,Leaf)))
+ TEST(BADFactor, test2) {
+
  // Create some values
  Values values;
  values.insert(1, Pose3());
@@ -40,15 +95,10 @@ TEST(BADFactor, test) {
  values.insert(3, Cal3_S2());
 
  // Create old-style factor to create expected value and derivatives
-  Point2 measured(-17, 30);
-  SharedNoiseModel model = noiseModel::Unit::Create(2);
  GeneralSFMFactor2<Cal3_S2> old(measured, model, 1, 2, 3);
  double expected_error = old.error(values);
  GaussianFactor::shared_ptr expected = old.linearize(values);
 
-  // Test Constant expression
-  Expression<int> c(0);
-
  // Create leaves
  Pose3_ x(1);
  Point3_ p(2);