Added invert to OrderingUnordered

gtsam/inference/OrderingUnordered.cpp

@@ -27,6 +27,15 @@ using namespace std;
 
 namespace gtsam {
 
+  /* ************************************************************************* */
+  FastMap<Key, size_t> OrderingUnordered::invert() const
+  {
+    FastMap<Key, size_t> inverted;
+    for(size_t pos = 0; pos < this->size(); ++pos)
+      inverted.insert(make_pair((*this)[pos], pos));
+    return inverted;
+  }
+
   /* ************************************************************************* */
   OrderingUnordered OrderingUnordered::COLAMD(const VariableIndexUnordered& variableIndex)
   {

gtsam/inference/OrderingUnordered.h

@@ -32,10 +32,17 @@ namespace gtsam {
     /// Create an empty ordering
     GTSAM_EXPORT OrderingUnordered() {}
 
+    /// Create from a container
+    template<typename KEYS>
+    explicit OrderingUnordered(const KEYS& keys) : Base(keys.begin(), keys.end()) {}
+
     /// Create an ordering using iterators over keys
     template<typename ITERATOR>
     OrderingUnordered(ITERATOR firstKey, ITERATOR lastKey) : Base(firstKey, lastKey) {}
 
+    /// Invert (not reverse) the ordering - returns a map from key to order position
+    FastMap<Key, size_t> invert() const;
+
     /// Compute an ordering using COLAMD directly from a factor graph - this internally builds a
     /// VariableIndex so if you already have a VariableIndex, it is faster to use COLAMD(const
     /// VariableIndexUnordered&)