86 lines
2.9 KiB
C++
86 lines
2.9 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file MetisIndex-inl.h
|
|
* @author Andrew Melim
|
|
* @date Oct. 10, 2014
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <boost/foreach.hpp>
|
|
#include <map>
|
|
#include <vector>
|
|
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
template<class FACTOR>
|
|
void MetisIndex::augment(const FactorGraph<FACTOR>& factors) {
|
|
std::map<int32_t, std::set<int32_t> > iAdjMap; // Stores a set of keys that are adjacent to key x, with adjMap.first
|
|
std::map<int32_t, std::set<int32_t> >::iterator iAdjMapIt;
|
|
std::set<Key> keySet;
|
|
|
|
/* ********** Convert to CSR format ********** */
|
|
// Assuming that vertex numbering starts from 0 (C style),
|
|
// then the adjacency list of vertex i is stored in array adjncy
|
|
// 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]).
|
|
int32_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) {
|
|
// Store both in Key and int32_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();
|
|
|
|
xadj_.push_back(0); // Always set the first index to zero
|
|
for (iAdjMapIt = iAdjMap.begin(); iAdjMapIt != iAdjMap.end(); ++iAdjMapIt) {
|
|
std::vector<int32_t> temp;
|
|
// Copy from the FastSet into a temporary vector
|
|
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);
|
|
xadj_.push_back((int32_t) adj_.size());
|
|
}
|
|
}
|
|
|
|
} // \ gtsam
|