diff --git a/gtsam/inference/OrderingUnordered.cpp b/gtsam/inference/OrderingUnordered.cpp index 6dacd66a3..72123df5e 100644 --- a/gtsam/inference/OrderingUnordered.cpp +++ b/gtsam/inference/OrderingUnordered.cpp @@ -27,6 +27,15 @@ using namespace std; namespace gtsam { + /* ************************************************************************* */ + FastMap OrderingUnordered::invert() const + { + FastMap 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) { diff --git a/gtsam/inference/OrderingUnordered.h b/gtsam/inference/OrderingUnordered.h index ab1f4dd25..9d359c946 100644 --- a/gtsam/inference/OrderingUnordered.h +++ b/gtsam/inference/OrderingUnordered.h @@ -32,10 +32,17 @@ namespace gtsam { /// Create an empty ordering GTSAM_EXPORT OrderingUnordered() {} + /// Create from a container + template + explicit OrderingUnordered(const KEYS& keys) : Base(keys.begin(), keys.end()) {} + /// Create an ordering using iterators over keys template OrderingUnordered(ITERATOR firstKey, ITERATOR lastKey) : Base(firstKey, lastKey) {} + /// Invert (not reverse) the ordering - returns a map from key to order position + FastMap 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&)