Added a test for Ordering, added pop_back() to ordering, made changing the ordering in NonlinearISAM possible

gtsam/nonlinear/Makefile.am

@@ -25,7 +25,7 @@ headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
 headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h NonlinearOptimizationParameters.h
 headers += NonlinearFactor.h
 sources += NonlinearOptimizer.cpp Ordering.cpp
-check_PROGRAMS += tests/testKey
+check_PROGRAMS += tests/testKey tests/testOrdering
 
 # Nonlinear iSAM
 headers += NonlinearISAM.h NonlinearISAM-inl.h

gtsam/nonlinear/NonlinearISAM.h

@@ -71,6 +71,12 @@ public:
 	/** Relinearization and reordering of variables */
 	void reorder_relinearize();
 
+	/** manually add a key to the end of the ordering */
+	void addKey(const Symbol& key) { ordering_.push_back(key); }
+
+	/** replace the current ordering */
+	void setOrdering(const Ordering& new_ordering) { ordering_ = new_ordering; }
+
 	// access
 
 	/** access the underlying bayes tree */

gtsam/nonlinear/Ordering.cpp

@@ -56,6 +56,20 @@ bool Ordering::equals(const Ordering& rhs, double tol) const {
   return order_ == rhs.order_;
 }
 
+/* ************************************************************************* */
+Ordering::value_type Ordering::pop_back() {
+	// FIXME: is there a way of doing this without searching over the entire structure?
+	for (iterator it=begin(); it!=end(); ++it) {
+		if (it->second == nVars_ - 1) {
+			value_type result = *it;
+			order_.erase(it);
+			--nVars_;
+			return result;
+		}
+	}
+	return value_type();
+}
+
 /* ************************************************************************* */
 void Unordered::print(const string& s) const {
   cout << s << " (" << size() << "):";

gtsam/nonlinear/Ordering.h

@@ -109,6 +109,9 @@ public:
 
   Index push_back(const Symbol& key) { return insert(std::make_pair(key, nVars_))->second; }
 
+  /** remove the last symbol/index pair from the ordering */
+  value_type pop_back();
+
   /**
    * += operator allows statements like 'ordering += x0,x1,x2,x3;', which are
    * very useful for unit tests.  This functionality is courtesy of

gtsam/nonlinear/tests/testOrdering.cpp

@@ -0,0 +1,40 @@
+/**
+ * @file testOrdering
+ * @author Alex Cunningham
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/nonlinear/Ordering.h>
+
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST( testOrdering, simple_modifications ) {
+	Ordering ordering;
+
+	// create an ordering
+	Symbol x1('x', 1), x2('x', 2), x3('x', 3), x4('x', 4);
+	ordering += x1, x2, x3, x4;
+
+	// pop the last two elements
+	Ordering::value_type x4p = ordering.pop_back();
+	EXPECT_LONGS_EQUAL(3, ordering.size());
+	EXPECT(assert_equal(x4, x4p.first));
+
+	Ordering::value_type x3p = ordering.pop_back();
+	EXPECT_LONGS_EQUAL(2, ordering.size());
+	EXPECT(assert_equal(x3, x3p.first));
+
+	// reassemble back make the ordering 1, 2, 4, 3
+	ordering.push_back(x4p.first);
+	ordering.push_back(x3p.first);
+
+	// verify
+	Ordering expectedFinal;
+	expectedFinal += x1, x2, x4, x3;
+	EXPECT(assert_equal(expectedFinal, ordering));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */

tests/testNonlinearISAM.cpp

@@ -43,6 +43,23 @@ TEST(testNonlinearISAM, markov_chain ) {
 		PoseKey key1(i-1), key2(i);
 		new_factors.addOdometry(key1, key2, z, model);
 		Values new_init;
+
+		// perform a check on changing orderings
+		if (i == 5) {
+			Ordering ordering = isam.getOrdering();
+
+			// swap last two elements
+			Symbol last = ordering.pop_back().first;
+			Symbol secondLast = ordering.pop_back().first;
+			ordering.push_back(last);
+			ordering.push_back(secondLast);
+			isam.setOrdering(ordering);
+
+			Ordering expected; expected += PoseKey(0), PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(3);
+			EXPECT(assert_equal(expected, isam.getOrdering()));
+		}
+
+
 		cur_pose = cur_pose.compose(z);
 		new_init.insert(key2, cur_pose.expmap(sampler.sample()));
 		expected.insert(key2, cur_pose);