Passing JacobianMap as an argument now..

gtsam_unstable/nonlinear/Expression-inl.h

@@ -45,6 +45,16 @@ class Expression;
 
 typedef std::map<Key, Matrix> JacobianMap;
 
+/// Move terms to array, destroys content
+void move(JacobianMap& jacobians, std::vector<Matrix>& H) {
+  assert(H.size()==jacobians.size());
+  size_t j = 0;
+  JacobianMap::iterator it = jacobians.begin();
+  for (; it != jacobians.end(); ++it)
+    it->second.swap(H[j++]);
+}
+
+
 //-----------------------------------------------------------------------------
 /**
  * The CallRecord class stores the Jacobians of applying a function
@@ -370,11 +380,7 @@ public:
 
   /// Move terms to array, destroys content
   void move(std::vector<Matrix>& H) {
-    assert(H.size()==jacobians_.size());
+    move(jacobians_, H);
-    size_t j = 0;
-    JacobianMap::iterator it = jacobians_.begin();
-    for (; it != jacobians_.end(); ++it)
-      it->second.swap(H[j++]);
   }
 
 };

gtsam_unstable/nonlinear/Expression.h

@@ -108,14 +108,11 @@ public:
     return root_->keys();
   }
 
-  /// Return value and optional derivatives
-  T value(const Values& values) const {
-    return root_->value(values);
-  }
-
   /// Return value and derivatives, forward AD version
-  Augmented<T> forward(const Values& values) const {
+  T forward(const Values& values, JacobianMap& jacobians) const {
-    return root_->forward(values);
+    Augmented<T> augmented = root_->forward(values);
+    jacobians = augmented.jacobians();
+    return augmented.value();
   }
 
   // Return size needed for memory buffer in traceExecution
@@ -130,22 +127,26 @@ public:
   }
 
   /// Return value and derivatives, reverse AD version
-  Augmented<T> reverse(const Values& values) const {
+  T reverse(const Values& values, JacobianMap& jacobians) const {
     size_t size = traceSize();
     char raw[size];
     ExecutionTrace<T> trace;
     T value(traceExecution(values, trace, raw));
-    Augmented<T> augmented(value);
+    trace.startReverseAD(jacobians);
-    trace.startReverseAD(augmented.jacobians());
+    return value;
-    return augmented;
+  }
+
+  /// Return value
+  T value(const Values& values) const {
+    return root_->value(values);
   }
 
   /// Return value and derivatives
-  Augmented<T> augmented(const Values& values) const {
+  T value(const Values& values, JacobianMap& jacobians) const {
 #ifdef EXPRESSION_FORWARD_AD
-    return forward(values);
+    return forward(values, jacobians);
 #else
-    return reverse(values);
+    return reverse(values, jacobians);
 #endif
   }
 

gtsam_unstable/nonlinear/ExpressionFactor.h

@@ -50,10 +50,11 @@ public:
       boost::optional<std::vector<Matrix>&> H = boost::none) const {
     if (H) {
       assert(H->size()==size());
-      Augmented<T> augmented = expression_.augmented(x);
+      JacobianMap jacobians;
+      T value = expression_.value(x, jacobians);
       // move terms to H, which is pre-allocated to correct size
-      augmented.move(*H);
+      move(jacobians, *H);
-      return measurement_.localCoordinates(augmented.value());
+      return measurement_.localCoordinates(value);
     } else {
       const T& value = expression_.value(x);
       return measurement_.localCoordinates(value);
@@ -67,15 +68,15 @@ public:
       return boost::shared_ptr<JacobianFactor>();
 
     // Evaluate error to get Jacobians and RHS vector b
-    Augmented<T> augmented = expression_.augmented(x);
+    JacobianMap terms;
-    Vector b = -measurement_.localCoordinates(augmented.value());
+    T value = expression_.value(x, terms);
+    Vector b = -measurement_.localCoordinates(value);
     // check(noiseModel_, b); // TODO: use, but defined in NonlinearFactor.cpp
 
     // Whiten the corresponding system now
     // TODO ! this->noiseModel_->WhitenSystem(A, b);
 
     // Terms, needed to create JacobianFactor below, are already here!
-    const JacobianMap& terms = augmented.jacobians();
     size_t n = terms.size();
 
     // Get dimensions of matrices

gtsam_unstable/nonlinear/tests/testExpression.cpp

@@ -44,10 +44,11 @@ static const Rot3 someR = Rot3::RzRyRx(1, 2, 3);
 TEST(Expression, constant) {
   Expression<Rot3> R(someR);
   Values values;
-  Augmented<Rot3> a = R.augmented(values);
+  JacobianMap actualMap;
-  EXPECT(assert_equal(someR, a.value()));
+  Rot3 actual = R.value(values, actualMap);
+  EXPECT(assert_equal(someR, actual));
   JacobianMap expected;
-  EXPECT(a.jacobians() == expected);
+  EXPECT(actualMap == expected);
 }
 
 /* ************************************************************************* */
@@ -56,11 +57,19 @@ TEST(Expression, leaf) {
   Expression<Rot3> R(100);
   Values values;
   values.insert(100, someR);
-  Augmented<Rot3> a = R.augmented(values);
+
-  EXPECT(assert_equal(someR, a.value()));
   JacobianMap expected;
   expected[100] = eye(3);
-  EXPECT(a.jacobians() == expected);
+
+  JacobianMap actualMap1;
+  Rot3 actual1 = R.forward(values, actualMap1);
+  EXPECT(assert_equal(someR, actual1));
+  EXPECT(actualMap1 == expected);
+
+  JacobianMap actualMap2;
+  Rot3 actual2 = R.reverse(values, actualMap2);
+  EXPECT(assert_equal(someR, actual2));
+  EXPECT(actualMap2 == expected);
 }
 
 /* ************************************************************************* */