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,141 +30,190 @@
 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(1, Pose3());
   values.insert(2, Point3(0, 0, 1));
-  values.insert(3, Cal3_S2());
 
-  // Create old-style factor to create expected value and derivatives
+  JacobianFactor expected( //
-  Point2 measured(-17, 30);
+      2, (Matrix(2, 3) << 1, 0, 0, 0, 1, 0), //
-  SharedNoiseModel model = noiseModel::Unit::Create(2);
+      (Vector(2) << -17, 30));
-  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);
-  Cal3_S2_ K(3);
-
-  // Create expression tree
-  Point3_ p_cam(x, &Pose3::transform_to, p);
-  Point2_ xy_hat(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
-  Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);
-
-  // Create factor and check value, dimension, linearization
-  BADFactor<Point2> f(model, measured, uv_hat);
-  EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);
-  EXPECT_LONGS_EQUAL(2, f.dim());
-  boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
-  EXPECT( assert_equal(*expected, *gf, 1e-9));
 
   // Try concise version
-  BADFactor<Point2> f2(model, measured,
+  BADFactor<Point2> f(model, measured, project(p));
-      uncalibrate(K, project(transform_to(x, p))));
+  EXPECT_LONGS_EQUAL(2, f.dim());
-  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));
-}
-
-/* ************************************************************************* */
-
-TEST(BADFactor, compose1) {
-
-  // Create expression
-  Rot3_ R1(1), R2(2);
-  Rot3_ R3 = R1 * R2;
-
-  // Create factor
-  BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
-
-  // Create some values
-  Values values;
-  values.insert(1, Rot3());
-  values.insert(2, Rot3());
-
-  // Check unwhitenedError
-  std::vector<Matrix> H(2);
-  Vector actual = f.unwhitenedError(values, H);
-  EXPECT( assert_equal(eye(3), H[0],1e-9));
-  EXPECT( assert_equal(eye(3), H[1],1e-9));
-
-  // Check linearization
-  JacobianFactor expected(1, eye(3), 2, eye(3), zero(3));
   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));
+  EXPECT( assert_equal(expected, *jf, 1e-9));
 }
 
 /* ************************************************************************* */
-// Test compose with arguments referring to the same rotation
+ // Unary(Binary(Leaf,Leaf))
-TEST(BADFactor, compose2) {
+ TEST(BADFactor, test1) {
 
-  // Create expression
+ // Create some values
-  Rot3_ R1(1), R2(1);
+ Values values;
-  Rot3_ R3 = R1 * R2;
+ values.insert(1, Pose3());
+ values.insert(2, Point3(0, 0, 1));
 
-  // Create factor
+ // Create old-style factor to create expected value and derivatives
-  BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
+ 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 some values
+ // Create leaves
-  Values values;
+ Pose3_ x(1);
-  values.insert(1, Rot3());
+ Point3_ p(2);
 
-  // Check unwhitenedError
+ // Try concise version
-  std::vector<Matrix> H(1);
+ BADFactor<Point2> f2(model, measured, project(transform_to(x, p)));
-  Vector actual = f.unwhitenedError(values, H);
+ EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
-  EXPECT_LONGS_EQUAL(1, H.size());
+ EXPECT_LONGS_EQUAL(2, f2.dim());
-  EXPECT( assert_equal(2*eye(3), H[0],1e-9));
+ boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
+ EXPECT( assert_equal(*expected, *gf2, 1e-9));
+ }
 
-  // Check linearization
+ /* ************************************************************************* */
-  JacobianFactor expected(1, 2 * eye(3), zero(3));
+ // Binary(Leaf,Unary(Binary(Leaf,Leaf)))
-  boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
+ TEST(BADFactor, test2) {
-  boost::shared_ptr<JacobianFactor> jf = //
-      boost::dynamic_pointer_cast<JacobianFactor>(gf);
-  EXPECT( assert_equal(expected, *jf,1e-9));
-}
 
-/* ************************************************************************* */
+ // Create some values
-// Test compose with one arguments referring to a constant same rotation
+ Values values;
-TEST(BADFactor, compose3) {
+ values.insert(1, Pose3());
+ values.insert(2, Point3(0, 0, 1));
+ values.insert(3, Cal3_S2());
 
-  // Create expression
+ // Create old-style factor to create expected value and derivatives
-  Rot3_ R1(Rot3::identity()), R2(3);
+ GeneralSFMFactor2<Cal3_S2> old(measured, model, 1, 2, 3);
-  Rot3_ R3 = R1 * R2;
+ double expected_error = old.error(values);
+ GaussianFactor::shared_ptr expected = old.linearize(values);
 
-  // Create factor
+ // Create leaves
-  BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
+ Pose3_ x(1);
+ Point3_ p(2);
+ Cal3_S2_ K(3);
 
-  // Create some values
+ // Create expression tree
-  Values values;
+ Point3_ p_cam(x, &Pose3::transform_to, p);
-  values.insert(3, Rot3());
+ Point2_ xy_hat(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
+ Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);
 
-  // Check unwhitenedError
+ // Create factor and check value, dimension, linearization
-  std::vector<Matrix> H(1);
+ BADFactor<Point2> f(model, measured, uv_hat);
-  Vector actual = f.unwhitenedError(values, H);
+ EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);
-  EXPECT_LONGS_EQUAL(1, H.size());
+ EXPECT_LONGS_EQUAL(2, f.dim());
-  EXPECT( assert_equal(eye(3), H[0],1e-9));
+ boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
+ EXPECT( assert_equal(*expected, *gf, 1e-9));
 
-  // Check linearization
+ // Try concise version
-  JacobianFactor expected(3, eye(3), zero(3));
+ BADFactor<Point2> f2(model, measured,
-  boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
+ uncalibrate(K, project(transform_to(x, p))));
-  boost::shared_ptr<JacobianFactor> jf = //
+ EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
-      boost::dynamic_pointer_cast<JacobianFactor>(gf);
+ EXPECT_LONGS_EQUAL(2, f2.dim());
-  EXPECT( assert_equal(expected, *jf,1e-9));
+ boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
-}
+ EXPECT( assert_equal(*expected, *gf2, 1e-9));
+ }
 
-/* ************************************************************************* */
+ /* ************************************************************************* */
+
+ TEST(BADFactor, compose1) {
+
+ // Create expression
+ Rot3_ R1(1), R2(2);
+ Rot3_ R3 = R1 * R2;
+
+ // Create factor
+ BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
+
+ // Create some values
+ Values values;
+ values.insert(1, Rot3());
+ values.insert(2, Rot3());
+
+ // Check unwhitenedError
+ std::vector<Matrix> H(2);
+ Vector actual = f.unwhitenedError(values, H);
+ EXPECT( assert_equal(eye(3), H[0],1e-9));
+ EXPECT( assert_equal(eye(3), H[1],1e-9));
+
+ // Check linearization
+ JacobianFactor expected(1, eye(3), 2, eye(3), zero(3));
+ 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));
+ }
+
+ /* ************************************************************************* */
+ // Test compose with arguments referring to the same rotation
+ TEST(BADFactor, compose2) {
+
+ // Create expression
+ Rot3_ R1(1), R2(1);
+ Rot3_ R3 = R1 * R2;
+
+ // Create factor
+ BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
+
+ // Create some values
+ Values values;
+ values.insert(1, Rot3());
+
+ // Check unwhitenedError
+ std::vector<Matrix> H(1);
+ Vector actual = f.unwhitenedError(values, H);
+ EXPECT_LONGS_EQUAL(1, H.size());
+ EXPECT( assert_equal(2*eye(3), H[0],1e-9));
+
+ // Check linearization
+ JacobianFactor expected(1, 2 * eye(3), zero(3));
+ 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));
+ }
+
+ /* ************************************************************************* */
+ // Test compose with one arguments referring to a constant same rotation
+ TEST(BADFactor, compose3) {
+
+ // Create expression
+ Rot3_ R1(Rot3::identity()), R2(3);
+ Rot3_ R3 = R1 * R2;
+
+ // Create factor
+ BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
+
+ // Create some values
+ Values values;
+ values.insert(3, Rot3());
+
+ // Check unwhitenedError
+ std::vector<Matrix> H(1);
+ Vector actual = f.unwhitenedError(values, H);
+ EXPECT_LONGS_EQUAL(1, H.size());
+ EXPECT( assert_equal(eye(3), H[0],1e-9));
+
+ // Check linearization
+ JacobianFactor expected(3, eye(3), zero(3));
+ 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));
+ }
+
+ /* ************************************************************************* */
 int main() {
   TestResult tr;
   return TestRegistry::runAllTests(tr);