From 86d1d42c369f76fb160b8e8389c674508cc5aa07 Mon Sep 17 00:00:00 2001
From: Enrique Fernandez <efernandez@clearpathrobotics.com>
Date: Wed, 3 Aug 2016 15:27:17 -0400
Subject: [PATCH 1/2] Fix CCOLAMD base cases for 0 and 1 variables

In both cases there's no need to find out any ordering at all:
- For 0 variables, an empty Ordering is returned.
- For 1 variable, an Ordering with that 1 variable is returned.
---
 gtsam/inference/Ordering.cpp | 13 ++++++++++++-
 1 file changed, 12 insertions(+), 1 deletion(-)

diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp
index e12c32df3..b94f01689 100644
--- a/gtsam/inference/Ordering.cpp
+++ b/gtsam/inference/Ordering.cpp
@@ -53,8 +53,19 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
   gttic(Ordering_COLAMDConstrained);
 
   gttic(Prepare);
+  const size_t nVars = variableIndex.size();
+  if (nVars == 0)
+  {
+    return Ordering();
+  }
+
+  if (nVars == 1)
+  {
+    return Ordering(std::vector<Key>(1, variableIndex.begin()->first));
+  }
+
   const size_t nEntries = variableIndex.nEntries(), nFactors =
-      variableIndex.nFactors(), nVars = variableIndex.size();
+      variableIndex.nFactors();
   // Convert to compressed column major format colamd wants it in (== MATLAB format!)
   const size_t Alen = ccolamd_recommended((int) nEntries, (int) nFactors,
       (int) nVars); /* colamd arg 3: size of the array A */

From 2fdb6ce48f02fabfd4138aa0e3006fe088988dd1 Mon Sep 17 00:00:00 2001
From: Enrique Fernandez <efernandez@clearpathrobotics.com>
Date: Wed, 3 Aug 2016 15:41:27 -0400
Subject: [PATCH 2/2] Remove unused indices in batch step

In the batch step (in recalculate) we need to remove the unused indices
from the variable index, otherwise the elimination would throw an
exception saying:

  "Requested to eliminate a key that is not in the factors"
---
 gtsam/nonlinear/ISAM2.cpp | 18 ++++++++++++++----
 1 file changed, 14 insertions(+), 4 deletions(-)

diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp
index 18296b393..122bf8aeb 100644
--- a/gtsam/nonlinear/ISAM2.cpp
+++ b/gtsam/nonlinear/ISAM2.cpp
@@ -334,13 +334,23 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
 
     gttic(add_keys);
     br::copy(variableIndex_ | br::map_keys, std::inserter(*affectedKeysSet, affectedKeysSet->end()));
+
+    // Removed unused keys:
+    VariableIndex affectedFactorsVarIndex = variableIndex_;
+
+    affectedFactorsVarIndex.removeUnusedVariables(unusedIndices.begin(), unusedIndices.end());
+
+    for (const Key key: unusedIndices)
+    {
+      affectedKeysSet->erase(key);
+    }
     gttoc(add_keys);
 
     gttic(ordering);
     Ordering order;
     if(constrainKeys)
     {
-      order = Ordering::ColamdConstrained(variableIndex_, *constrainKeys);
+      order = Ordering::ColamdConstrained(affectedFactorsVarIndex, *constrainKeys);
     }
     else
     {
@@ -350,11 +360,11 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
         FastMap<Key, int> constraintGroups;
         for(Key var: observedKeys)
           constraintGroups[var] = 1;
-        order = Ordering::ColamdConstrained(variableIndex_, constraintGroups);
+        order = Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups);
       }
       else
       {
-        order = Ordering::Colamd(variableIndex_);
+        order = Ordering::Colamd(affectedFactorsVarIndex);
       }
     }
     gttoc(ordering);
@@ -366,7 +376,7 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
     gttoc(linearize);
 
     gttic(eliminate);
-    ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree(linearized, variableIndex_, order))
+    ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree(linearized, affectedFactorsVarIndex, order))
       .eliminate(params_.getEliminationFunction()).first;
     gttoc(eliminate);