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); }
/* ************************************************************************* */