Slight refactor

gtsam_unstable/nonlinear/BADFactor.h

@@ -50,14 +50,9 @@ public:
       boost::optional<std::vector<Matrix>&> H = boost::none) const {
     if (H) {
       assert(H->size()==size());
-      typedef std::map<Key, Matrix> MapType;
-      MapType terms;
       Augmented<T> augmented = expression_.augmented(x);
       // move terms to H, which is pre-allocated to correct size
-      size_t j = 0;
-      MapType::iterator it = augmented.jacobians().begin();
-      for (; it != augmented.jacobians().end(); ++it)
-        it->second.swap((*H)[j++]);
+      augmented.move(*H);
       return measurement_.localCoordinates(augmented.value());
     } else {
       const T& value = expression_.value(x);

gtsam_unstable/nonlinear/Expression-inl.h

@@ -244,6 +244,16 @@ public:
           << "x" << term.second.cols() << ") ";
     std::cout << std::endl;
   }
+
+  /// Move terms to array, destroys content
+  void move(std::vector<Matrix>& H) {
+    assert(H.size()==jacobains.size());
+    size_t j = 0;
+    JacobianMap::iterator it = jacobians_.begin();
+    for (; it != jacobians_.end(); ++it)
+      it->second.swap(H[j++]);
+  }
+
 };
 
 //-----------------------------------------------------------------------------