Option to reuse old factor slots in ISAM2

gtsam/nonlinear/ISAM2-impl.cpp

@@ -42,6 +42,47 @@ void ISAM2::Impl::AddVariables(
   RgProd.insert(newTheta.zeroVectors());
 }
 
+/* ************************************************************************* */
+void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots,
+  NonlinearFactorGraph& nonlinearFactors, FastVector<size_t>& newFactorIndices)
+{
+  newFactorIndices.resize(newFactors.size());
+
+  if(useUnusedSlots)
+  {
+    size_t globalFactorIndex = 0;
+    for(size_t newFactorIndex = 0; newFactorIndex < newFactors.size(); ++newFactorIndex)
+    {
+      // Loop to find the next available factor slot
+      do 
+      {
+        // If we need to add more factors than we have room for, resize nonlinearFactors,
+        // filling the new slots with NULL factors.  Otherwise, check if the current
+        // factor in nonlinearFactors is already used, and 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())
+          nonlinearFactors.resize(nonlinearFactors.size() + newFactors.size() - newFactorIndex);
+        else if(nonlinearFactors[globalFactorIndex])
+          ++ globalFactorIndex;
+        else
+          break;
+      } while(true);
+
+      // Use the current slot, updating nonlinearFactors and newFactorSlots.
+      nonlinearFactors[globalFactorIndex] = newFactors[newFactorIndex];
+      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();
+    nonlinearFactors.push_back(newFactors);
+  }
+}
+
 /* ************************************************************************* */
 void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const FastVector<ISAM2::sharedClique>& roots,
                                   Values& theta, VariableIndex& variableIndex,

gtsam/nonlinear/ISAM2-impl.h

@@ -47,6 +47,12 @@ struct GTSAM_EXPORT ISAM2::Impl {
   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, FastVector<size_t>& newFactorIndices);
     
   /**
    * Remove variables from the ISAM2 system.

gtsam/nonlinear/ISAM2.cpp

@@ -550,14 +550,10 @@ ISAM2Result ISAM2::update(
   }
 
   gttic(push_back_factors);
-  // Add the new factor indices to the result struct
-  result.newFactorsIndices.resize(newFactors.size());
-  for(size_t i=0; i<newFactors.size(); ++i)
-    result.newFactorsIndices[i] = i + nonlinearFactors_.size();
-
   // 1. Add any new factors \Factors:=\Factors\cup\Factors'.
+  // Add the new factor indices to the result struct
   if(debug || verbose) newFactors.print("The new factors are: ");
-  nonlinearFactors_.push_back(newFactors);
+  Impl::AddFactorsStep1(newFactors, params_.findUnusedFactorSlots, nonlinearFactors_, result.newFactorsIndices);
 
   // Remove the removed factors
   NonlinearFactorGraph removeFactors; removeFactors.reserve(removeFactorIndices.size());
@@ -707,7 +703,16 @@ ISAM2Result ISAM2::update(
   if(params_.cacheLinearizedFactors) {
     gttic(linearize);
     GaussianFactorGraph::shared_ptr linearFactors = newFactors.linearize(theta_);
-    linearFactors_.push_back(*linearFactors);
+    if(params_.findUnusedFactorSlots)
+    {
+      linearFactors_.resize(nonlinearFactors_.size());
+      for(size_t newFactorI = 0; newFactorI < newFactors.size(); ++newFactorI)
+        linearFactors_[result.newFactorsIndices[newFactorI]] = (*linearFactors)[newFactorI];
+    }
+    else
+    {
+      linearFactors_.push_back(*linearFactors);
+    }
     assert(nonlinearFactors_.size() == linearFactors_.size());
     gttoc(linearize);
   }
@@ -715,7 +720,10 @@ ISAM2Result ISAM2::update(
 
   gttic(augment_VI);
   // Augment the variable index with the new factors
-  variableIndex_.augment(newFactors);
+  if(params_.findUnusedFactorSlots)
+    variableIndex_.augment(newFactors, result.newFactorsIndices);
+  else
+    variableIndex_.augment(newFactors);
   gttoc(augment_VI);
 
   gttic(recalculate);
@@ -729,9 +737,7 @@ ISAM2Result ISAM2::update(
     deltaReplacedMask_.insert(replacedKeys->begin(), replacedKeys->end());
   gttoc(recalculate);
 
-  // After the top of the tree has been redone and may have index gaps from
-  // unused keys, condense the indices to remove gaps by rearranging indices
-  // in all data structures.
+  // Update data structures to remove unused keys
   if(!unusedKeys.empty()) {
     gttic(remove_variables);
     Impl::RemoveVariables(unusedKeys, roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,

gtsam/nonlinear/ISAM2.h

@@ -164,6 +164,11 @@ struct GTSAM_EXPORT ISAM2Params {
    */
   bool enablePartialRelinearizationCheck;
 
+  /// When you will be removing many factors, e.g. when using ISAM2 as a fixed-lag smoother, enable this option to
+  /// add factors in the first available factor slots, to avoid accumulating NULL factor slots, at the cost of
+  /// having to search for slots every time a factor is added.
+  bool findUnusedFactorSlots;
+
   /** Specify parameters as constructor arguments */
   ISAM2Params(
       OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params::optimizationParams
@@ -178,7 +183,8 @@ struct GTSAM_EXPORT ISAM2Params {
       relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization),
       evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization),
       cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter),
-      enableDetailedResults(false), enablePartialRelinearizationCheck(false) {}
+      enableDetailedResults(false), enablePartialRelinearizationCheck(false),
+      findUnusedFactorSlots(false) {}
 
   void print(const std::string& str = "") const {
     std::cout << str << "\n";
@@ -204,6 +210,7 @@ struct GTSAM_EXPORT ISAM2Params {
     std::cout << "cacheLinearizedFactors:            " << cacheLinearizedFactors << "\n";
     std::cout << "enableDetailedResults:             " << enableDetailedResults << "\n";
     std::cout << "enablePartialRelinearizationCheck: " << enablePartialRelinearizationCheck << "\n";
+    std::cout << "findUnusedFactorSlots:             " << findUnusedFactorSlots << "\n";
     std::cout.flush();
   }
 
@@ -301,7 +308,7 @@ struct GTSAM_EXPORT ISAM2Result {
    * factors passed as \c newFactors to ISAM2::update().  These indices may be
    * used later to refer to the factors in order to remove them.
    */
-  std::vector<size_t> newFactorsIndices;
+  FastVector<size_t> newFactorsIndices;
 
   /** A struct holding detailed results, which must be enabled with
    * ISAM2Params::enableDetailedResults.

tests/testGaussianISAM2.cpp

@@ -284,6 +284,34 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c
   return nodeGradientsOk && expectedGradOk && totalGradOk && isamEqual && isamTreeEqual && consistent;
 }
 
+/* ************************************************************************* */
+TEST(ISAM2, AddFactorsStep1)
+{
+  NonlinearFactorGraph nonlinearFactors;
+  nonlinearFactors += PriorFactor<LieVector>(10, LieVector(), gtsam::SharedNoiseModel());
+  nonlinearFactors += NonlinearFactor::shared_ptr();
+  nonlinearFactors += PriorFactor<LieVector>(11, LieVector(), gtsam::SharedNoiseModel());
+
+  NonlinearFactorGraph newFactors;
+  newFactors += PriorFactor<LieVector>(1, LieVector(), gtsam::SharedNoiseModel());
+  newFactors += PriorFactor<LieVector>(2, LieVector(), gtsam::SharedNoiseModel());
+
+  NonlinearFactorGraph expectedNonlinearFactors;
+  expectedNonlinearFactors += PriorFactor<LieVector>(10, LieVector(), gtsam::SharedNoiseModel());
+  expectedNonlinearFactors += PriorFactor<LieVector>(1, LieVector(), gtsam::SharedNoiseModel());
+  expectedNonlinearFactors += PriorFactor<LieVector>(11, LieVector(), gtsam::SharedNoiseModel());
+  expectedNonlinearFactors += PriorFactor<LieVector>(2, LieVector(), gtsam::SharedNoiseModel());
+
+  const FastVector<size_t> expectedNewFactorIndices = list_of(1)(3);
+
+  FastVector<size_t> actualNewFactorIndices;
+
+  ISAM2::Impl::AddFactorsStep1(newFactors, true, nonlinearFactors, actualNewFactorIndices);
+
+  EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors));
+  EXPECT(assert_container_equality(expectedNewFactorIndices, actualNewFactorIndices));
+}
+
 /* ************************************************************************* */
 TEST(ISAM2, simple)
 {