Added find function and more efficient version of pop_back to Ordering

gtsam/nonlinear/Ordering.cpp

@@ -70,6 +70,22 @@ Ordering::value_type Ordering::pop_back() {
 	return value_type();
 }
 
+/* ************************************************************************* */
+Index Ordering::pop_back(const Symbol& key) {
+  Map::iterator item = order_.find(key);
+  if(item == order_.end()) {
+    throw invalid_argument("Attempting to remove a key from an ordering that does not contain that key");
+  } else {
+    if(item->second != nVars_ - 1) {
+      throw invalid_argument("Attempting to remove a key from an ordering in which that key is not last");
+    } else {
+      order_.erase(item);
+      -- nVars_;
+      return nVars_;
+    }
+  }
+}
+
 /* ************************************************************************* */
 void Unordered::print(const string& s) const {
   cout << s << " (" << size() << "):";

gtsam/nonlinear/Ordering.h

@@ -78,6 +78,22 @@ public:
   Index operator[](const Symbol& key) const {
     const_iterator i=order_.find(key); assert(i != order_.end()); return i->second; }
 
+  /** Returns an iterator pointing to the symbol/index pair with the requested,
+   * or the end iterator if it does not exist.
+   *
+   * @return An iterator pointing to the symbol/index pair with the requested,
+   * or the end iterator if it does not exist.
+   */
+  iterator find(const Symbol& key) { return order_.find(key); }
+
+  /** Returns an iterator pointing to the symbol/index pair with the requested,
+   * or the end iterator if it does not exist.
+   *
+   * @return An iterator pointing to the symbol/index pair with the requested,
+   * or the end iterator if it does not exist.
+   */
+  const_iterator find(const Symbol& key) const { return order_.find(key); }
+
   // adding symbols
 
   /**
@@ -105,9 +121,21 @@ public:
 
   Index push_back(const Symbol& key) { return insert(std::make_pair(key, nVars_))->second; }
 
-  /** remove the last symbol/index pair from the ordering */
+  /** Remove the last symbol/index pair from the ordering - this is inefficient.
+   * If you already know the last-ordered symbol, call the version of pop_back
+   * that accepts this symbol as an argument.
+   *
+   * @return The symbol and index that were removed.
+   */
   value_type pop_back();
 
+  /** Remove the last-ordered symbol from the ordering.  This is efficient, as
+   * opposed to the no-argument version of pop_back.
+   *
+   * @return The index of the symbol that was removed.
+   */
+  Index pop_back(const Symbol& key);
+
   /**
    * += 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

@@ -32,13 +32,17 @@ TEST( testOrdering, simple_modifications ) {
 	EXPECT_LONGS_EQUAL(3, ordering.size());
 	EXPECT(assert_equal(x4, x4p.first));
 
-	Ordering::value_type x3p = ordering.pop_back();
+	Index x3p = ordering.pop_back(x3);
 	EXPECT_LONGS_EQUAL(2, ordering.size());
-	EXPECT(assert_equal(x3, x3p.first));
+	EXPECT_LONGS_EQUAL(2, (int)x3p);
+
+	// try to pop an element that doesn't exist and isn't last
+	CHECK_EXCEPTION(ordering.pop_back(x4), std::invalid_argument);
+	CHECK_EXCEPTION(ordering.pop_back(x1), std::invalid_argument);
 
 	// reassemble back make the ordering 1, 2, 4, 3
-	ordering.push_back(x4p.first);
-	ordering.push_back(x3p.first);
+	ordering.push_back(x4);
+	ordering.push_back(x3);
 
 	// verify
 	Ordering expectedFinal;