Merge pull request #1172 from gradyrw/fix/iSAM2

gtsam/nonlinear/ISAM2.cpp

@@ -552,9 +552,12 @@ void ISAM2::marginalizeLeaves(
         // We do not need the marginal factors associated with this clique
         // because their information is already incorporated in the new
         // marginal factor.  So, now associate this marginal factor with the
-        // parent of this clique.
-        marginalFactors[clique->parent()->conditional()->front()].push_back(
-            marginalFactor);
+        // parent of this clique. If the clique is a root and has no parent, then
+        // we can discard it without keeping track of the marginal factor.
+        if (clique->parent()) {
+            marginalFactors[clique->parent()->conditional()->front()].push_back(
+                    marginalFactor);
+        }
         // Now remove this clique and its subtree - all of its marginal
         // information has been stored in marginalFactors.
         trackingRemoveSubtree(clique);
@@ -632,7 +635,7 @@ void ISAM2::marginalizeLeaves(
 
         // Make the clique's matrix appear as a subset
         const DenseIndex dimToRemove = cg->matrixObject().offset(nToRemove);
-        cg->matrixObject().firstBlock() = nToRemove;
+        cg->matrixObject().firstBlock() += nToRemove;
         cg->matrixObject().rowStart() = dimToRemove;
 
         // Change the keys in the clique
@@ -658,42 +661,55 @@ void ISAM2::marginalizeLeaves(
   // At this point we have updated the BayesTree, now update the remaining iSAM2
   // data structures
 
+  // Remove the factors to remove that will be summarized in marginal factors
+  NonlinearFactorGraph removedFactors;
+  for (const auto index : factorIndicesToRemove) {
+    removedFactors.push_back(nonlinearFactors_[index]);
+    nonlinearFactors_.remove(index);
+    if (params_.cacheLinearizedFactors) {
+      linearFactors_.remove(index);
+    }
+  }
+  variableIndex_.remove(factorIndicesToRemove.begin(),
+                        factorIndicesToRemove.end(), removedFactors);
+
   // Gather factors to add - the new marginal factors
-  GaussianFactorGraph factorsToAdd;
+  GaussianFactorGraph factorsToAdd{};
+  NonlinearFactorGraph nonlinearFactorsToAdd{};
   for (const auto& key_factors : marginalFactors) {
     for (const auto& factor : key_factors.second) {
       if (factor) {
         factorsToAdd.push_back(factor);
-        if (marginalFactorsIndices)
-          marginalFactorsIndices->push_back(nonlinearFactors_.size());
-        nonlinearFactors_.push_back(
-            boost::make_shared<LinearContainerFactor>(factor));
-        if (params_.cacheLinearizedFactors) linearFactors_.push_back(factor);
+        nonlinearFactorsToAdd.emplace_shared<LinearContainerFactor>(factor);
         for (Key factorKey : *factor) {
           fixedVariables_.insert(factorKey);
         }
       }
     }
   }
-  variableIndex_.augment(factorsToAdd);  // Augment the variable index
-
-  // Remove the factors to remove that have been summarized in the newly-added
-  // marginal factors
-  NonlinearFactorGraph removedFactors;
-  for (const auto index : factorIndicesToRemove) {
-    removedFactors.push_back(nonlinearFactors_[index]);
-    nonlinearFactors_.remove(index);
-    if (params_.cacheLinearizedFactors) linearFactors_.remove(index);
+  // Add the nonlinear factors and keep track of the new factor indices
+  auto newFactorIndices = nonlinearFactors_.add_factors(nonlinearFactorsToAdd,
+                                                        params_.findUnusedFactorSlots);
+  // Add cached linear factors.
+  if (params_.cacheLinearizedFactors){
+    linearFactors_.resize(nonlinearFactors_.size());
+    for (std::size_t i = 0; i < nonlinearFactorsToAdd.size(); ++i){
+      linearFactors_[newFactorIndices[i]] = factorsToAdd[i];
+    }
   }
-  variableIndex_.remove(factorIndicesToRemove.begin(),
-                        factorIndicesToRemove.end(), removedFactors);
-
-  if (deletedFactorsIndices)
-    deletedFactorsIndices->assign(factorIndicesToRemove.begin(),
-                                  factorIndicesToRemove.end());
+  // Augment the variable index
+  variableIndex_.augment(factorsToAdd, newFactorIndices);
 
   // Remove the marginalized variables
   removeVariables(KeySet(leafKeys.begin(), leafKeys.end()));
+
+  if (deletedFactorsIndices) {
+    deletedFactorsIndices->assign(factorIndicesToRemove.begin(),
+                                  factorIndicesToRemove.end());
+  }
+  if (marginalFactorsIndices){
+    *marginalFactorsIndices = std::move(newFactorIndices);
+  }
 }
 
 /* ************************************************************************* */

tests/testGaussianISAM2.cpp

@@ -714,16 +714,17 @@ namespace {
     const boost::optional<FastMap<Key, int>> orderingConstraints = createOrderingConstraints(isam, newValues.keys(), marginalizableKeys);
 
     // Mark additional keys between the marginalized keys and the leaves
-    KeyList additionalMarkedKeys;
+    KeyList markedKeys;
     for (Key key : marginalizableKeys) {
+      markedKeys.push_back(key);
       ISAM2Clique::shared_ptr clique = isam[key];
       for (const ISAM2Clique::shared_ptr& child : clique->children) {
-        markAffectedKeys(key, child, additionalMarkedKeys);
+        markAffectedKeys(key, child, markedKeys);
       }
     }
 
     // Update
-    isam.update(newFactors, newValues, FactorIndices{}, orderingConstraints, boost::none, additionalMarkedKeys);
+    isam.update(newFactors, newValues, FactorIndices{}, orderingConstraints, boost::none, markedKeys);
 
     if (!marginalizableKeys.empty()) {
       FastList<Key> leafKeys(marginalizableKeys.begin(), marginalizableKeys.end());
@@ -945,6 +946,37 @@ TEST(ISAM2, MarginalizeRoot)
   EXPECT(estimate.empty());
 }
 
+/* ************************************************************************* */
+TEST(ISAM2, marginalizationSize)
+{
+  const boost::shared_ptr<noiseModel::Isotropic> nm = noiseModel::Isotropic::Sigma(6, 1.0);
+
+  NonlinearFactorGraph factors;
+  Values values;
+  ISAM2Params params;
+  params.findUnusedFactorSlots = true;
+  ISAM2 isam{params};
+
+  // Create a pose variable
+  Key aKey(0);
+  values.insert(aKey, Pose3::identity());
+  factors.addPrior(aKey, Pose3::identity(), nm);
+  // Create another pose variable linked to the first
+  Pose3 b = Pose3::identity();
+  Key bKey(1);
+  values.insert(bKey, Pose3::identity());
+  factors.emplace_shared<BetweenFactor<Pose3>>(aKey, bKey, Pose3::identity(), nm);
+  // Optimize the graph
+  EXPECT(updateAndMarginalize(factors, values, {}, isam));
+
+  // Now remove a variable -> we should not see the number of factors increase
+  gtsam::KeySet to_remove;
+  to_remove.insert(aKey);
+  const auto numFactorsBefore = isam.getFactorsUnsafe().size();
+  updateAndMarginalize({}, {}, to_remove, isam);
+  EXPECT(numFactorsBefore == isam.getFactorsUnsafe().size());
+}
+
 /* ************************************************************************* */
 TEST(ISAM2, marginalCovariance)
 {