Added invert() to Ordering to enable decoding of linearized factors, changed all target back to use workspace build settings (such as not -j5)

.cproject

@@ -713,6 +713,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="nonlinear.testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>nonlinear.testOrdering.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="tests/testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -2074,10 +2082,10 @@
 			</target>
 			<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments>-j5 VERBOSE=1</buildArguments>
+				<buildArguments>-j5</buildArguments>
 				<buildTarget>all</buildTarget>
 				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
+				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
 			<target name="cmake" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">

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<class V2>
+bool assert_container_equality(const std::map<size_t,V2>& expected, const std::map<size_t,V2>& actual) {
+	typedef typename std::map<size_t,V2> 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==
  */

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() << "):";

gtsam/nonlinear/Ordering.h

@@ -40,6 +40,7 @@ protected:
 
 public:
 
+  typedef std::map<Index, Key> InvertedMap;
   typedef boost::shared_ptr<Ordering> shared_ptr;
 
   typedef std::pair<const Key, Index> 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
 	/// @{

gtsam/nonlinear/tests/testOrdering.cpp

@@ -18,6 +18,7 @@
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/nonlinear/Ordering.h>
 
+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); }
 /* ************************************************************************* */