diff --git a/gtsam/nonlinear/Ordering.cpp b/gtsam/nonlinear/Ordering.cpp index 4f3d71bee..8733af955 100644 --- a/gtsam/nonlinear/Ordering.cpp +++ b/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() << "):"; diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h index 1be51df3e..be2992503 100644 --- a/gtsam/nonlinear/Ordering.h +++ b/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 diff --git a/gtsam/nonlinear/tests/testOrdering.cpp b/gtsam/nonlinear/tests/testOrdering.cpp index ac4a57b40..64f5e7f4c 100644 --- a/gtsam/nonlinear/tests/testOrdering.cpp +++ b/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;