diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h index 006ba075f..b9272990a 100644 --- a/gtsam/inference/MetisIndex-inl.h +++ b/gtsam/inference/MetisIndex-inl.h @@ -27,8 +27,8 @@ namespace gtsam { template void MetisIndex::augment(const FactorGraph& factors) { - std::map > adjMap; - std::map >::iterator adjMapIt; + std::map > iAdjMap; // Stores a set of keys that are adjacent to key x, with adjMap.first + std::map >::iterator iAdjMapIt; std::set keySet; /* ********** Convert to CSR format ********** */ @@ -37,31 +37,43 @@ void MetisIndex::augment(const FactorGraph& factors) // starting at index xadj[i] and ending at(but not including) // index xadj[i + 1](i.e., adjncy[xadj[i]] through // and including adjncy[xadj[i + 1] - 1]). + idx_t keyCounter = 0; + + // First: Record a copy of each key inside the factorgraph and create a + // key to integer mapping. This is referenced during the adjaceny step + for (size_t i = 0; i < factors.size(); i++){ + if (factors[i]) + BOOST_FOREACH(const Key& key, *factors[i]){ + keySet.insert(keySet.end(), key); // Keep a track of all unique keys + if (intKeyBMap_.left.find(key) == intKeyBMap_.left.end()){ + intKeyBMap_.insert(bm_type::value_type(key, keyCounter)); + keyCounter++; + } + } + } + + // Create an adjacency mapping that stores the set of all adjacent keys for every key for (size_t i = 0; i < factors.size(); i++){ if (factors[i]){ - BOOST_FOREACH(const Key& k1, *factors[i]){ - BOOST_FOREACH(const Key& k2, *factors[i]){ - if (k1 != k2) - adjMap[k1].insert(adjMap[k1].end(), k2); // Insert at the end - } - keySet.insert(keySet.end(), k1); // Keep a track of all unique keySet - } + BOOST_FOREACH(const Key& k1, *factors[i]) + BOOST_FOREACH(const Key& k2, *factors[i]) + if (k1 != k2){ + // Store both in Key and idx_t format + int i = intKeyBMap_.left.at(k1); + int j = intKeyBMap_.left.at(k2); + iAdjMap[i].insert(iAdjMap[i].end(), j); + } } } // 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 - 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; + for (iAdjMapIt = iAdjMap.begin(); iAdjMapIt != iAdjMap.end(); ++iAdjMapIt) { + std::vector temp; // Copy from the FastSet into a temporary vector - std::copy(adjMapIt->second.begin(), adjMapIt->second.end(), std::back_inserter(temp)); + std::copy(iAdjMapIt->second.begin(), iAdjMapIt->second.end(), std::back_inserter(temp)); // Insert each index's set in order by appending them to the end of adj_ adj_.insert(adj_.end(), temp.begin(), temp.end()); //adj_.push_back(temp); diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index 476d34980..e3b7ea33d 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -25,12 +25,14 @@ #include #include #include +#include +#include namespace gtsam { /** * The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in * METIS algorithms. Specifically, two vectors store the adjacency structure of the graph. It is built - * fromt a factor graph prior to elimination, and stores the list of factors + * from a factor graph prior to elimination, and stores the list of factors * that involve each variable. * \nosubgrouping */ @@ -38,13 +40,15 @@ class GTSAM_EXPORT MetisIndex { public: typedef boost::shared_ptr shared_ptr; + typedef boost::bimap bm_type; 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 nKeys_; // - size_t minKey_; + FastVector xadj_; // Index of node's adjacency list in adj + FastVector adj_; // Stores ajacency lists of all nodes, appended into a single vector + FastVector iadj_; // Integer keys for passing into metis. One to one mapping with adj_; + boost::bimap intKeyBMap_; // Stores Key <-> integer value relationship + size_t nFactors_; // Number of factors in the original factor graph + size_t nKeys_; public: /// @name Standard Constructors @@ -69,10 +73,13 @@ public: template void augment(const FactorGraph& factors); - std::vector xadj() const { return xadj_; } - std::vector adj() const { return adj_; } - size_t nValues() const { return nKeys_; } - size_t minKey() const { return minKey_; } + std::vector xadj() const { return xadj_; } + std::vector adj() const { return adj_; } + size_t nValues() const { return nKeys_; } + Key intToKey(idx_t value) const { + assert(value >= 0); + return intKeyBMap_.right.find(value)->second; + } /// @} }; diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index dcc2bd00a..a069fd4f6 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -204,56 +204,20 @@ namespace gtsam { { gttic(Ordering_METIS); - vector xadj = met.xadj(); - vector adj = met.adj(); - Key minKey = met.minKey(); - - // TODO(Andrew): Debug - Key min = std::numeric_limits::max(); - for (int i = 0; i < adj.size(); i++) - { - if (adj[i] < min) - min = adj[i]; - } - - std::cout << "Min: " << min << " minkey: " << minKey << std::endl; - - // Normalize, subtract the smallest key - //std::transform(adj.begin(), adj.end(), adj.begin(), std::bind2nd(std::minus(), minKey)); - for (vector::iterator it = adj.begin(); it != adj.end(); ++it) - *it = *it - minKey; - - // Cast the adjacency formats into idx_t (int32) - // NOTE: Keys can store quite large values and hence may overflow during conversion to int - // It's important that the normalization is performed first. - vector adj_idx; - for (vector::iterator it = adj.begin(); it != adj.end(); ++it) - adj_idx.push_back(static_cast(*it)); - - vector xadj_idx; - for (vector::iterator it = xadj.begin(); it != xadj.end(); ++it) - xadj_idx.push_back(static_cast(*it)); - - // TODO(Andrew): Debug - for (int i = 0; i < adj.size(); i++) - { - assert(adj[i] >= 0); - if (adj[i] < 0) - std::cout << adj[i] << std::endl; - } - + vector xadj = met.xadj(); + vector adj = met.adj(); vector perm, iperm; - idx_t size = met.nValues(); - for (idx_t i = 0; i < size; i++) - { - perm.push_back(0); - iperm.push_back(0); - } + idx_t size = met.nValues(); + for (idx_t i = 0; i < size; i++) + { + perm.push_back(0); + iperm.push_back(0); + } int outputError; - outputError = METIS_NodeND(&size, &xadj_idx[0], &adj_idx[0], NULL, NULL, &perm[0], &iperm[0]); + outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0], &iperm[0]); Ordering result; if (outputError != METIS_OK) @@ -262,11 +226,11 @@ namespace gtsam { return result; } - result.resize(size); - for (size_t j = 0; j < size; ++j){ - // We have to add the minKey value back to obtain the original key in the Values - result[j] = (Key)perm[j] + minKey; - } + result.resize(size); + for (size_t j = 0; j < size; ++j){ + // We have to add the minKey value back to obtain the original key in the Values + result[j] = met.intToKey(perm[j]); + } return result; } diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 57de00646..5cd4c77b2 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -159,18 +159,18 @@ TEST(Ordering, csr_format_3) { vector xadjExpected, adjExpected; xadjExpected += 0, 1, 4, 6, 8, 10; adjExpected += 1, 0, 2, 4, 1, 3, 2, 4, 1, 3; - size_t minKey = mi.minKey(); + //size_t minKey = mi.minKey(); vector adjAcutal = mi.adj(); // Normalize, subtract the smallest key - std::transform(adjAcutal.begin(), adjAcutal.end(), adjAcutal.begin(), - std::bind2nd(std::minus(), minKey)); + //std::transform(adjAcutal.begin(), adjAcutal.end(), adjAcutal.begin(), + // std::bind2nd(std::minus(), minKey)); EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == adjAcutal); - + } /* ************************************************************************* */