In progress removing variables from iSAM2

gtsam/nonlinear/ISAM2-impl.cpp

@@ -63,6 +63,65 @@ void ISAM2::Impl::AddVariables(
   nodes.resize(ordering.nVars());
 }
 
+/* ************************************************************************* */
+void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, Values& theta, VariableIndex& variableIndex,
+   VectorValues& delta, VectorValues& deltaNewton, VectorValues& deltaGradSearch,
+   std::vector<bool>& replacedKeys, Ordering& ordering, Base::Nodes& nodes) {
+
+		 // 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() - unusedIndices.size());
+
+			 for(size_t j = 0; j < newNodes.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]];
+			 }
+
+			 // 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);
+		 }
+}
+
 /* ************************************************************************* */
 FastSet<Index> ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors) {
   FastSet<Index> indices;

gtsam/nonlinear/ISAM2-impl.h

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

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>
@@ -205,6 +206,93 @@ TEST_UNSAFE(ISAM2, AddVariables) {
   EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys));
   EXPECT(assert_equal(orderingExpected, ordering));
 }
+/* ************************************************************************* */
+TEST_UNSAFE(ISAM2, RemoveVariables) {
+
+	// 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);
+
+	// Expand 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);
+	ISAM2::Impl::RemoveVariables(unusedKeys, theta, variableIndex, delta, deltaNewton, deltaRg, replacedKeys, ordering, nodes);
+
+	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) {
@@ -1011,6 +1099,134 @@ TEST(ISAM2, removeFactors)
   EXPECT(assert_equal(expectedGradient, actualGradient));
 }
 
+/* ************************************************************************* */
+//TEST(ISAM2, removeVariables)
+//{
+//
+//	// 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;
+//
+//	vector<size_t> factorsToRemove;
+//
+//	// 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)));
+//
+//		ISAM2Result result = isam.update(newfactors, init);
+//		factorsToRemove.push_back(result.newFactorsIndices[1]);
+//		++ 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));
+//
+//		ISAM2Result result = isam.update(newfactors, init);
+//		factorsToRemove.push_back(result.newFactorsIndices[1]);
+//		++ i;
+//	}
+//
+//	// Compare solutions
+//	fullgraph.remove(factorsToRemove[0]);
+//	fullgraph.remove(factorsToRemove[1]);
+//	fullinit.erase(100);
+//	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));
+//}
+
 /* ************************************************************************* */
 TEST_UNSAFE(ISAM2, swapFactors)
 {