Reworked variable removal in iSAM2 to hopefully fix difficult bug

gtsam/nonlinear/ISAM2-impl.cpp

@@ -96,18 +96,16 @@ void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const ISAM2Cli
 			 VectorValues newDeltaNewton(dims);
 			 VectorValues newDeltaGradSearch(dims);
 			 std::vector<bool> newReplacedKeys(replacedKeys.size() - unusedIndices.size());
-			 Base::Nodes newNodes(nodes.size()); // We still keep unused keys at the end until later in ISAM2::recalculate
+			 Base::Nodes newNodes(replacedKeys.size() - unusedIndices.size());
 
 			 for(size_t j = 0; j < dims.size(); ++j) {
 				 newDelta[j] = delta[unusedToEnd[j]];
 				 newDeltaNewton[j] = deltaNewton[unusedToEnd[j]];
 				 newDeltaGradSearch[j] = deltaGradSearch[unusedToEnd[j]];
 				 newReplacedKeys[j] = replacedKeys[unusedToEnd[j]];
+				 newNodes[j] = nodes[unusedToEnd[j]];
 			 }
 
-			 // Permute the nodes index so the unused variables are the end
-			 unusedToEnd.applyToCollection(newNodes, nodes);
-
 			 // Swap the new data structures with the outputs of this function
 			 delta.swap(newDelta);
 			 deltaNewton.swap(newDeltaNewton);

gtsam/nonlinear/ISAM2.cpp

@@ -203,8 +203,8 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
   return cachedBoundary;
 }
 
-boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
+boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& markedKeys,
-    const FastSet<Index>& markedKeys, const FastSet<Index>& relinKeys, const FastVector<Index>& observedKeys,
+		const FastSet<Index>& relinKeys, const FastVector<Index>& observedKeys, const FastSet<Index>& unusedIndices,
     const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result) {
 
   // TODO:  new factors are linearized twice, the newFactors passed in are not used.
@@ -249,9 +249,6 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
   this->removeTop(markedKeys, affectedBayesNet, orphans);
   toc(1, "removetop");
 
-	// Now that the top is removed, correct the size of the Nodes index
-	this->nodes_.resize(delta_.size());
-
   if(debug) affectedBayesNet.print("Removed top: ");
   if(debug) orphans.print("Orphans: ");
 
@@ -269,14 +266,6 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
   // ordering provides all keys in conditionals, there cannot be others because path to root included
   tic(2,"affectedKeys");
   FastList<Index> affectedKeys = affectedBayesNet.ordering();
-	// The removed top also contained removed variables, which will be ordered
-	// higher than the number of variables in the system since unused variables
-	// were already removed in ISAM2::update.
-	for(FastList<Index>::iterator key = affectedKeys.begin(); key != affectedKeys.end(); )
-		if(*key >= delta_.size())
-			affectedKeys.erase(key++);
-		else
-			++key;
   toc(2,"affectedKeys");
 
   boost::shared_ptr<FastSet<Index> > affectedKeysSet(new FastSet<Index>()); // Will return this result
@@ -430,8 +419,12 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
       reorderingMode.constrainedKeys = FastMap<Index,int>();
       BOOST_FOREACH(Index var, observedKeys) { reorderingMode.constrainedKeys->insert(make_pair(var, 1)); }
     }
+		FastSet<Index> affectedUsedKeys = *affectedKeysSet; // Remove unused keys from the set we pass to PartialSolve
+		BOOST_FOREACH(Index unused, unusedIndices) {
+			affectedUsedKeys.erase(unused);
+		}
     Impl::PartialSolveResult partialSolveResult =
-        Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode, (params_.factorization == ISAM2Params::QR));
+        Impl::PartialSolve(factors, affectedUsedKeys, reorderingMode, (params_.factorization == ISAM2Params::QR));
     toc(2,"PartialSolve");
 
     // We now need to permute everything according this partial reordering: the
@@ -559,50 +552,25 @@ ISAM2Result ISAM2::update(
   // Remove removed factors from the variable index so we do not attempt to relinearize them
   variableIndex_.remove(removeFactorIndices, *removeFactors.symbolic(ordering_));
 
-	// We now need to start keeping track of the marked keys involved in added or
+	// Compute unused keys and indices
-	// removed factors.
+	FastSet<Key> unusedKeys;
-	FastSet<Index> markedKeys;
+	FastSet<Index> unusedIndices;
-
-	// Remove unused keys and add keys from removed factors that are still used
-	// in other factors to markedKeys.
 	{
-		// Get keys from removed factors
+		// Get keys from removed factors and new factors, and compute unused keys,
-		FastSet<Key> removedFactorSymbKeys = removeFactors.keys();
+		// i.e., keys that are empty now and do not appear in the new factors.
-
+		FastSet<Key> removedAndEmpty;
-		// For each key, if still used in other factors, add to markedKeys to be
+		BOOST_FOREACH(Key key, removeFactors.keys()) {
-		// recalculated, or if not used, add to unusedKeys to be removed from the
+			if(variableIndex_[ordering_[key]].empty())
-		// system.  Note that unusedKeys stores Key while markedKeys stores Index.
+				removedAndEmpty.insert(removedAndEmpty.end(), key);
-		FastSet<Key> unusedKeys;
-		BOOST_FOREACH(Key key, removedFactorSymbKeys) {
-			Index index = ordering_[key];
-			if(variableIndex_[index].empty())
-				unusedKeys.insert(key);
 		}
+		FastSet<Key> newFactorSymbKeys = newFactors.keys();
+		std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(),
+			newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(unusedKeys, unusedKeys.end()));
 
-		// Delete any keys from 'unusedKeys' that are actually used by the new, incoming factors
+		// Get indices for unused keys
-		FastSet<Key> newNonlinearKeys = newFactors.keys();
+		BOOST_FOREACH(Key key, unusedKeys) {
-		BOOST_FOREACH(Key key, newNonlinearKeys) {
+			unusedIndices.insert(unusedIndices.end(), ordering_[key]);
-		  FastSet<Key>::iterator iter = unusedKeys.find(key);
-		  if(iter != unusedKeys.end()) {
-		    unusedKeys.erase(iter);
-		  }
 		}
-
-		// Remove unused keys.  We must hold on to the new nodes index for now
-		// instead of placing it into the tree because removeTop will need to
-		// update it.
-		Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
-			deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_);
-
-		// Mark keys that are still in the use and are also included in the removed factors
-		// Note: The ordering has been modified during the RemoveVariables() function call.
-		// Hence, we do not create this list until after that call
-		BOOST_FOREACH(Key key, removedFactorSymbKeys) {
-		  Index index;
-		  if(ordering_.tryAt(key, index))
-			markedKeys.insert(index);
-		}
-
 	}
   toc(1,"push_back factors");
 
@@ -622,10 +590,12 @@ ISAM2Result ISAM2::update(
 
   tic(4,"gather involved keys");
   // 3. Mark linear update
-	{
+  FastSet<Index> markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors
-		FastSet<Index> newFactorIndices = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors
+  // Also mark keys involved in removed factors
-		markedKeys.insert(newFactorIndices.begin(), newFactorIndices.end());
+  {
-	}
+    FastSet<Index> markedRemoveKeys = Impl::IndicesFromFactors(ordering_, removeFactors); // Get keys involved in removed factors
+    markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys
+  }
 
 	// Observed keys for detailed results
   if(params_.enableDetailedResults) {
@@ -636,7 +606,11 @@ ISAM2Result ISAM2::update(
   // NOTE: we use assign instead of the iterator constructor here because this
   // is a vector of size_t, so the constructor unintentionally resolves to
   // vector(size_t count, Index value) instead of the iterator constructor.
-  FastVector<Index> observedKeys; observedKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them
+  FastVector<Index> observedKeys;  observedKeys.reserve(markedKeys.size());
+	BOOST_FOREACH(Index index, markedKeys) {
+		if(unusedIndices.find(index) == unusedIndices.end()) // Only add if not unused
+			observedKeys.push_back(index); // Make a copy of these, as we'll soon add to them
+	}
   toc(4,"gather involved keys");
 
   // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold
@@ -668,15 +642,6 @@ ISAM2Result ISAM2::update(
       // add other cliques that have the marked ones in the separator
       Impl::FindAll(this->root(), markedKeys, markedRelinMask);
 
-			// FindAll might have marked some removed keys because the removed keys
-			// are still in the old BayesTree structure - we remove them from
-			// markedKeys here.  Note that the limit for removing indices that we use
-			// (delta_.size()) is after all of the new variables - so we do not
-			// remove indices that were removed but then recreated by adding new
-			// keys.
-			FastSet<Index>::iterator firstRemovedIndex = markedKeys.lower_bound(delta_.size());
-			markedKeys.erase(firstRemovedIndex, markedKeys.end());
-
       // Relin involved keys for detailed results
       if(params_.enableDetailedResults) {
         FastSet<Index> involvedRelinKeys;
@@ -738,7 +703,7 @@ ISAM2Result ISAM2::update(
   }
   boost::shared_ptr<FastSet<Index> > replacedKeys;
   if(!markedKeys.empty() || !observedKeys.empty())
-    replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, constrainedIndices, result);
+    replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, unusedIndices, constrainedIndices, result);
 
   // Update replaced keys mask (accumulates until back-substitution takes place)
   if(replacedKeys) {
@@ -746,10 +711,11 @@ ISAM2Result ISAM2::update(
       deltaReplacedMask_[var] = true; } }
   toc(9,"recalculate");
 
-  //tic(9,"solve");
+	// After the top of the tree has been redone and may have index gaps from
-  // 9. Solve
+	// unused keys, condense the indices to remove gaps by rearranging indices
-  if(debug) delta_.print("delta_: ");
+	// in all data structures.
-  //toc(9,"solve");
+	Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
+		deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_);
 
   result.cliques = this->nodes().size();
   deltaDoglegUptodate_ = false;

gtsam/nonlinear/ISAM2.h

@@ -516,7 +516,7 @@ private:
   GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans);
 
   boost::shared_ptr<FastSet<Index> > recalculate(const FastSet<Index>& markedKeys, const FastSet<Index>& relinKeys,
-      const FastVector<Index>& observedKeys, const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result);
+      const FastVector<Index>& observedKeys, const FastSet<Index>& unusedIndices, const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result);
   //	void linear_update(const GaussianFactorGraph& newFactors);
   void updateDelta(bool forceFullSolve = false) const;