Changed several Impl functions to methods in ISAM2 and ISAM2Clique

gtsam/nonlinear/ISAM2-impl.cpp

@@ -31,27 +31,12 @@ using namespace std;
 
 namespace gtsam {
 
-/* ************************************************************************* */
-void ISAM2::Impl::AddVariables(const Values& newTheta, Values& theta,
-                               VectorValues& delta, VectorValues& deltaNewton,
-                               VectorValues& RgProd,
-                               const KeyFormatter& keyFormatter) {
-  const bool debug = ISDEBUG("ISAM2 AddVariables");
-
-  theta.insert(newTheta);
-  if (debug) newTheta.print("The new variables are: ");
-  // Add zeros into the VectorValues
-  delta.insert(newTheta.zeroVectors());
-  deltaNewton.insert(newTheta.zeroVectors());
-  RgProd.insert(newTheta.zeroVectors());
-}
-
 /* ************************************************************************* */
 void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors,
                                   bool useUnusedSlots,
-                                  NonlinearFactorGraph& nonlinearFactors,
+                                  NonlinearFactorGraph* nonlinearFactors,
-                                  FactorIndices& newFactorIndices) {
+                                  FactorIndices* newFactorIndices) {
-  newFactorIndices.resize(newFactors.size());
+  newFactorIndices->resize(newFactors.size());
 
   if (useUnusedSlots) {
     size_t globalFactorIndex = 0;
@@ -65,44 +50,24 @@ void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors,
         // if so, increase globalFactorIndex.  If the current factor in
         // nonlinearFactors is unused, break out of the loop and use the current
         // slot.
-        if (globalFactorIndex >= nonlinearFactors.size())
+        if (globalFactorIndex >= nonlinearFactors->size())
-          nonlinearFactors.resize(nonlinearFactors.size() + newFactors.size() -
+          nonlinearFactors->resize(nonlinearFactors->size() +
-                                  newFactorIndex);
+                                   newFactors.size() - newFactorIndex);
-        else if (nonlinearFactors[globalFactorIndex])
+        else if ((*nonlinearFactors)[globalFactorIndex])
           ++globalFactorIndex;
         else
           break;
       } while (true);
 
       // Use the current slot, updating nonlinearFactors and newFactorSlots.
-      nonlinearFactors[globalFactorIndex] = newFactors[newFactorIndex];
+      (*nonlinearFactors)[globalFactorIndex] = newFactors[newFactorIndex];
-      newFactorIndices[newFactorIndex] = globalFactorIndex;
+      (*newFactorIndices)[newFactorIndex] = globalFactorIndex;
     }
   } else {
     // We're not looking for unused slots, so just add the factors at the end.
     for (size_t i = 0; i < newFactors.size(); ++i)
-      newFactorIndices[i] = i + nonlinearFactors.size();
+      (*newFactorIndices)[i] = i + nonlinearFactors->size();
-    nonlinearFactors.push_back(newFactors);
+    nonlinearFactors->push_back(newFactors);
-  }
-}
-
-/* ************************************************************************* */
-void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys,
-                                  const FastVector<ISAM2::sharedClique>& roots,
-                                  Values& theta, VariableIndex& variableIndex,
-                                  VectorValues& delta,
-                                  VectorValues& deltaNewton,
-                                  VectorValues& RgProd, KeySet& replacedKeys,
-                                  Base::Nodes& nodes, KeySet& fixedVariables) {
-  variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end());
-  for (Key key : unusedKeys) {
-    delta.erase(key);
-    deltaNewton.erase(key);
-    RgProd.erase(key);
-    replacedKeys.erase(key);
-    nodes.unsafe_erase(key);
-    theta.erase(key);
-    fixedVariables.erase(key);
   }
 }
 
@@ -195,7 +160,7 @@ static void CheckRelinearizationRecursiveMap(
 
 /* ************************************************************************* */
 KeySet ISAM2::Impl::CheckRelinearizationPartial(
-    const FastVector<ISAM2::sharedClique>& roots, const VectorValues& delta,
+    const ISAM2::Roots& roots, const VectorValues& delta,
     const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
   KeySet relinKeys;
   for (const ISAM2::sharedClique& root : roots) {
@@ -210,71 +175,6 @@ KeySet ISAM2::Impl::CheckRelinearizationPartial(
   return relinKeys;
 }
 
-/* ************************************************************************* */
-void ISAM2::Impl::FindAll(ISAM2::sharedClique clique, KeySet& keys,
-                          const KeySet& markedMask) {
-  static const bool debug = false;
-  // does the separator contain any of the variables?
-  bool found = false;
-  for (Key key : clique->conditional()->parents()) {
-    if (markedMask.exists(key)) {
-      found = true;
-      break;
-    }
-  }
-  if (found) {
-    // then add this clique
-    keys.insert(clique->conditional()->beginFrontals(),
-                clique->conditional()->endFrontals());
-    if (debug) clique->print("Key(s) marked in clique ");
-    if (debug)
-      cout << "so marking key " << clique->conditional()->front() << endl;
-  }
-  for (const ISAM2::sharedClique& child : clique->children) {
-    FindAll(child, keys, markedMask);
-  }
-}
-
-/* ************************************************************************* */
-void ISAM2::Impl::ExpmapMasked(const VectorValues& delta, const KeySet& mask,
-                               Values* values,
-                               boost::optional<VectorValues&> invalidateIfDebug,
-                               const KeyFormatter& keyFormatter) {
-  // If debugging, invalidate if requested, otherwise do not invalidate.
-  // Invalidating means setting expmapped entries to Inf, to trigger assertions
-  // if we try to re-use them.
-#ifdef NDEBUG
-  invalidateIfDebug = boost::none;
-#endif
-
-  assert(values->size() == delta.size());
-  Values::iterator key_value;
-  VectorValues::const_iterator key_delta;
-#ifdef GTSAM_USE_TBB
-  for (key_value = values->begin(); key_value != values->end(); ++key_value) {
-    key_delta = delta.find(key_value->key);
-#else
-  for (key_value = values->begin(), key_delta = delta.begin();
-       key_value != values->end(); ++key_value, ++key_delta) {
-    assert(key_value->key == key_delta->first);
-#endif
-    Key var = key_value->key;
-    assert(static_cast<size_t>(delta[var].size()) == key_value->value.dim());
-    assert(delta[var].allFinite());
-    if (mask.exists(var)) {
-      Value* retracted = key_value->value.retract_(delta[var]);
-      key_value->value = *retracted;
-      retracted->deallocate_();
-      if (invalidateIfDebug)
-        (*invalidateIfDebug)[var].operator=(Vector::Constant(
-            delta[var].rows(),
-            numeric_limits<double>::infinity()));  // Strange syntax to work
-                                                   // with clang++ (bug in
-                                                   // clang?)
-    }
-  }
-}
-
 /* ************************************************************************* */
 namespace internal {
 inline static void optimizeInPlace(const ISAM2::sharedClique& clique,
@@ -289,9 +189,10 @@ inline static void optimizeInPlace(const ISAM2::sharedClique& clique,
 }  // namespace internal
 
 /* ************************************************************************* */
-size_t ISAM2::Impl::UpdateGaussNewtonDelta(
+size_t ISAM2::Impl::UpdateGaussNewtonDelta(const ISAM2::Roots& roots,
-    const FastVector<ISAM2::sharedClique>& roots, const KeySet& replacedKeys,
+                                           const KeySet& replacedKeys,
-    double wildfireThreshold, VectorValues* delta) {
+                                           double wildfireThreshold,
+                                           VectorValues* delta) {
   size_t lastBacksubVariableCount;
 
   if (wildfireThreshold <= 0.0) {

gtsam/nonlinear/ISAM2-impl.h

@@ -23,7 +23,6 @@
 namespace gtsam {
 
 struct GTSAM_EXPORT ISAM2::Impl {
-
   struct GTSAM_EXPORT PartialSolveResult {
     ISAM2::sharedClique bayesTree;
   };
@@ -35,32 +34,11 @@ struct GTSAM_EXPORT ISAM2::Impl {
     boost::optional<FastMap<Key, int> > constrainedKeys;
   };
 
-  /**
-   * Add new variables to the ISAM2 system.
-   * @param newTheta Initial values for new variables
-   * @param theta Current solution to be augmented with new initialization
-   * @param delta Current linear delta to be augmented with zeros
-   * @param ordering Current ordering to be augmented with new variables
-   * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables
-   * @param keyFormatter Formatter for printing nonlinear keys during debugging
-   */
-  static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta,
-      VectorValues& deltaNewton, VectorValues& RgProd,
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter);
-
   /// Perform the first part of the bookkeeping updates for adding new factors.  Adds them to the
   /// complete list of nonlinear factors, and populates the list of new factor indices, both
   /// optionally finding and reusing empty factor slots.
   static void AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots,
-    NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices);
+    NonlinearFactorGraph* nonlinearFactors, FactorIndices* newFactorIndices);
-    
-  /**
-   * Remove variables from the ISAM2 system.
-   */
-  static void RemoveVariables(const KeySet& unusedKeys, const FastVector<ISAM2::sharedClique>& roots,
-    Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton,
-    VectorValues& RgProd, KeySet& replacedKeys, Base::Nodes& nodes,
-    KeySet& fixedVariables);
 
   /**
    * Find the set of variables to be relinearized according to relinearizeThreshold.
@@ -85,47 +63,13 @@ struct GTSAM_EXPORT ISAM2::Impl {
    * @return The set of variable indices in delta whose magnitude is greater than or
    * equal to relinearizeThreshold
    */
-  static KeySet CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots,
+  static KeySet CheckRelinearizationPartial(const ISAM2::Roots& roots,
     const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold);
 
-  /**
-   * Recursively search this clique and its children for marked keys appearing
-   * in the separator, and add the *frontal* keys of any cliques whose
-   * separator contains any marked keys to the set \c keys.  The purpose of
-   * this is to discover the cliques that need to be redone due to information
-   * propagating to them from cliques that directly contain factors being
-   * relinearized.
-   *
-   * The original comment says this finds all variables directly connected to
-   * the marked ones by measurements.  Is this true, because it seems like this
-   * would also pull in variables indirectly connected through other frontal or
-   * separator variables?
-   *
-   * Alternatively could we trace up towards the root for each variable here?
-   */
-  static void FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask);
-
-  /**
-   * Apply expmap to the given values, but only for indices appearing in
-   * \c markedRelinMask.  Values are expmapped in-place.
-   * \param delta The linear delta with which to expmap
-   * \param mask Mask on linear indices, only \c true entries are expmapped
-   * \param [in, out] values The value to expmap in-place
-   * \param invalidateIfDebug If this is true, *and* NDEBUG is not defined,
-   * expmapped deltas will be set to an invalid value (infinity) to catch bugs
-   * where we might expmap something twice, or expmap it but then not
-   * recalculate its delta.
-   * @param keyFormatter Formatter for printing nonlinear keys during debugging
-   */
-  static void ExpmapMasked(
-      const VectorValues& delta, const KeySet& mask, Values* values,
-      boost::optional<VectorValues&> invalidateIfDebug = boost::none,
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter);
-
   /**
    * Update the Newton's method step point, using wildfire
    */
-  static size_t UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>& roots,
+  static size_t UpdateGaussNewtonDelta(const ISAM2::Roots& roots,
       const KeySet& replacedKeys, double wildfireThreshold, VectorValues* delta);
 
   /**

gtsam/nonlinear/ISAM2.cpp

@@ -33,6 +33,7 @@ using namespace boost::adaptors;
 }  // namespace br
 
 #include <algorithm>
+#include <limits>
 #include <map>
 #include <utility>
 
@@ -484,6 +485,62 @@ boost::shared_ptr<KeySet> ISAM2::recalculate(
   return affectedKeysSet;
 }
 
+/* ************************************************************************* */
+void ISAM2::addVariables(const Values& newTheta) {
+  const bool debug = ISDEBUG("ISAM2 AddVariables");
+
+  theta_.insert(newTheta);
+  if (debug) newTheta.print("The new variables are: ");
+  // Add zeros into the VectorValues
+  delta_.insert(newTheta.zeroVectors());
+  deltaNewton_.insert(newTheta.zeroVectors());
+  RgProd_.insert(newTheta.zeroVectors());
+}
+
+/* ************************************************************************* */
+void ISAM2::removeVariables(const KeySet& unusedKeys) {
+  variableIndex_.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end());
+  for (Key key : unusedKeys) {
+    delta_.erase(key);
+    deltaNewton_.erase(key);
+    RgProd_.erase(key);
+    deltaReplacedMask_.erase(key);
+    Base::nodes_.unsafe_erase(key);
+    theta_.erase(key);
+    fixedVariables_.erase(key);
+  }
+}
+
+/* ************************************************************************* */
+void ISAM2::expmapMasked(const KeySet& mask) {
+  assert(theta_.size() == delta_.size());
+  Values::iterator key_value;
+  VectorValues::const_iterator key_delta;
+#ifdef GTSAM_USE_TBB
+  for (key_value = theta_.begin(); key_value != theta_.end(); ++key_value) {
+    key_delta = delta_.find(key_value->key);
+#else
+  for (key_value = theta_.begin(), key_delta = delta_.begin();
+       key_value != theta_.end(); ++key_value, ++key_delta) {
+    assert(key_value->key == key_delta->first);
+#endif
+    Key var = key_value->key;
+    assert(static_cast<size_t>(delta_[var].size()) == key_value->value.dim());
+    assert(delta_[var].allFinite());
+    if (mask.exists(var)) {
+      Value* retracted = key_value->value.retract_(delta_[var]);
+      key_value->value = *retracted;
+      retracted->deallocate_();
+#ifndef NDEBUG
+      // If debugging, invalidate delta_ entries to Inf, to trigger assertions
+      // if we try to re-use them.
+      delta_[var] = Vector::Constant(delta_[var].rows(),
+                                     numeric_limits<double>::infinity());
+#endif
+    }
+  }
+}
+
 /* ************************************************************************* */
 ISAM2Result ISAM2::update(
     const NonlinearFactorGraph& newFactors, const Values& newTheta,
@@ -529,7 +586,7 @@ ISAM2Result ISAM2::update(
   // Add the new factor indices to the result struct
   if (debug || verbose) newFactors.print("The new factors are: ");
   Impl::AddFactorsStep1(newFactors, params_.findUnusedFactorSlots,
-                        nonlinearFactors_, result.newFactorsIndices);
+                        &nonlinearFactors_, &result.newFactorsIndices);
 
   // Remove the removed factors
   NonlinearFactorGraph removeFactors;
@@ -571,7 +628,7 @@ ISAM2Result ISAM2::update(
   gttic(add_new_variables);
   // 2. Initialize any new variables \Theta_{new} and add
   // \Theta:=\Theta\cup\Theta_{new}.
-  Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_);
+  addVariables(newTheta);
   // New keys for detailed results
   if (params_.enableDetailedResults) {
     for (Key key : newTheta.keys()) {
@@ -669,13 +726,13 @@ ISAM2Result ISAM2::update(
     if (!relinKeys.empty()) {
       for (const sharedClique& root : roots_)
         // add other cliques that have the marked ones in the separator
-        Impl::FindAll(root, markedKeys, markedRelinMask);
+        root->findAll(markedRelinMask, &markedKeys);
 
       // Relin involved keys for detailed results
       if (params_.enableDetailedResults) {
         KeySet involvedRelinKeys;
         for (const sharedClique& root : roots_)
-          Impl::FindAll(root, involvedRelinKeys, markedRelinMask);
+          root->findAll(markedRelinMask, &involvedRelinKeys);
         for (Key key : involvedRelinKeys) {
           if (!result.detail->variableStatus[key].isAboveRelinThreshold) {
             result.detail->variableStatus[key].isRelinearizeInvolved = true;
@@ -689,8 +746,7 @@ ISAM2Result ISAM2::update(
     gttic(expmap);
     // 6. Update linearization point for marked variables:
     // \Theta_{J}:=\Theta_{J}+\Delta_{J}.
-    if (!relinKeys.empty())
+    if (!relinKeys.empty()) expmapMasked(markedRelinMask);
-      Impl::ExpmapMasked(delta_, markedRelinMask, &theta_, delta_);
     gttoc(expmap);
 
     result.variablesRelinearized = markedKeys.size();
@@ -740,9 +796,7 @@ ISAM2Result ISAM2::update(
   // Update data structures to remove unused keys
   if (!unusedKeys.empty()) {
     gttic(remove_variables);
-    Impl::RemoveVariables(unusedKeys, roots_, theta_, variableIndex_, delta_,
+    removeVariables(unusedKeys);
-                          deltaNewton_, RgProd_, deltaReplacedMask_,
-                          Base::nodes_, fixedVariables_);
     gttoc(remove_variables);
   }
   result.cliques = this->nodes().size();
@@ -998,9 +1052,7 @@ void ISAM2::marginalizeLeaves(
                                   factorIndicesToRemove.end());
 
   // Remove the marginalized variables
-  Impl::RemoveVariables(KeySet(leafKeys.begin(), leafKeys.end()), roots_,
+  removeVariables(KeySet(leafKeys.begin(), leafKeys.end()));
-                        theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
-                        deltaReplacedMask_, nodes_, fixedVariables_);
 }
 
 /* ************************************************************************* */

gtsam/nonlinear/ISAM2.h

@@ -269,6 +269,29 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
   /// @}
 
  protected:
+  /**
+   * Add new variables to the ISAM2 system.
+   * @param newTheta Initial values for new variables
+   * @param theta Current solution to be augmented with new initialization
+   * @param delta Current linear delta to be augmented with zeros
+   * @param deltaNewton 
+   * @param RgProd
+   * @param keyFormatter Formatter for printing nonlinear keys during debugging
+   */
+  void addVariables(const Values& newTheta);
+
+  /**
+   * Remove variables from the ISAM2 system.
+   */
+  void removeVariables(const KeySet& unusedKeys);
+
+  /**
+   * Apply expmap to the given values, but only for indices appearing in
+   * \c mask.  Values are expmapped in-place.
+   * \param mask Mask on linear indices, only \c true entries are expmapped
+   */
+  void expmapMasked(const KeySet& mask);
+
   FastSet<Key> getAffectedFactors(const FastList<Key>& keys) const;
   GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(
       const FastList<Key>& affectedKeys, const KeySet& relinKeys) const;
@@ -279,6 +302,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
       const std::vector<Key>& observedKeys, const KeySet& unusedIndices,
       const boost::optional<FastMap<Key, int> >& constrainKeys,
       ISAM2Result& result);
+
   void updateDelta(bool forceFullSolve = false) const;
 };  // ISAM2
 

gtsam/nonlinear/ISAM2Clique.cpp

@@ -15,10 +15,10 @@
  * @author  Michael Kaess, Richard Roberts, Frank Dellaert
  */
 
-#include <gtsam/nonlinear/ISAM2Clique.h>
+#include <gtsam/inference/BayesTreeCliqueBase-inst.h>
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/linear/linearAlgorithms-inst.h>
-#include <gtsam/inference/BayesTreeCliqueBase-inst.h>
+#include <gtsam/nonlinear/ISAM2Clique.h>
 #include <stack>
 
 using namespace std;
@@ -299,4 +299,27 @@ size_t ISAM2Clique::calculate_nnz() const {
   return result;
 }
 
+/* ************************************************************************* */
+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()) {
+    if (markedMask.exists(key)) {
+      found = true;
+      break;
+    }
+  }
+  if (found) {
+    // then add this clique
+    keys->insert(conditional()->beginFrontals(), conditional()->endFrontals());
+    if (debug) print("Key(s) marked in clique ");
+    if (debug) cout << "so marking key " << conditional()->front() << endl;
+  }
+  for (const auto& child : children) {
+    child->findAll(markedMask, keys);
+  }
+}
+
+/* ************************************************************************* */
 }  // namespace gtsam

gtsam/nonlinear/ISAM2Clique.h

@@ -19,11 +19,11 @@
 
 #pragma once
 
+#include <gtsam/inference/BayesTreeCliqueBase.h>
+#include <gtsam/inference/Key.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianConditional.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/inference/BayesTreeCliqueBase.h>
-#include <gtsam/inference/Key.h>
 #include <string>
 
 namespace gtsam {
@@ -99,6 +99,23 @@ class GTSAM_EXPORT ISAM2Clique
   void nnz_internal(size_t* result) const;
   size_t calculate_nnz() const;
 
+  /**
+   * Recursively search this clique and its children for marked keys appearing
+   * in the separator, and add the *frontal* keys of any cliques whose
+   * separator contains any marked keys to the set \c keys.  The purpose of
+   * this is to discover the cliques that need to be redone due to information
+   * propagating to them from cliques that directly contain factors being
+   * relinearized.
+   *
+   * The original comment says this finds all variables directly connected to
+   * the marked ones by measurements.  Is this true, because it seems like this
+   * would also pull in variables indirectly connected through other frontal or
+   * separator variables?
+   *
+   * Alternatively could we trace up towards the root for each variable here?
+   */
+  void findAll(const KeySet& markedMask, KeySet* keys) const;
+
  private:
   /**
    * Check if clique was replaced, or if any parents were changed above the

tests/testGaussianISAM2.cpp

@@ -304,7 +304,7 @@ TEST(ISAM2, AddFactorsStep1)
 
   FactorIndices actualNewFactorIndices;
 
-  ISAM2::Impl::AddFactorsStep1(newFactors, true, nonlinearFactors, actualNewFactorIndices);
+  ISAM2::Impl::AddFactorsStep1(newFactors, true, &nonlinearFactors, &actualNewFactorIndices);
 
   EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors));
   EXPECT(assert_container_equality(expectedNewFactorIndices, actualNewFactorIndices));