moved gradientAtZero helper to clique class, where it belongs

gtsam/nonlinear/ISAM2.cpp

@@ -1113,6 +1113,8 @@ void ISAM2::updateDelta(bool forceFullSolve) const {
         doglegResult
             .dx_d;  // Copy the VectorValues containing with the linear solution
     gttoc(Copy_dx_d);
+  } else {
+    throw std::runtime_error("iSAM2: unknown ISAM2Params type");
   }
 }
 
@@ -1155,37 +1157,14 @@ double ISAM2::error(const VectorValues& x) const {
   return GaussianFactorGraph(*this).error(x);
 }
 
-/* ************************************************************************* */
-static void gradientAtZeroTreeAdder(const boost::shared_ptr<ISAM2Clique>& root,
-                                    VectorValues* g) {
-  // Loop through variables in each clique, adding contributions
-  DenseIndex variablePosition = 0;
-  for (GaussianConditional::const_iterator jit = root->conditional()->begin();
-       jit != root->conditional()->end(); ++jit) {
-    const DenseIndex dim = root->conditional()->getDim(jit);
-    pair<VectorValues::iterator, bool> pos_ins = g->tryInsert(
-        *jit, root->gradientContribution().segment(variablePosition, dim));
-    if (!pos_ins.second)
-      pos_ins.first->second +=
-          root->gradientContribution().segment(variablePosition, dim);
-    variablePosition += dim;
-  }
-
-  // Recursively add contributions from children
-  typedef boost::shared_ptr<ISAM2Clique> sharedClique;
-  for (const sharedClique& child : root->children) {
-    gradientAtZeroTreeAdder(child, g);
-  }
-}
-
 /* ************************************************************************* */
 VectorValues ISAM2::gradientAtZero() const {
   // Create result
   VectorValues g;
 
   // Sum up contributions for each clique
-  for (const ISAM2::sharedClique& root : this->roots())
-    gradientAtZeroTreeAdder(root, &g);
+  for (const auto& root : this->roots())
+    root->addGradientAtZero(&g);
 
   return g;
 }

gtsam/nonlinear/ISAM2Clique.cpp

@@ -19,7 +19,9 @@
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/linear/linearAlgorithms-inst.h>
 #include <gtsam/nonlinear/ISAM2Clique.h>
+
 #include <stack>
+#include <utility>
 
 using namespace std;
 
@@ -304,7 +306,7 @@ void ISAM2Clique::findAll(const KeySet& markedMask, KeySet* keys) const {
   static const bool debug = false;
   // does the separator contain any of the variables?
   bool found = false;
-  for (Key key : conditional()->parents()) {
+  for (Key key : conditional_->parents()) {
     if (markedMask.exists(key)) {
       found = true;
       break;
@@ -312,14 +314,34 @@ void ISAM2Clique::findAll(const KeySet& markedMask, KeySet* keys) const {
   }
   if (found) {
     // then add this clique
-    keys->insert(conditional()->beginFrontals(), conditional()->endFrontals());
+    keys->insert(conditional_->beginFrontals(), conditional_->endFrontals());
     if (debug) print("Key(s) marked in clique ");
-    if (debug) cout << "so marking key " << conditional()->front() << endl;
+    if (debug) cout << "so marking key " << conditional_->front() << endl;
   }
   for (const auto& child : children) {
     child->findAll(markedMask, keys);
   }
 }
 
+/* ************************************************************************* */
+void ISAM2Clique::addGradientAtZero(VectorValues* g) const {
+  // Loop through variables in each clique, adding contributions
+  DenseIndex position = 0;
+  for (auto it = conditional_->begin(); it != conditional_->end(); ++it) {
+    const DenseIndex dim = conditional_->getDim(it);
+    const Vector contribution = gradientContribution_.segment(position, dim);
+    VectorValues::iterator values_it;
+    bool success;
+    std::tie(values_it, success) = g->tryInsert(*it, contribution);
+    if (!success) values_it->second += contribution;
+    position += dim;
+  }
+
+  // Recursively add contributions from children
+  for (const auto& child : children) {
+    child->addGradientAtZero(g);
+  }
+}
+
 /* ************************************************************************* */
 }  // namespace gtsam

gtsam/nonlinear/ISAM2Clique.h

@@ -75,9 +75,12 @@ class GTSAM_EXPORT ISAM2Clique
   /** Access the cached factor */
   Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
 
-  /** Access the gradient contribution */
+  /// Access the gradient contribution
   const Vector& gradientContribution() const { return gradientContribution_; }
 
+  /// Recursively add gradient at zero to g
+  void addGradientAtZero(VectorValues* g) const;
+
   bool equals(const This& other, double tol = 1e-9) const;
 
   /** print this node */