Merged from branch 'branches/isam2-remove-vars'

CppUnitLite/Test.h

@@ -110,7 +110,7 @@ protected:
 { try { condition; \
 		result_.addFailure (Failure (name_, __FILE__,__LINE__, SimpleString("Didn't throw: ") + StringFrom(#condition))); \
 		return; } \
-  catch (exception_name& e) {} \
+  catch (exception_name&) {} \
   catch (...) { \
 	result_.addFailure (Failure (name_, __FILE__,__LINE__, SimpleString("Wrong exception: ") + StringFrom(#condition) + StringFrom(", expected: ") + StringFrom(#exception_name))); \
 	return; } }

gtsam/inference/Permutation.h

@@ -133,8 +133,8 @@ public:
   static Permutation PullToFront(const std::vector<Index>& toFront, size_t size, bool filterDuplicates = false);
 
   /**
-   * Create a permutation that pulls the given variables to the front while
-   * pushing the rest to the back.
+   * Create a permutation that pushes the given variables to the back while
+   * pulling the rest to the front.
    */
   static Permutation PushToBack(const std::vector<Index>& toBack, size_t size, bool filterDuplicates = false);
 
@@ -154,6 +154,17 @@ public:
   const_iterator begin() const { return rangeIndices_.begin(); }	///< Iterate through the indices
   const_iterator end() const { return rangeIndices_.end(); }			///< Iterate through the indices
 
+	/** Apply the permutation to a collection, which must have operator[] defined.
+	 * Note that permutable gtsam data structures typically have their own
+	 * permute function to apply a permutation.  Permutation::applyToCollection is
+	 * a generic function, e.g. for STL classes.
+	 * @param input The input collection.
+	 * @param output The preallocated output collection, which is assigned output[i] = input[permutation[i]]
+	 */
+	template<typename INPUT_COLLECTION, typename OUTPUT_COLLECTION>
+	void applyToCollection(OUTPUT_COLLECTION& output, const INPUT_COLLECTION& input) const {
+		for(size_t i = 0; i < output.size(); ++i) output[i] = input[(*this)[i]]; }
+
 
 	/// @}
 	/// @name Advanced Interface

gtsam/inference/VariableIndex.cpp

@@ -76,4 +76,15 @@ void VariableIndex::permuteInPlace(const Permutation& permutation) {
 	index_.swap(newIndex);
 }
 
+/* ************************************************************************* */
+void VariableIndex::removeUnusedAtEnd(size_t nToRemove) {
+#ifndef NDEBUG
+	for(size_t i = this->size() - nToRemove; i < this->size(); ++i)
+		if(!(*this)[i].empty())
+			throw std::invalid_argument("Attempting to remove non-empty variables with VariableIndex::removeUnusedAtEnd()");
+#endif
+
+	index_.resize(this->size() - nToRemove);
+}
+
 }

gtsam/inference/VariableIndex.h

@@ -119,6 +119,11 @@ public:
 
   /**
    * Remove entries corresponding to the specified factors.
+	 * NOTE: We intentionally do not decrement nFactors_ because the factor
+	 * indices need to remain consistent.  Removing factors from a factor graph
+	 * does not shift the indices of other factors.  Also, we keep nFactors_
+	 * one greater than the highest-numbered factor referenced in a VariableIndex.
+	 *
    * @param indices The indices of the factors to remove, which must match \c factors
    * @param factors The factors being removed, which must symbolically correspond
    * exactly to the factors with the specified \c indices that were added.
@@ -128,6 +133,12 @@ public:
 	/// Permute the variables in the VariableIndex according to the given permutation
 	void permuteInPlace(const Permutation& permutation);
 
+	/** Remove unused empty variables at the end of the ordering (in debug mode
+	 * verifies they are empty).
+	 * @param nToRemove The number of unused variables at the end to remove
+	 */
+	void removeUnusedAtEnd(size_t nToRemove);
+
 protected:
   Factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); }	  ///<TODO: comment
   Factor_iterator factorsEnd(Index variable) { checkVar(variable); return index_[variable].end(); }				///<TODO: comment
@@ -235,6 +246,10 @@ void VariableIndex::augment(const FactorGraph& factors) {
 /* ************************************************************************* */
 template<typename CONTAINER, class FactorGraph>
 void VariableIndex::remove(const CONTAINER& indices, const FactorGraph& factors) {
+	// NOTE: We intentionally do not decrement nFactors_ because the factor
+	// indices need to remain consistent.  Removing factors from a factor graph
+	// does not shift the indices of other factors.  Also, we keep nFactors_
+	// one greater than the highest-numbered factor referenced in a VariableIndex.
   for(size_t fi=0; fi<factors.size(); ++fi)
     if(factors[fi]) {
       for(size_t ji = 0; ji < factors[fi]->keys().size(); ++ji) {

gtsam/inference/tests/testVariableIndex.cpp

@@ -44,7 +44,9 @@ TEST(VariableIndex, augment) {
   VariableIndex actual(fg1);
   actual.augment(fg2);
 
-  CHECK(assert_equal(expected, actual));
+	LONGS_EQUAL(16, actual.nEntries());
+	LONGS_EQUAL(8, actual.nFactors());
+  EXPECT(assert_equal(expected, actual));
 }
 
 /* ************************************************************************* */

gtsam/linear/VectorValues.cpp

@@ -45,6 +45,14 @@ VectorValues VectorValues::Zero(const VectorValues& x) {
 	return cloned;
 }
 
+/* ************************************************************************* */
+vector<size_t> VectorValues::dims() const {
+  std::vector<size_t> result(this->size());
+	for(Index j = 0; j < this->size(); ++j)
+		result[j] = this->dim(j);
+	return result;
+}
+
 /* ************************************************************************* */
 void VectorValues::insert(Index j, const Vector& value) {
   // Make sure j does not already exist
@@ -188,4 +196,10 @@ VectorValues VectorValues::permute(const Permutation& permutation) const {
 	return lhs;
 }
 
+/* ************************************************************************* */
+void VectorValues::swap(VectorValues& other) {
+	this->values_.swap(other.values_);
+	this->maps_.swap(other.maps_);
+}
+
 }

gtsam/linear/VectorValues.h

@@ -134,6 +134,9 @@ namespace gtsam {
     /** Return the summed dimensionality of all variables. */
     size_t dim() const { return values_.rows(); }
 
+		/** Return the dimension of each vector in this container */
+		std::vector<size_t> dims() const;
+
     /** Check whether a variable with index \c j exists. */
     bool exists(Index j) const { return j < size() && maps_[j].rows() > 0; }
 
@@ -296,6 +299,11 @@ namespace gtsam {
 		 */
 		VectorValues permute(const Permutation& permutation) const;
 
+		/**
+		 * Swap the data in this VectorValues with another.
+		 */
+		void swap(VectorValues& other);
+
     /// @}
 
   private:

gtsam/nonlinear/ISAM2-impl.cpp

@@ -31,7 +31,7 @@ namespace gtsam {
 void ISAM2::Impl::AddVariables(
     const Values& newTheta, Values& theta, VectorValues& delta,
     VectorValues& deltaNewton, VectorValues& deltaGradSearch, vector<bool>& replacedKeys,
-    Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter) {
+    Ordering& ordering, const KeyFormatter& keyFormatter) {
   const bool debug = ISDEBUG("ISAM2 AddVariables");
 
   theta.insert(newTheta);
@@ -58,9 +58,75 @@ void ISAM2::Impl::AddVariables(
     assert(ordering.nVars() == delta.size());
     assert(ordering.size() == delta.size());
   }
-  assert(ordering.nVars() >= nodes.size());
   replacedKeys.resize(ordering.nVars(), false);
-  nodes.resize(ordering.nVars());
+}
+
+/* ************************************************************************* */
+void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const ISAM2Clique::shared_ptr& root,
+																	Values& theta, VariableIndex& variableIndex,
+																	VectorValues& delta, VectorValues& deltaNewton, VectorValues& deltaGradSearch,
+																	std::vector<bool>& replacedKeys, Ordering& ordering, Base::Nodes& nodes,
+																	GaussianFactorGraph& linearFactors) {
+
+		 // Get indices of unused keys
+		 vector<Index> unusedIndices;  unusedIndices.reserve(unusedKeys.size());
+		 BOOST_FOREACH(Key key, unusedKeys) { unusedIndices.push_back(ordering[key]); }
+
+		 // Create a permutation that shifts the unused variables to the end
+		 Permutation unusedToEnd = Permutation::PushToBack(unusedIndices, delta.size());
+		 Permutation unusedToEndInverse = *unusedToEnd.inverse();
+
+		 // Use the permutation to remove unused variables while shifting all the others to take up the space
+		 variableIndex.permuteInPlace(unusedToEnd);
+		 variableIndex.removeUnusedAtEnd(unusedIndices.size());
+		 {
+			 // Create a vector of variable dimensions with the unused ones removed
+			 // by applying the unusedToEnd permutation to the original vector of
+			 // variable dimensions.  We only allocate space in the shifted dims
+			 // vector for the used variables, so that the unused ones are dropped
+			 // when the permutation is applied.
+			 vector<size_t> originalDims = delta.dims();
+			 vector<size_t> dims(delta.size() - unusedIndices.size());
+			 unusedToEnd.applyToCollection(dims, originalDims);
+
+			 // Copy from the old data structures to new ones, only iterating up to
+			 // the number of used variables, and applying the unusedToEnd permutation
+			 // in order to remove the unused variables.
+			 VectorValues newDelta(dims);
+			 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
+
+			 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]];
+			 }
+
+			 // 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);
+			 deltaGradSearch.swap(newDeltaGradSearch);
+			 replacedKeys.swap(newReplacedKeys);
+			 nodes.swap(newNodes);
+		 }
+
+		 // Reorder and remove from ordering and solution
+		 ordering.permuteWithInverse(unusedToEndInverse);
+		 BOOST_REVERSE_FOREACH(Key key, unusedKeys) {
+		   ordering.pop_back(key);
+			 theta.erase(key);
+		 }
+
+		 // Finally, permute references to variables
+		 if(root)
+			 root->permuteWithInverse(unusedToEndInverse);
+		 linearFactors.permuteWithInverse(unusedToEndInverse);
 }
 
 /* ************************************************************************* */

gtsam/nonlinear/ISAM2-impl.h

@@ -48,8 +48,16 @@ struct ISAM2::Impl {
    */
   static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta,
       VectorValues& deltaNewton, VectorValues& deltaGradSearch, std::vector<bool>& replacedKeys,
-      Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+      Ordering& ordering, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
 	  
+  /**
+   * Remove variables from the ISAM2 system.
+   */
+	static void RemoveVariables(const FastSet<Key>& unusedKeys, const ISAM2Clique::shared_ptr& root,
+		Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton,
+		VectorValues& deltaGradSearch, std::vector<bool>& replacedKeys, Ordering& ordering, Base::Nodes& nodes,
+		GaussianFactorGraph& linearFactors);
+
   /**
    * Extract the set of variable indices from a NonlinearFactorGraph.  For each Symbol
    * in each NonlinearFactor, obtains the index by calling ordering[symbol].

gtsam/nonlinear/ISAM2.cpp

@@ -249,6 +249,9 @@ 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: ");
 
@@ -266,6 +269,14 @@ 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
@@ -539,15 +550,46 @@ ISAM2Result ISAM2::update(
   BOOST_FOREACH(size_t index, removeFactorIndices) {
     removeFactors.push_back(nonlinearFactors_[index]);
     nonlinearFactors_.remove(index);
+		if(params_.cacheLinearizedFactors)
+			linearFactors_.remove(index);
   }
 
   // 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
+	// removed factors.
+	FastSet<Index> markedKeys;
+
+	// Remove unused keys and add keys from removed factors that are still used
+	// in other factors to markedKeys.
+	{
+		// Get keys from removed factors
+		FastSet<Key> removedFactorSymbKeys = removeFactors.keys();
+
+		// For each key, if still used in other factors, add to markedKeys to be
+		// recalculated, or if not used, add to unusedKeys to be removed from the
+		// system.  Note that unusedKeys stores Key while markedKeys stores Index.
+		FastSet<Key> unusedKeys;
+		BOOST_FOREACH(Key key, removedFactorSymbKeys) {
+			Index index = ordering_[key];
+			if(variableIndex_[index].empty())
+				unusedKeys.insert(key);
+			else
+				markedKeys.insert(index);
+		}
+
+		// 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_);
+	}
   toc(1,"push_back factors");
 
   tic(2,"add new variables");
   // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}.
-  Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_, Base::nodes_);
+  Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_);
   // New keys for detailed results
   if(params_.enableDetailedResults) {
     inverseOrdering_ = ordering_.invert();
@@ -561,13 +603,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
-  // Also mark keys involved in removed factors
-  {
-    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
+	{
+		FastSet<Index> newFactorIndices = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors
+		markedKeys.insert(newFactorIndices.begin(), newFactorIndices.end());
+	}
+
+	// Observed keys for detailed results
   if(params_.enableDetailedResults) {
     BOOST_FOREACH(Index index, markedKeys) {
       result.detail->variableStatus[inverseOrdering_->at(index)].isObserved = true;

gtsam/nonlinear/ISAM2.h

@@ -516,8 +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 boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result);
   //	void linear_update(const GaussianFactorGraph& newFactors);
   void updateDelta(bool forceFullSolve = false) const;
 

gtsam/nonlinear/NonlinearFactorGraph.cpp

@@ -54,8 +54,8 @@ double NonlinearFactorGraph::error(const Values& c) const {
 }
 
 /* ************************************************************************* */
-std::set<Key> NonlinearFactorGraph::keys() const {
-	std::set<Key> keys;
+FastSet<Key> NonlinearFactorGraph::keys() const {
+	FastSet<Key> keys;
 	BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
 		if(factor)
 			keys.insert(factor->begin(), factor->end());

gtsam/nonlinear/NonlinearFactorGraph.h

@@ -47,7 +47,7 @@ namespace gtsam {
     void print(const std::string& str = "NonlinearFactorGraph: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
     /** return keys as an ordered set - ordering is by key value */
-    std::set<Key> keys() const;
+    FastSet<Key> keys() const;
 
 		/** unnormalized error, \f$ 0.5 \sum_i (h_i(X_i)-z)^2/\sigma^2 \f$ in the most common case */
 		double error(const Values& c) const;

tests/testGaussianISAM2.cpp

@@ -6,6 +6,7 @@
 
 #include <gtsam/base/debug.h>
 #include <gtsam/base/TestableAssertions.h>
+#include <gtsam/inference/SymbolicFactorGraph.h>
 #include <gtsam/nonlinear/Ordering.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianSequentialSolver.h>
@@ -43,7 +44,6 @@ ISAM2 createSlamlikeISAM2(
 
   // These variables will be reused and accumulate factors and values
   ISAM2 isam(params);
-//  ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true));
   Values fullinit;
   planarSLAM::Graph fullgraph;
 
@@ -135,7 +135,7 @@ ISAM2 createSlamlikeISAM2(
 }
 
 /* ************************************************************************* */
-TEST_UNSAFE(ISAM2, AddVariables) {
+TEST_UNSAFE(ISAM2, ImplAddVariables) {
 
   // Create initial state
   Values theta;
@@ -160,8 +160,6 @@ TEST_UNSAFE(ISAM2, AddVariables) {
 
   Ordering ordering; ordering += 100, 0;
 
-  ISAM2::Nodes nodes(2);
-
   // Verify initial state
   LONGS_EQUAL(0, ordering[100]);
   LONGS_EQUAL(1, ordering[0]);
@@ -193,10 +191,8 @@ TEST_UNSAFE(ISAM2, AddVariables) {
 
   Ordering orderingExpected; orderingExpected += 100, 0, 1;
 
-  ISAM2::Nodes nodesExpected(3, ISAM2::sharedClique());
-
   // Expand initial state
-  ISAM2::Impl::AddVariables(newTheta, theta, delta, deltaNewton, deltaRg, replacedKeys, ordering, nodes);
+  ISAM2::Impl::AddVariables(newTheta, theta, delta, deltaNewton, deltaRg, replacedKeys, ordering);
 
   EXPECT(assert_equal(thetaExpected, theta));
   EXPECT(assert_equal(deltaExpected, delta));
@@ -205,6 +201,95 @@ TEST_UNSAFE(ISAM2, AddVariables) {
   EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys));
   EXPECT(assert_equal(orderingExpected, ordering));
 }
+/* ************************************************************************* */
+TEST_UNSAFE(ISAM2, ImplRemoveVariables) {
+
+	// Create initial state
+	Values theta;
+	theta.insert(0, Pose2(.1, .2, .3));
+	theta.insert(1, Pose2(.6, .7, .8));
+	theta.insert(100, Point2(.4, .5));
+
+	SymbolicFactorGraph sfg;
+	sfg.push_back(boost::make_shared<IndexFactor>(Index(0), Index(2)));
+	sfg.push_back(boost::make_shared<IndexFactor>(Index(0), Index(1)));
+	VariableIndex variableIndex(sfg);
+
+	VectorValues delta;
+	delta.insert(0, Vector_(3, .1, .2, .3));
+	delta.insert(1, Vector_(3, .4, .5, .6));
+	delta.insert(2, Vector_(2, .7, .8));
+
+	VectorValues deltaNewton;
+	deltaNewton.insert(0, Vector_(3, .1, .2, .3));
+	deltaNewton.insert(1, Vector_(3, .4, .5, .6));
+	deltaNewton.insert(2, Vector_(2, .7, .8));
+
+	VectorValues deltaRg;
+	deltaRg.insert(0, Vector_(3, .1, .2, .3));
+	deltaRg.insert(1, Vector_(3, .4, .5, .6));
+	deltaRg.insert(2, Vector_(2, .7, .8));
+
+	vector<bool> replacedKeys(3, false);
+	replacedKeys[0] = true;
+	replacedKeys[1] = false;
+	replacedKeys[2] = true;
+
+	Ordering ordering; ordering += 100, 1, 0;
+
+	ISAM2::Nodes nodes(3);
+
+	// Verify initial state
+	LONGS_EQUAL(0, ordering[100]);
+	LONGS_EQUAL(1, ordering[1]);
+	LONGS_EQUAL(2, ordering[0]);
+
+	// Create expected state
+	Values thetaExpected;
+	thetaExpected.insert(0, Pose2(.1, .2, .3));
+	thetaExpected.insert(100, Point2(.4, .5));
+
+	SymbolicFactorGraph sfgRemoved;
+	sfgRemoved.push_back(boost::make_shared<IndexFactor>(Index(0), Index(1)));
+	sfgRemoved.push_back(SymbolicFactorGraph::sharedFactor()); // Add empty factor to keep factor indices consistent
+	VariableIndex variableIndexExpected(sfgRemoved);
+
+	VectorValues deltaExpected;
+	deltaExpected.insert(0, Vector_(3, .1, .2, .3));
+	deltaExpected.insert(1, Vector_(2, .7, .8));
+
+	VectorValues deltaNewtonExpected;
+	deltaNewtonExpected.insert(0, Vector_(3, .1, .2, .3));
+	deltaNewtonExpected.insert(1, Vector_(2, .7, .8));
+
+	VectorValues deltaRgExpected;
+	deltaRgExpected.insert(0, Vector_(3, .1, .2, .3));
+	deltaRgExpected.insert(1, Vector_(2, .7, .8));
+
+	vector<bool> replacedKeysExpected(2, true);
+
+	Ordering orderingExpected; orderingExpected += 100, 0;
+
+	ISAM2::Nodes nodesExpected(2);
+
+	// Reduce initial state
+	FastSet<Key> unusedKeys;
+	unusedKeys.insert(1);
+	vector<size_t> removedFactorsI; removedFactorsI.push_back(1);
+	SymbolicFactorGraph removedFactors; removedFactors.push_back(sfg[1]);
+	variableIndex.remove(removedFactorsI, removedFactors);
+	GaussianFactorGraph linearFactors;
+	ISAM2::Impl::RemoveVariables(unusedKeys, ISAM2::sharedClique(), theta, variableIndex, delta, deltaNewton, deltaRg,
+		replacedKeys, ordering, nodes, linearFactors);
+
+	EXPECT(assert_equal(thetaExpected, theta));
+	EXPECT(assert_equal(variableIndexExpected, variableIndex));
+	EXPECT(assert_equal(deltaExpected, delta));
+	EXPECT(assert_equal(deltaNewtonExpected, deltaNewton));
+	EXPECT(assert_equal(deltaRgExpected, deltaRg));
+	EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys));
+	EXPECT(assert_equal(orderingExpected, ordering));
+}
 
 /* ************************************************************************* */
 //TEST(ISAM2, IndicesFromFactors) {
@@ -293,7 +378,11 @@ TEST(ISAM2, optimize2) {
 }
 
 /* ************************************************************************* */
-bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const ISAM2& isam) {
+bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) {
+
+	TestResult& result_ = result;
+	const SimpleString name_ = test.getName();
+
   Values actual = isam.calculateEstimate();
   Ordering ordering = isam.getOrdering(); // *fullgraph.orderingCOLAMD(fullinit).first;
   GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering);
@@ -303,488 +392,94 @@ bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, cons
   VectorValues delta = optimize(gbn);
   Values expected = fullinit.retract(delta, ordering);
 
-  return assert_equal(expected, actual);
+  bool isamEqual = assert_equal(expected, actual);
+
+	// The following two checks make sure that the cached gradients are maintained and used correctly
+
+	// Check gradient at each node
+	bool nodeGradientsOk = true;
+	typedef ISAM2::sharedClique sharedClique;
+	BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
+		// Compute expected gradient
+		FactorGraph<JacobianFactor> jfg;
+		jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional())));
+		VectorValues expectedGradient(*allocateVectorValues(isam));
+		gradientAtZero(jfg, expectedGradient);
+		// Compare with actual gradients
+		int variablePosition = 0;
+		for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
+			const int dim = clique->conditional()->dim(jit);
+			Vector actual = clique->gradientContribution().segment(variablePosition, dim);
+			bool gradOk = assert_equal(expectedGradient[*jit], actual);
+			EXPECT(gradOk);
+			nodeGradientsOk = nodeGradientsOk && gradOk;
+			variablePosition += dim;
+		}
+		bool dimOk = clique->gradientContribution().rows() == variablePosition;
+		EXPECT(dimOk);
+		nodeGradientsOk = nodeGradientsOk && dimOk;
+	}
+
+	// Check gradient
+	VectorValues expectedGradient(*allocateVectorValues(isam));
+	gradientAtZero(FactorGraph<JacobianFactor>(isam), expectedGradient);
+	VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
+	VectorValues actualGradient(*allocateVectorValues(isam));
+	gradientAtZero(isam, actualGradient);
+	bool expectedGradOk = assert_equal(expectedGradient2, expectedGradient);
+	EXPECT(expectedGradOk);
+	bool totalGradOk = assert_equal(expectedGradient, actualGradient);
+	EXPECT(totalGradOk);
+
+	return nodeGradientsOk && expectedGradOk && totalGradOk;
 }
 
 /* ************************************************************************* */
 TEST(ISAM2, slamlike_solution_gaussnewton)
 {
-
   // These variables will be reused and accumulate factors and values
-  ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
   Values fullinit;
   planarSLAM::Graph fullgraph;
-
-  // i keeps track of the time step
-  size_t i = 0;
-
-  // Add a prior at time 0 and update isam
-  {
-    planarSLAM::Graph newfactors;
-    newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((0), Pose2(0.01, 0.01, 0.01));
-    fullinit.insert((0), Pose2(0.01, 0.01, 0.01));
-
-    isam.update(newfactors, init);
-  }
-
-  CHECK(isam_check(fullgraph, fullinit, isam));
-
-  // Add odometry from time 0 to time 5
-  for( ; i<5; ++i) {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-    fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-
-    isam.update(newfactors, init);
-  }
-
-  // Add odometry from time 5 to 6 and landmark measurement at time 5
-  {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
-    newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(1.01, 0.01, 0.01));
-    init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
-    init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
-    fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
-    fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
-    fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
-
-    isam.update(newfactors, init);
-    ++ i;
-  }
-
-  // Add odometry from time 6 to time 10
-  for( ; i<10; ++i) {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-    fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-
-    isam.update(newfactors, init);
-  }
-
-  // Add odometry from time 10 to 11 and landmark measurement at time 10
-  {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
-    newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(6.9, 0.1, 0.01));
-    fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));
-
-    isam.update(newfactors, init);
-    ++ i;
-  }
+	ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
 
   // Compare solutions
-  CHECK(isam_check(fullgraph, fullinit, isam));
-
-  // Check gradient at each node
-  typedef ISAM2::sharedClique sharedClique;
-  BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
-    // Compute expected gradient
-    FactorGraph<JacobianFactor> jfg;
-    jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional())));
-    VectorValues expectedGradient(*allocateVectorValues(isam));
-    gradientAtZero(jfg, expectedGradient);
-    // Compare with actual gradients
-    int variablePosition = 0;
-    for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
-      const int dim = clique->conditional()->dim(jit);
-      Vector actual = clique->gradientContribution().segment(variablePosition, dim);
-      EXPECT(assert_equal(expectedGradient[*jit], actual));
-      variablePosition += dim;
-    }
-    LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition);
-  }
-
-  // Check gradient
-  VectorValues expectedGradient(*allocateVectorValues(isam));
-  gradientAtZero(FactorGraph<JacobianFactor>(isam), expectedGradient);
-  VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
-  VectorValues actualGradient(*allocateVectorValues(isam));
-  gradientAtZero(isam, actualGradient);
-  EXPECT(assert_equal(expectedGradient2, expectedGradient));
-  EXPECT(assert_equal(expectedGradient, actualGradient));
+  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
 }
 
 /* ************************************************************************* */
 TEST(ISAM2, slamlike_solution_dogleg)
 {
   // These variables will be reused and accumulate factors and values
-  ISAM2 isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false));
   Values fullinit;
   planarSLAM::Graph fullgraph;
-
-  // i keeps track of the time step
-  size_t i = 0;
-
-  // Add a prior at time 0 and update isam
-  {
-    planarSLAM::Graph newfactors;
-    newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((0), Pose2(0.01, 0.01, 0.01));
-    fullinit.insert((0), Pose2(0.01, 0.01, 0.01));
-
-    isam.update(newfactors, init);
-  }
-
-  CHECK(isam_check(fullgraph, fullinit, isam));
-
-  // Add odometry from time 0 to time 5
-  for( ; i<5; ++i) {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-    fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-
-    isam.update(newfactors, init);
-  }
-
-  // Add odometry from time 5 to 6 and landmark measurement at time 5
-  {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
-    newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(1.01, 0.01, 0.01));
-    init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
-    init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
-    fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
-    fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
-    fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
-
-    isam.update(newfactors, init);
-    ++ i;
-  }
-
-  // Add odometry from time 6 to time 10
-  for( ; i<10; ++i) {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-    fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-
-    isam.update(newfactors, init);
-  }
-
-  // Add odometry from time 10 to 11 and landmark measurement at time 10
-  {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
-    newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(6.9, 0.1, 0.01));
-    fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));
-
-    isam.update(newfactors, init);
-    ++ i;
-  }
+	ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false));
 
   // Compare solutions
-  CHECK(isam_check(fullgraph, fullinit, isam));
-
-  // Check gradient at each node
-  typedef ISAM2::sharedClique sharedClique;
-  BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
-    // Compute expected gradient
-    FactorGraph<JacobianFactor> jfg;
-    jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional())));
-    VectorValues expectedGradient(*allocateVectorValues(isam));
-    gradientAtZero(jfg, expectedGradient);
-    // Compare with actual gradients
-    int variablePosition = 0;
-    for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
-      const int dim = clique->conditional()->dim(jit);
-      Vector actual = clique->gradientContribution().segment(variablePosition, dim);
-      EXPECT(assert_equal(expectedGradient[*jit], actual));
-      variablePosition += dim;
-    }
-    LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition);
-  }
-
-  // Check gradient
-  VectorValues expectedGradient(*allocateVectorValues(isam));
-  gradientAtZero(FactorGraph<JacobianFactor>(isam), expectedGradient);
-  VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
-  VectorValues actualGradient(*allocateVectorValues(isam));
-  gradientAtZero(isam, actualGradient);
-  EXPECT(assert_equal(expectedGradient2, expectedGradient));
-  EXPECT(assert_equal(expectedGradient, actualGradient));
+  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
 }
 
 /* ************************************************************************* */
 TEST(ISAM2, slamlike_solution_gaussnewton_qr)
 {
   // These variables will be reused and accumulate factors and values
-  ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR));
   Values fullinit;
   planarSLAM::Graph fullgraph;
+	ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR));
 
-  // i keeps track of the time step
-  size_t i = 0;
-
-  // Add a prior at time 0 and update isam
-  {
-    planarSLAM::Graph newfactors;
-    newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((0), Pose2(0.01, 0.01, 0.01));
-    fullinit.insert((0), Pose2(0.01, 0.01, 0.01));
-
-    isam.update(newfactors, init);
-  }
-
-  CHECK(isam_check(fullgraph, fullinit, isam));
-
-  // Add odometry from time 0 to time 5
-  for( ; i<5; ++i) {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-    fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-
-    isam.update(newfactors, init);
-  }
-
-  // Add odometry from time 5 to 6 and landmark measurement at time 5
-  {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
-    newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(1.01, 0.01, 0.01));
-    init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
-    init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
-    fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
-    fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
-    fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
-
-    isam.update(newfactors, init);
-    ++ i;
-  }
-
-  // Add odometry from time 6 to time 10
-  for( ; i<10; ++i) {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-    fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-
-    isam.update(newfactors, init);
-  }
-
-  // Add odometry from time 10 to 11 and landmark measurement at time 10
-  {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
-    newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(6.9, 0.1, 0.01));
-    fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));
-
-    isam.update(newfactors, init);
-    ++ i;
-  }
-
-  // Compare solutions
-  CHECK(isam_check(fullgraph, fullinit, isam));
-
-  // Check gradient at each node
-  typedef ISAM2::sharedClique sharedClique;
-  BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
-    // Compute expected gradient
-    FactorGraph<JacobianFactor> jfg;
-    jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional())));
-    VectorValues expectedGradient(*allocateVectorValues(isam));
-    gradientAtZero(jfg, expectedGradient);
-    // Compare with actual gradients
-    int variablePosition = 0;
-    for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
-      const int dim = clique->conditional()->dim(jit);
-      Vector actual = clique->gradientContribution().segment(variablePosition, dim);
-      EXPECT(assert_equal(expectedGradient[*jit], actual));
-      variablePosition += dim;
-    }
-    LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition);
-  }
-
-  // Check gradient
-  VectorValues expectedGradient(*allocateVectorValues(isam));
-  gradientAtZero(FactorGraph<JacobianFactor>(isam), expectedGradient);
-  VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
-  VectorValues actualGradient(*allocateVectorValues(isam));
-  gradientAtZero(isam, actualGradient);
-  EXPECT(assert_equal(expectedGradient2, expectedGradient));
-  EXPECT(assert_equal(expectedGradient, actualGradient));
+	// Compare solutions
+  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
 }
 
 /* ************************************************************************* */
 TEST(ISAM2, slamlike_solution_dogleg_qr)
 {
   // These variables will be reused and accumulate factors and values
-  ISAM2 isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR));
   Values fullinit;
   planarSLAM::Graph fullgraph;
-
-  // i keeps track of the time step
-  size_t i = 0;
-
-  // Add a prior at time 0 and update isam
-  {
-    planarSLAM::Graph newfactors;
-    newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((0), Pose2(0.01, 0.01, 0.01));
-    fullinit.insert((0), Pose2(0.01, 0.01, 0.01));
-
-    isam.update(newfactors, init);
-  }
-
-  CHECK(isam_check(fullgraph, fullinit, isam));
-
-  // Add odometry from time 0 to time 5
-  for( ; i<5; ++i) {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-    fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-
-    isam.update(newfactors, init);
-  }
-
-  // Add odometry from time 5 to 6 and landmark measurement at time 5
-  {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
-    newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(1.01, 0.01, 0.01));
-    init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
-    init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
-    fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
-    fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
-    fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
-
-    isam.update(newfactors, init);
-    ++ i;
-  }
-
-  // Add odometry from time 6 to time 10
-  for( ; i<10; ++i) {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-    fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-
-    isam.update(newfactors, init);
-  }
-
-  // Add odometry from time 10 to 11 and landmark measurement at time 10
-  {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
-    newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(6.9, 0.1, 0.01));
-    fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));
-
-    isam.update(newfactors, init);
-    ++ i;
-  }
+	ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR));
 
   // Compare solutions
-  CHECK(isam_check(fullgraph, fullinit, isam));
-
-  // Check gradient at each node
-  typedef ISAM2::sharedClique sharedClique;
-  BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
-    // Compute expected gradient
-    FactorGraph<JacobianFactor> jfg;
-    jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional())));
-    VectorValues expectedGradient(*allocateVectorValues(isam));
-    gradientAtZero(jfg, expectedGradient);
-    // Compare with actual gradients
-    int variablePosition = 0;
-    for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
-      const int dim = clique->conditional()->dim(jit);
-      Vector actual = clique->gradientContribution().segment(variablePosition, dim);
-      EXPECT(assert_equal(expectedGradient[*jit], actual));
-      variablePosition += dim;
-    }
-    LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition);
-  }
-
-  // Check gradient
-  VectorValues expectedGradient(*allocateVectorValues(isam));
-  gradientAtZero(FactorGraph<JacobianFactor>(isam), expectedGradient);
-  VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
-  VectorValues actualGradient(*allocateVectorValues(isam));
-  gradientAtZero(isam, actualGradient);
-  EXPECT(assert_equal(expectedGradient2, expectedGradient));
-  EXPECT(assert_equal(expectedGradient, actualGradient));
+  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
 }
 
 /* ************************************************************************* */
@@ -888,127 +583,43 @@ TEST(ISAM2, removeFactors)
   // then removes the 2nd-to-last landmark measurement
 
   // These variables will be reused and accumulate factors and values
-  ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
   Values fullinit;
   planarSLAM::Graph fullgraph;
+	ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
 
-  // i keeps track of the time step
-  size_t i = 0;
+	// Remove the 2nd measurement on landmark 0 (Key 100)
+	FastVector<size_t> toRemove;
+	toRemove.push_back(12);
+	isam.update(planarSLAM::Graph(), Values(), toRemove);
 
-  // Add a prior at time 0 and update isam
-  {
-    planarSLAM::Graph newfactors;
-    newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((0), Pose2(0.01, 0.01, 0.01));
-    fullinit.insert((0), Pose2(0.01, 0.01, 0.01));
-
-    isam.update(newfactors, init);
-  }
-
-  CHECK(isam_check(fullgraph, fullinit, isam));
-
-  // Add odometry from time 0 to time 5
-  for( ; i<5; ++i) {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-    fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-
-    isam.update(newfactors, init);
-  }
-
-  // Add odometry from time 5 to 6 and landmark measurement at time 5
-  {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
-    newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(1.01, 0.01, 0.01));
-    init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
-    init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
-    fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
-    fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
-    fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
-
-    isam.update(newfactors, init);
-    ++ i;
-  }
-
-  // Add odometry from time 6 to time 10
-  for( ; i<10; ++i) {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-    fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-
-    isam.update(newfactors, init);
-  }
-
-  // Add odometry from time 10 to 11 and landmark measurement at time 10
-  {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
-    newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
-    fullgraph.push_back(newfactors[0]);
-    fullgraph.push_back(newfactors[2]); // Don't add measurement on landmark 0
-
-    Values init;
-    init.insert((i+1), Pose2(6.9, 0.1, 0.01));
-    fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));
-
-    ISAM2Result result = isam.update(newfactors, init);
-    ++ i;
-
-    // Remove the measurement on landmark 0
-    FastVector<size_t> toRemove;
-    EXPECT_LONGS_EQUAL(isam.getFactorsUnsafe().size()-2, result.newFactorsIndices[1]);
-    toRemove.push_back(result.newFactorsIndices[1]);
-    isam.update(planarSLAM::Graph(), Values(), toRemove);
-  }
+	// Remove the factor from the full system
+	fullgraph.remove(12);
 
   // Compare solutions
-  CHECK(isam_check(fullgraph, fullinit, isam));
+  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
+}
 
-  // Check gradient at each node
-  typedef ISAM2::sharedClique sharedClique;
-  BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
-    // Compute expected gradient
-    FactorGraph<JacobianFactor> jfg;
-    jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional())));
-    VectorValues expectedGradient(*allocateVectorValues(isam));
-    gradientAtZero(jfg, expectedGradient);
-    // Compare with actual gradients
-    int variablePosition = 0;
-    for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
-      const int dim = clique->conditional()->dim(jit);
-      Vector actual = clique->gradientContribution().segment(variablePosition, dim);
-      EXPECT(assert_equal(expectedGradient[*jit], actual));
-      variablePosition += dim;
-    }
-    LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition);
-  }
+/* ************************************************************************* */
+TEST_UNSAFE(ISAM2, removeVariables)
+{
+	// These variables will be reused and accumulate factors and values
+	Values fullinit;
+	planarSLAM::Graph fullgraph;
+	ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
 
-  // Check gradient
-  VectorValues expectedGradient(*allocateVectorValues(isam));
-  gradientAtZero(FactorGraph<JacobianFactor>(isam), expectedGradient);
-  VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
-  VectorValues actualGradient(*allocateVectorValues(isam));
-  gradientAtZero(isam, actualGradient);
-  EXPECT(assert_equal(expectedGradient2, expectedGradient));
-  EXPECT(assert_equal(expectedGradient, actualGradient));
+	// Remove the measurement on landmark 0 (Key 100)
+	FastVector<size_t> toRemove;
+	toRemove.push_back(7);
+	toRemove.push_back(14);
+	isam.update(planarSLAM::Graph(), Values(), toRemove);
+
+	// Remove the factors and variable from the full system
+	fullgraph.remove(7);
+	fullgraph.remove(14);
+	fullinit.erase(100);
+
+	// Compare solutions
+	CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
 }
 
 /* ************************************************************************* */
@@ -1037,7 +648,7 @@ TEST_UNSAFE(ISAM2, swapFactors)
 
   // Compare solutions
   EXPECT(assert_equal(fullgraph, planarSLAM::Graph(isam.getFactorsUnsafe())));
-  EXPECT(isam_check(fullgraph, fullinit, isam));
+  EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_));
 
   // Check gradient at each node
   typedef ISAM2::sharedClique sharedClique;
@@ -1097,7 +708,7 @@ TEST(ISAM2, constrained_ordering)
     isam.update(newfactors, init);
   }
 
-  CHECK(isam_check(fullgraph, fullinit, isam));
+  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
 
   // Add odometry from time 0 to time 5
   for( ; i<5; ++i) {
@@ -1165,7 +776,7 @@ TEST(ISAM2, constrained_ordering)
   }
 
   // Compare solutions
-  EXPECT(isam_check(fullgraph, fullinit, isam));
+  EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_));
 
   // Check that x3 and x4 are last, but either can come before the other
   EXPECT(isam.getOrdering()[(3)] == 12 && isam.getOrdering()[(4)] == 13);
@@ -1204,122 +815,14 @@ TEST(ISAM2, slamlike_solution_partial_relinearization_check)
 {
 
   // These variables will be reused and accumulate factors and values
-  ISAM2Params params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false);
-  params.enablePartialRelinearizationCheck = true;
-  ISAM2 isam(params);
   Values fullinit;
   planarSLAM::Graph fullgraph;
-
-  // i keeps track of the time step
-  size_t i = 0;
-
-  // Add a prior at time 0 and update isam
-  {
-    planarSLAM::Graph newfactors;
-    newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((0), Pose2(0.01, 0.01, 0.01));
-    fullinit.insert((0), Pose2(0.01, 0.01, 0.01));
-
-    isam.update(newfactors, init);
-  }
-
-  CHECK(isam_check(fullgraph, fullinit, isam));
-
-  // Add odometry from time 0 to time 5
-  for( ; i<5; ++i) {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-    fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-
-    isam.update(newfactors, init);
-  }
-
-  // Add odometry from time 5 to 6 and landmark measurement at time 5
-  {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
-    newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(1.01, 0.01, 0.01));
-    init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
-    init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
-    fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
-    fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
-    fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
-
-    isam.update(newfactors, init);
-    ++ i;
-  }
-
-  // Add odometry from time 6 to time 10
-  for( ; i<10; ++i) {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-    fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
-
-    isam.update(newfactors, init);
-  }
-
-  // Add odometry from time 10 to 11 and landmark measurement at time 10
-  {
-    planarSLAM::Graph newfactors;
-    newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
-    newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
-    fullgraph.push_back(newfactors);
-
-    Values init;
-    init.insert((i+1), Pose2(6.9, 0.1, 0.01));
-    fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));
-
-    isam.update(newfactors, init);
-    ++ i;
-  }
+	ISAM2Params params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false);
+	params.enablePartialRelinearizationCheck = true;
+	ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, params);
 
   // Compare solutions
-  CHECK(isam_check(fullgraph, fullinit, isam));
-
-  // Check gradient at each node
-  typedef ISAM2::sharedClique sharedClique;
-  BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
-    // Compute expected gradient
-    FactorGraph<JacobianFactor> jfg;
-    jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional())));
-    VectorValues expectedGradient(*allocateVectorValues(isam));
-    gradientAtZero(jfg, expectedGradient);
-    // Compare with actual gradients
-    int variablePosition = 0;
-    for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
-      const int dim = clique->conditional()->dim(jit);
-      Vector actual = clique->gradientContribution().segment(variablePosition, dim);
-      EXPECT(assert_equal(expectedGradient[*jit], actual));
-      variablePosition += dim;
-    }
-    LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition);
-  }
-
-  // Check gradient
-  VectorValues expectedGradient(*allocateVectorValues(isam));
-  gradientAtZero(FactorGraph<JacobianFactor>(isam), expectedGradient);
-  VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
-  VectorValues actualGradient(*allocateVectorValues(isam));
-  gradientAtZero(isam, actualGradient);
-  EXPECT(assert_equal(expectedGradient2, expectedGradient));
-  EXPECT(assert_equal(expectedGradient, actualGradient));
+  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
 }
 
 /* ************************************************************************* */

tests/testNonlinearFactorGraph.cpp

@@ -65,9 +65,9 @@ TEST( Graph, error )
 TEST( Graph, keys )
 {
 	Graph fg = createNonlinearFactorGraph();
-	set<Key> actual = fg.keys();
+	FastSet<Key> actual = fg.keys();
 	LONGS_EQUAL(3, actual.size());
-	set<Key>::const_iterator it = actual.begin();
+	FastSet<Key>::const_iterator it = actual.begin();
 	LONGS_EQUAL(L(1), *(it++));
 	LONGS_EQUAL(X(1), *(it++));
 	LONGS_EQUAL(X(2), *(it++));