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 */
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);