16 #include <boost/tuple/tuple.hpp> 17 #include <boost/shared_array.hpp> 30 namespace gtsam {
namespace partition {
45 std::vector<idx_t>
vwgt;
49 sharedInts part_(
new idx_t[n]);
57 printf(
"**********************************************************************\n");
58 printf(
"Graph Information ---------------------------------------------------\n");
59 printf(
" #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+
n) / 2);
60 printf(
"\nND Partitioning... -------------------------------------------\n");
72 printf(
" Sep size: \t\t %d\n", sepsize);
73 printf(
"**********************************************************************\n");
76 return std::make_pair(sepsize, part_);
94 graph =
SetupGraph(ctrl, *nvtxs, 1, xadj, adjncy, vwgt,
nullptr,
nullptr);
105 for (i=0; i<
ncon; i++) {
107 tpwgts2[ncon+
i] = 1.0 - tpwgts2[
i];
116 std::cout <<
"Finished bisection:" << *edgecut << std::endl;
132 std::vector<idx_t>
vwgt;
136 sharedInts part_(
new idx_t[n]);
144 printf(
"**********************************************************************\n");
145 printf(
"Graph Information ---------------------------------------------------\n");
146 printf(
" #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+
n) / 2);
147 printf(
"\nND Partitioning... -------------------------------------------\n");
161 printf(
"\nTiming Information --------------------------------------------------\n");
163 printf(
" Edge cuts: \t\t %d\n", edgecut);
164 printf(
"**********************************************************************\n");
167 return std::make_pair(edgecut, part_);
176 template <
class GenericGraph>
178 sharedInts* ptr_xadj, sharedInts* ptr_adjncy, sharedInts* ptr_adjwgt) {
180 typedef std::vector<int> Weights;
181 typedef std::vector<int> Neighbors;
182 typedef std::pair<Neighbors, Weights> NeighborsInfo;
185 std::vector<int>& dictionary = workspace.
dictionary;
189 int numNodes = keys.size();
191 std::vector<NeighborsInfo> adjacencyMap;
192 adjacencyMap.resize(numNodes);
193 std::cout <<
"Number of nodes: " << adjacencyMap.size() << std::endl;
196 for(
const typename GenericGraph::value_type& factor: graph){
197 index1 = dictionary[factor->key1.index];
198 index2 = dictionary[factor->key2.index];
199 std::cout <<
"index1: " << index1 << std::endl;
200 std::cout <<
"index2: " << index2 << std::endl;
202 if (index1 >= 0 && index2 >= 0) {
203 std::pair<Neighbors, Weights>& adjacencyMap1 = adjacencyMap[index1];
204 std::pair<Neighbors, Weights>& adjacencyMap2 = adjacencyMap[index2];
206 adjacencyMap1.first.push_back(index2);
207 adjacencyMap1.second.push_back(factor->weight);
208 adjacencyMap2.first.push_back(index1);
209 adjacencyMap2.second.push_back(factor->weight);
210 }
catch(std::exception&
e){
211 std::cout << e.what() << std::endl;
221 sharedInts&
xadj = *ptr_xadj;
222 sharedInts&
adjncy = *ptr_adjncy;
223 sharedInts&
adjwgt = *ptr_adjwgt;
224 int ind_xadj = 0, ind_adjncy = 0;
225 for(
const NeighborsInfo&
info: adjacencyMap) {
226 *(xadj.get() + ind_xadj) = ind_adjncy;
227 std::copy(
info.first .begin(),
info.first .end(), adjncy.get() + ind_adjncy);
228 std::copy(
info.second.begin(),
info.second.end(), adjwgt.get() + ind_adjncy);
229 assert(
info.first.size() ==
info.second.size());
230 ind_adjncy +=
info.first.size();
233 if (ind_xadj != numNodes)
throw std::runtime_error(
"prepareMetisGraph_: ind_xadj != numNodes");
234 *(xadj.get() + ind_xadj) = ind_adjncy;
238 template<
class GenericGraph>
242 size_t numKeys = keys.size();
244 std::cout << graph.size() <<
" factors,\t" << numKeys <<
" nodes;\t" << std::endl;
253 boost::tie(sepsize, part) =
separatorMetis(numKeys, xadj, adjncy, adjwgt, verbose);
254 if (!sepsize)
return boost::optional<MetisResult>();
259 result.C.reserve(sepsize);
260 result.A.reserve(numKeys - sepsize);
261 result.B.reserve(numKeys - sepsize);
262 int* ptr_part = part.get();
263 std::vector<size_t>::const_iterator itKey = keys.begin();
264 std::vector<size_t>::const_iterator itKeyLast = keys.end();
265 while(itKey != itKeyLast) {
266 switch(*(ptr_part++)) {
267 case 0: result.A.push_back(*(itKey++));
break;
268 case 1: result.B.push_back(*(itKey++));
break;
269 case 2: result.C.push_back(*(itKey++));
break;
270 default:
throw std::runtime_error(
"separatorPartitionByMetis: invalid results from Metis ND!");
275 std::cout <<
"total key: " << keys.size()
276 <<
" result(A,B,C) = " << result.A.size() <<
", " << result.B.size() <<
", " 277 << result.C.size() <<
"; sepsize from Metis = " << sepsize << std::endl;
281 if(result.C.size() !=
sepsize) {
282 std::cout <<
"total key: " << keys.size()
283 <<
" result(A,B,C) = " << result.A.size() <<
", " << result.B.size() <<
", " << result.C.size()
284 <<
"; sepsize from Metis = " << sepsize << std::endl;
285 throw std::runtime_error(
"separatorPartitionByMetis: invalid sepsize from Metis ND!");
292 template<
class GenericGraph>
297 if (graph.size() == 1 && keys.size() == 2) {
299 result.
A.push_back(keys.front());
300 result.
B.push_back(keys.back());
305 size_t numKeys = keys.size();
306 if (verbose) std::cout << graph.size() <<
" factors,\t" << numKeys <<
" nodes;\t" << std::endl;
313 boost::tie(edgecut, part) =
edgeMetis(numKeys, xadj, adjncy, adjwgt, verbose);
317 result.
A.reserve(numKeys);
318 result.
B.reserve(numKeys);
319 int* ptr_part = part.get();
320 std::vector<size_t>::const_iterator itKey = keys.begin();
321 std::vector<size_t>::const_iterator itKeyLast = keys.end();
322 while(itKey != itKeyLast) {
323 if (*ptr_part != 0 && *ptr_part != 1)
324 std::cout << *ptr_part <<
"!!!" << std::endl;
325 switch(*(ptr_part++)) {
326 case 0: result.
A.push_back(*(itKey++));
break;
327 case 1: result.
B.push_back(*(itKey++));
break;
328 default:
throw std::runtime_error(
"edgePartitionByMetis: invalid results from Metis ND!");
333 std::cout <<
"the size of two submaps in the reduced graph: " << result.
A.size()
334 <<
" " << result.
B.size() << std::endl;
337 for(
const typename GenericGraph::value_type& factor: graph){
338 int key1 = factor->key1.index;
339 int key2 = factor->key2.index;
342 if (std::find(result.
A.begin(), result.
A.end(),
key1) != result.
A.end()) std::cout <<
"A ";
343 if (std::find(result.
B.begin(), result.
B.end(),
key1) != result.
B.end()) std::cout <<
"B ";
346 if (std::find(result.
A.begin(), result.
A.end(),
key2) != result.
A.end()) std::cout <<
"A ";
347 if (std::find(result.
B.begin(), result.
B.end(),
key2) != result.
B.end()) std::cout <<
"B ";
348 std::cout <<
"weight " << factor->weight;;
351 if ((std::find(result.
A.begin(), result.
A.end(),
key1) != result.
A.end() &&
352 std::find(result.
B.begin(), result.
B.end(),
key2) != result.
B.end()) ||
353 (std::find(result.
B.begin(), result.
B.end(),
key1) != result.
B.end() &&
354 std::find(result.
A.begin(), result.
A.end(),
key2) != result.
A.end())){
356 std::cout <<
" CUT ";
358 std::cout << std::endl;
360 std::cout <<
"edgeCut: " << edgeCut << std::endl;
367 bool isLargerIsland(
const std::vector<size_t>& island1,
const std::vector<size_t>& island2) {
368 return island1.size() > island2.size();
374 std::cout <<
"island: ";
375 for(
const size_t key: island)
376 std::cout <<
key <<
" ";
377 std::cout << std::endl;
381 for(
const std::vector<std::size_t>& island: islands)
386 int numCamera = 0, numLandmark = 0;
387 for(
const size_t key: keys)
388 if (int2symbol[
key].chr() ==
'x')
392 std::cout <<
"numCamera: " << numCamera <<
" numLandmark: " << numLandmark << std::endl;
396 template<
class GenericGraph>
401 std::vector<size_t>&
A = partitionResult.
A;
402 std::vector<size_t>&
B = partitionResult.
B;
403 std::vector<size_t>&
C = partitionResult.
C;
404 std::vector<int>& dictionary = workspace.
dictionary;
405 std::fill(dictionary.begin(), dictionary.end(), -1);
406 for(
const size_t a: A)
408 for(
const size_t b: B)
411 throw std::runtime_error(
"addLandmarkToPartitionResult: C is not empty");
415 for(
const typename GenericGraph::value_type& factor: graph) {
416 i = factor->key1.index;
417 j = factor->key2.index;
418 if (dictionary[j] == 0)
420 else if (dictionary[j] == -1)
421 dictionary[
j] = dictionary[
i];
423 if (dictionary[j] != dictionary[i])
430 for(
const size_t j: landmarkKeys) {
431 switch(dictionary[j]) {
432 case 0: C.push_back(j);
break;
433 case 1: A.push_back(j);
break;
434 case 2: B.push_back(j);
break;
435 default: std::cout << j <<
": " << dictionary[
j] << std::endl;
436 throw std::runtime_error(
"addLandmarkToPartitionResult: wrong status for landmark");
441 #define REDUCE_CAMERA_GRAPH 444 template<
class GenericGraph>
447 const boost::optional<std::vector<Symbol> >& int2symbol,
const bool reduceGraph) {
448 boost::optional<MetisResult>
result;
449 GenericGraph reducedGraph;
450 std::vector<size_t> keyToPartition;
451 std::vector<size_t> cameraKeys, landmarkKeys;
453 if (!int2symbol.is_initialized())
454 throw std::invalid_argument(
"findSeparator: int2symbol must be valid!");
457 cameraKeys.reserve(keys.size());
458 landmarkKeys.reserve(keys.size());
459 for(
const size_t key: keys) {
460 if((*int2symbol)[
key].chr() ==
'x')
461 cameraKeys.push_back(
key);
463 landmarkKeys.push_back(
key);
466 keyToPartition = cameraKeys;
468 const std::vector<int>& dictionary = workspace.
dictionary;
470 std::cout <<
"original graph: V" << keys.size() <<
", E" << graph.size()
471 <<
" --> reduced graph: V" << cameraKeys.size() <<
", E" << reducedGraph.size() << std::endl;
476 if (!result.is_initialized()) {
477 std::cout <<
"metis failed!" << std::endl;
483 std::cout <<
"the separator size: " << result->C.size() <<
" landmarks" << std::endl;
490 template<
class GenericGraph>
493 const boost::optional<std::vector<Symbol> >& int2symbol,
const bool reduceGraph,
494 const int minNrConstraintsPerCamera,
const int minNrConstraintsPerLandmark) {
497 verbose, int2symbol, reduceGraph);
500 typedef std::vector<size_t> Island;
501 std::list<Island> islands;
503 std::list<Island> islands_in_A =
findIslands(graph, result->A, workspace,
504 minNrConstraintsPerCamera, minNrConstraintsPerLandmark);
506 std::list<Island> islands_in_B =
findIslands(graph, result->B, workspace,
507 minNrConstraintsPerCamera, minNrConstraintsPerLandmark);
509 islands.insert(islands.end(), islands_in_A.begin(), islands_in_A.end());
510 islands.insert(islands.end(), islands_in_B.begin(), islands_in_B.end());
512 size_t numIsland0 = islands.size();
531 size_t oldSize = islands.size();
533 if (islands.size() < 2) {
534 std::cout <<
"numIsland: " << numIsland0 << std::endl;
535 throw std::runtime_error(
"findSeparator: found fewer than 2 submaps!");
538 std::list<Island>::reference island = islands.back();
539 if ((
int)island.size() >= minNodesPerMap)
break;
540 result->C.insert(result->C.end(), island.begin(), island.end());
543 if (islands.size() != oldSize){
544 if (verbose) std::cout << oldSize <<
"-" << oldSize - islands.size() <<
" submap(s);\t" << std::endl;
547 if (verbose) std::cout << oldSize <<
" submap(s);\t" << std::endl;
552 std::fill(partitionTable.begin(), partitionTable.end(), -1);
553 for(
const size_t key: result->C)
554 partitionTable[
key] = 0;
556 for(
const Island& island: islands) {
558 for(
const size_t key: island) {
559 partitionTable[
key] = idx;
563 return islands.size();
list< vector< size_t > > findIslands(const GenericGraph2D &graph, const vector< size_t > &keys, WorkSpace &workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark)
idx_t idx_t idx_t idx_t * vwgt
idx_t idx_t idx_t idx_t idx_t * sepsize
idx_t idx_t idx_t idx_t idx_t idx_t idx_t real_t real_t idx_t idx_t idx_t * part
boost::shared_array< idx_t > sharedInts
boost::optional< MetisResult > findPartitoning(const GenericGraph &graph, const std::vector< size_t > &keys, WorkSpace &workspace, bool verbose, const boost::optional< std::vector< Symbol > > &int2symbol, const bool reduceGraph)
std::pair< int, sharedInts > edgeMetis(idx_t n, const sharedInts &xadj, const sharedInts &adjncy, const sharedInts &adjwgt, bool verbose)
void printIsland(const std::vector< size_t > &island)
int METIS_ComputeVertexSeparator(idx_t *nvtxs, idx_t *xadj, idx_t *adjncy, idx_t *vwgt, idx_t *options, idx_t *r_sepsize, idx_t *part)
NonlinearFactorGraph graph
int METIS_SetDefaultOptions(idx_t *options)
const Symbol key1('v', 1)
PartitionTable partitionTable
#define AllocateWorkSpace
idx_t idx_t idx_t idx_t idx_t idx_t idx_t real_t real_t idx_t idx_t * edgecut
bool isLargerIsland(const std::vector< size_t > &island1, const std::vector< size_t > &island2)
void addLandmarkToPartitionResult(const GenericGraph &graph, const std::vector< size_t > &landmarkKeys, MetisResult &partitionResult, WorkSpace &workspace)
idx_t idx_t idx_t idx_t idx_t idx_t idx_t real_t real_t idx_t * options
void reduceGenericGraph(const GenericGraph3D &graph, const std::vector< size_t > &cameraKeys, const std::vector< size_t > &landmarkKeys, const std::vector< int > &dictionary, GenericGraph3D &reducedGraph)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
std::vector< int > dictionary
boost::optional< MetisResult > separatorPartitionByMetis(const GenericGraph &graph, const std::vector< size_t > &keys, WorkSpace &workspace, bool verbose)
Matrix< Scalar, Dynamic, Dynamic > C
idx_t idx_t idx_t idx_t idx_t idx_t * adjwgt
void prepareMetisGraph(const GenericGraph &graph, const std::vector< size_t > &keys, WorkSpace &workspace, sharedInts *ptr_xadj, sharedInts *ptr_adjncy, sharedInts *ptr_adjwgt)
void modefied_EdgeComputeSeparator(idx_t *nvtxs, idx_t *xadj, idx_t *adjncy, idx_t *vwgt, idx_t *adjwgt, idx_t *options, idx_t *edgecut, idx_t *part)
std::pair< int, sharedInts > separatorMetis(idx_t n, const sharedInts &xadj, const sharedInts &adjncy, const sharedInts &adjwgt, bool verbose)
const Symbol key2('v', 2)
idx_t idx_t idx_t * adjncy
void printNumCamerasLandmarks(const std::vector< size_t > &keys, const std::vector< Symbol > &int2symbol)
void prepareDictionary(const std::vector< size_t > &keys)
int findSeparator(const GenericGraph &graph, const std::vector< size_t > &keys, const int minNodesPerMap, WorkSpace &workspace, bool verbose, const boost::optional< std::vector< Symbol > > &int2symbol, const bool reduceGraph, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark)
void printIslands(const std::list< std::vector< size_t > > &islands)
boost::optional< MetisResult > edgePartitionByMetis(const GenericGraph &graph, const std::vector< size_t > &keys, WorkSpace &workspace, bool verbose)