diff --git a/.cproject b/.cproject index 09812db7d..a157d6426 100644 --- a/.cproject +++ b/.cproject @@ -713,6 +713,14 @@ true true + + make + -j5 + nonlinear.testOrdering.run + true + true + true + make -j2 @@ -2074,10 +2082,10 @@ make - -j5 VERBOSE=1 + -j5 all true - false + true true diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index bf96c5d48..dad1c6d20 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -255,6 +255,45 @@ bool assert_container_equal(const V& expected, const V& actual, double tol = 1e- return true; } +/** + * Function for comparing maps of size_t->testable + * Types are assumed to have operator == + */ +template +bool assert_container_equality(const std::map& expected, const std::map& actual) { + typedef typename std::map Map; + bool match = true; + if (expected.size() != actual.size()) + match = false; + typename Map::const_iterator + itExp = expected.begin(), + itAct = actual.begin(); + if(match) { + for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { + if (itExp->first != itAct->first || itExp->second != itAct->second) { + match = false; + break; + } + } + } + if(!match) { + std::cout << "expected: " << std::endl; + BOOST_FOREACH(const typename Map::value_type& a, expected) { + std::cout << "Key: " << a.first << std::endl; + std::cout << "Value: " << a.second << std::endl; + } + std::cout << "\nactual: " << std::endl; + BOOST_FOREACH(const typename Map::value_type& a, actual) { + std::cout << "Key: " << a.first << std::endl; + std::cout << "Value: " << a.second << std::endl; + } + std::cout << std::endl; + return false; + } + return true; +} + + /** * General function for comparing containers of objects with operator== */ diff --git a/gtsam/nonlinear/Ordering.cpp b/gtsam/nonlinear/Ordering.cpp index 10c3370cf..14be537d3 100644 --- a/gtsam/nonlinear/Ordering.cpp +++ b/gtsam/nonlinear/Ordering.cpp @@ -85,6 +85,14 @@ Index Ordering::pop_back(Key key) { } } +/* ************************************************************************* */ +Ordering::InvertedMap Ordering::invert() const { + InvertedMap result; + BOOST_FOREACH(const value_type& p, *this) + result.insert(make_pair(p.second, p.first)); + return result; +} + /* ************************************************************************* */ void Unordered::print(const string& s) const { cout << s << " (" << size() << "):"; diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h index fcb3d5bfb..37f0fe106 100644 --- a/gtsam/nonlinear/Ordering.h +++ b/gtsam/nonlinear/Ordering.h @@ -40,6 +40,7 @@ protected: public: + typedef std::map InvertedMap; typedef boost::shared_ptr shared_ptr; typedef std::pair value_type; @@ -203,6 +204,12 @@ public: /// Synonym for operator[](Key) Index& at(Key key) { return operator[](key); } + /** + * Create an inverse mapping from Index->Key, useful for decoding linear systems + * @return inverse mapping structure + */ + InvertedMap invert() const; + /// @} /// @name Testable /// @{ diff --git a/gtsam/nonlinear/tests/testOrdering.cpp b/gtsam/nonlinear/tests/testOrdering.cpp index 8c1804016..f57e3281a 100644 --- a/gtsam/nonlinear/tests/testOrdering.cpp +++ b/gtsam/nonlinear/tests/testOrdering.cpp @@ -18,6 +18,7 @@ #include #include +using namespace std; using namespace gtsam; /* ************************************************************************* */ @@ -51,6 +52,25 @@ TEST( testOrdering, simple_modifications ) { EXPECT(assert_equal(expectedFinal, ordering)); } +/* ************************************************************************* */ +TEST( testOrdering, invert ) { + // creates a map with the opposite mapping: Index->Key + Ordering ordering; + + // create an ordering + Symbol x1('x', 1), x2('x', 2), x3('x', 3), x4('x', 4); + ordering += x1, x2, x3, x4; + + Ordering::InvertedMap actual = ordering.invert(); + Ordering::InvertedMap expected; + expected.insert(make_pair(0, x1)); + expected.insert(make_pair(1, x2)); + expected.insert(make_pair(2, x3)); + expected.insert(make_pair(3, x4)); + + EXPECT(assert_container_equality(expected, actual)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */