diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am index 331f2758a..88f4d4a40 100644 --- a/gtsam/nonlinear/Makefile.am +++ b/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 diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h index 4a5c7f361..8c6dc6bbd 100644 --- a/gtsam/nonlinear/NonlinearISAM.h +++ b/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 */ diff --git a/gtsam/nonlinear/Ordering.cpp b/gtsam/nonlinear/Ordering.cpp index bababd71c..4f3d71bee 100644 --- a/gtsam/nonlinear/Ordering.cpp +++ b/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() << "):"; diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h index c81390aeb..37c9706f3 100644 --- a/gtsam/nonlinear/Ordering.h +++ b/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 diff --git a/gtsam/nonlinear/tests/testOrdering.cpp b/gtsam/nonlinear/tests/testOrdering.cpp new file mode 100644 index 000000000..477279da0 --- /dev/null +++ b/gtsam/nonlinear/tests/testOrdering.cpp @@ -0,0 +1,40 @@ +/** + * @file testOrdering + * @author Alex Cunningham + */ + +#include +#include + +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); } +/* ************************************************************************* */ diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index ea47b0b9a..e8e0341c7 100644 --- a/tests/testNonlinearISAM.cpp +++ b/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);