diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h index 08a0566a2..43bc7a111 100644 --- a/gtsam/inference/MetisIndex-inl.h +++ b/gtsam/inference/MetisIndex-inl.h @@ -29,7 +29,7 @@ namespace gtsam { { std::map > adjMap; std::map >::iterator adjMapIt; - std::set values; + std::set keySet; /* ********** Convert to CSR format ********** */ // Assuming that vertex numbering starts from 0 (C style), @@ -44,17 +44,22 @@ namespace gtsam { if (k1 != k2) adjMap[k1].insert(adjMap[k1].end(), k2); // Insert at the end } - values.insert(values.end(), k1); // Keep a track of all unique values + keySet.insert(keySet.end(), k1); // Keep a track of all unique keySet } } } - // Number of values referenced in this factorgraph - nValues_ = values.size(); - + // Number of keys referenced in this factor graph + nKeys_ = keySet.size(); + + + // Starting with a nonzero key crashes METIS + // Find the smallest key in the graph + size_t minKey = *keySet.begin(); // set is ordered + xadj_.push_back(0);// Always set the first index to zero for (adjMapIt = adjMap.begin(); adjMapIt != adjMap.end(); ++adjMapIt) { - std::vector temp; + std::vector temp; // Copy from the FastSet into a temporary vector std::copy(adjMapIt->second.begin(), adjMapIt->second.end(), std::back_inserter(temp)); // Insert each index's set in order by appending them to the end of adj_ @@ -62,6 +67,11 @@ namespace gtsam { //adj_.push_back(temp); xadj_.push_back(adj_.size()); } + + // Normalize, subtract the smallest key + std::transform(adj_.begin(), adj_.end(), adj_.begin(), std::bind2nd(std::minus(), minKey)); + + } } diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index 7bc595aba..6aaa9246d 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -43,18 +43,18 @@ public: private: FastVector xadj_; // Index of node's adjacency list in adj FastVector adj_; // Stores ajacency lists of all nodes, appended into a single vector - size_t nFactors_; // Number of factors in the original factor graph - size_t nValues_; // + size_t nFactors_; // Number of factors in the original factor graph + size_t nKeys_; // public: /// @name Standard Constructors /// @{ /** Default constructor, creates empty MetisIndex */ - MetisIndex() : nFactors_(0), nValues_(0) {} + MetisIndex() : nFactors_(0), nKeys_(0) {} template - MetisIndex(const FG& factorGraph) : nFactors_(0), nValues_(0) { + MetisIndex(const FG& factorGraph) : nFactors_(0), nKeys_(0) { augment(factorGraph); } ~MetisIndex(){} @@ -69,9 +69,9 @@ public: template void augment(const FactorGraph& factors); - std::vector xadj() const { return xadj_; } - std::vector adj() const { return adj_; } - size_t nValues() const { return nValues_; } + std::vector xadj() const { return xadj_; } + std::vector adj() const { return adj_; } + size_t nValues() const { return nKeys_; } /// @} }; diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 424eb7c19..f2a0e1443 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -139,7 +139,27 @@ TEST(Ordering, csr_format_2) { EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == mi.adj()); - //Ordering metis = Ordering::METIS(sfg); +} +/* ************************************************************************* */ + +TEST(Ordering, csr_format_3) { + SymbolicFactorGraph sfg; + + sfg.push_factor(100); + sfg.push_factor(100, 101); + sfg.push_factor(101, 102); + sfg.push_factor(102, 103); + sfg.push_factor(103, 104); + sfg.push_factor(104, 101); + + MetisIndex mi(sfg); + + vector xadjExpected{ 0, 1, 4, 6, 8, 10 }; + vector adjExpected{ 1, 0, 2, 4, 1, 3, 2, 4, 1, 3 }; + + EXPECT(xadjExpected == mi.xadj()); + EXPECT(adjExpected.size() == mi.adj().size()); + EXPECT(adjExpected == mi.adj()); }