From 1d9aa38a62b651ecb1310f0549e26a8b923cfdc5 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Tue, 28 Jan 2014 19:38:28 -0500 Subject: [PATCH] Move partition over --- gtsam_unstable/partition/FindSeparator-inl.h | 545 ++++++++++++++++++ gtsam_unstable/partition/FindSeparator.h | 43 ++ gtsam_unstable/partition/GenericGraph.cpp | 476 +++++++++++++++ gtsam_unstable/partition/GenericGraph.h | 149 +++++ .../partition/NestedDissection-inl.h | 253 ++++++++ gtsam_unstable/partition/NestedDissection.h | 69 +++ gtsam_unstable/partition/PartitionWorkSpace.h | 43 ++ .../partition/testFindSeparator.cpp | 222 +++++++ gtsam_unstable/partition/testGenericGraph.cpp | 254 ++++++++ .../partition/testNestedDissection.cpp | 339 +++++++++++ 10 files changed, 2393 insertions(+) create mode 100644 gtsam_unstable/partition/FindSeparator-inl.h create mode 100644 gtsam_unstable/partition/FindSeparator.h create mode 100644 gtsam_unstable/partition/GenericGraph.cpp create mode 100644 gtsam_unstable/partition/GenericGraph.h create mode 100644 gtsam_unstable/partition/NestedDissection-inl.h create mode 100644 gtsam_unstable/partition/NestedDissection.h create mode 100644 gtsam_unstable/partition/PartitionWorkSpace.h create mode 100644 gtsam_unstable/partition/testFindSeparator.cpp create mode 100644 gtsam_unstable/partition/testGenericGraph.cpp create mode 100644 gtsam_unstable/partition/testNestedDissection.cpp diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h new file mode 100644 index 000000000..a9d83a198 --- /dev/null +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -0,0 +1,545 @@ +/* + * FindSeparator-inl.h + * + * Created on: Nov 23, 2010 + * Author: nikai + * Description: find the separator of bisectioning for a given graph + */ + +#pragma once + +#include +#include +#include +#include +#include + +extern "C" { +#include +} + +#include "FindSeparator.h" + +using namespace std; + +namespace gtsam { namespace partition { + + typedef boost::shared_array sharedInts; + + /* ************************************************************************* */ + /** + * Return the size of the separator and the partiion indices {part} + * Part [j] is 0, 1, or 2, depending on + * whether node j is in the left part of the graph, the right part, or the + * separator, respectively + */ + pair separatorMetis(int n, const sharedInts& xadj, const sharedInts& adjncy, const sharedInts& adjwgt, bool verbose) { + + // control parameters + idxtype vwgt[n]; // the weights of the vertices + int options[8]; options [0] = 0 ; // use defaults + int sepsize; // the size of the separator, output + sharedInts part_(new int[n]); // the partition of each vertex, output + + // set uniform weights on the vertices + std::fill(vwgt, vwgt+n, 1); + + timer TOTALTmr; + if (verbose) { + printf("**********************************************************************\n"); + printf("Graph Information ---------------------------------------------------\n"); + printf(" #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2); + printf("\nND Partitioning... -------------------------------------------\n"); + cleartimer(TOTALTmr); + starttimer(TOTALTmr); + } + + // call metis parition routine + METIS_NodeComputeSeparator(&n, xadj.get(), adjncy.get(), vwgt, adjwgt.get(), + options, &sepsize, part_.get()); + + if (verbose) { + stoptimer(TOTALTmr); + printf("\nTiming Information --------------------------------------------------\n"); + printf(" Total: \t\t %7.3f\n", gettimer(TOTALTmr)); + printf(" Sep size: \t\t %d\n", sepsize); + printf("**********************************************************************\n"); + } + + return make_pair(sepsize, part_); + } + + /* ************************************************************************* */ + void modefied_EdgeComputeSeparator(int *nvtxs, idxtype *xadj, idxtype *adjncy, idxtype *vwgt, + idxtype *adjwgt, int *options, int *edgecut, idxtype *part) + { + int i, j, tvwgt, tpwgts[2]; + GraphType graph; + CtrlType ctrl; + + SetUpGraph(&graph, OP_ONMETIS, *nvtxs, 1, xadj, adjncy, vwgt, adjwgt, 3); + tvwgt = idxsum(*nvtxs, graph.vwgt); + + if (options[0] == 0) { /* Use the default parameters */ + ctrl.CType = ONMETIS_CTYPE; + ctrl.IType = ONMETIS_ITYPE; + ctrl.RType = ONMETIS_RTYPE; + ctrl.dbglvl = ONMETIS_DBGLVL; + } + else { + ctrl.CType = options[OPTION_CTYPE]; + ctrl.IType = options[OPTION_ITYPE]; + ctrl.RType = options[OPTION_RTYPE]; + ctrl.dbglvl = options[OPTION_DBGLVL]; + } + + ctrl.oflags = 0; + ctrl.pfactor = 0; + ctrl.nseps = 1; + ctrl.optype = OP_OEMETIS; + ctrl.CoarsenTo = amin(100, *nvtxs-1); + ctrl.maxvwgt = 1.5*tvwgt/ctrl.CoarsenTo; + + InitRandom(options[7]); + + AllocateWorkSpace(&ctrl, &graph, 2); + + /*============================================================ + * Perform the bisection + *============================================================*/ + tpwgts[0] = tvwgt/2; + tpwgts[1] = tvwgt-tpwgts[0]; + + MlevelEdgeBisection(&ctrl, &graph, tpwgts, 1.05); + // ConstructMinCoverSeparator(&ctrl, &graph, 1.05); + *edgecut = graph.mincut; + // *sepsize = graph.pwgts[2]; + idxcopy(*nvtxs, graph.where, part); + + GKfree((void**)&graph.gdata, &graph.rdata, &graph.label, LTERM); + + + FreeWorkSpace(&ctrl, &graph); + + } + + /* ************************************************************************* */ + /** + * Return the number of edge cuts and the partiion indices {part} + * Part [j] is 0 or 1, depending on + * whether node j is in the left part of the graph or the right part respectively + */ + pair edgeMetis(int n, const sharedInts& xadj, const sharedInts& adjncy, const sharedInts& adjwgt, bool verbose) { + + // control parameters + idxtype vwgt[n]; // the weights of the vertices + int options[10]; options [0] = 1; options [1] = 3; options [2] = 1; options [3] = 1; options [4] = 0; // use defaults + int edgecut; // the number of edge cuts, output + sharedInts part_(new int[n]); // the partition of each vertex, output + + // set uniform weights on the vertices + std::fill(vwgt, vwgt+n, 1); + + timer TOTALTmr; + if (verbose) { + printf("**********************************************************************\n"); + printf("Graph Information ---------------------------------------------------\n"); + printf(" #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2); + printf("\nND Partitioning... -------------------------------------------\n"); + cleartimer(TOTALTmr); + starttimer(TOTALTmr); + } + + // call metis parition routine + int wgtflag = 1; // only edge weights + int numflag = 0; // c style numbering starting from 0 + int nparts = 2; // partition the graph to 2 submaps +// METIS_PartGraphRecursive(&n, xadj.get(), adjncy.get(), NULL, adjwgt.get(), &wgtflag, +// &numflag, &nparts, options, &edgecut, part_.get()); + + modefied_EdgeComputeSeparator(&n, xadj.get(), adjncy.get(), vwgt, adjwgt.get(), + options, &edgecut, part_.get()); + + if (verbose) { + stoptimer(TOTALTmr); + printf("\nTiming Information --------------------------------------------------\n"); + printf(" Total: \t\t %7.3f\n", gettimer(TOTALTmr)); + printf(" Edge cuts: \t\t %d\n", edgecut); + printf("**********************************************************************\n"); + } + + return make_pair(edgecut, part_); + } + + /* ************************************************************************* */ + /** + * Prepare the data structure {xadj} and {adjncy} required by metis + * xadj always has the size equal to the no. of the nodes plus 1 + * adjncy always has the size equal to two times of the no. of the edges in the Metis graph + */ + template + void prepareMetisGraph(const GenericGraph& graph, const vector& keys, WorkSpace& workspace, + sharedInts* ptr_xadj, sharedInts* ptr_adjncy, sharedInts* ptr_adjwgt) { + + typedef int Weight; + typedef vector Weights; + typedef vector Neighbors; + typedef pair NeighborsInfo; + + // set up dictionary + std::vector& dictionary = workspace.dictionary; + workspace.prepareDictionary(keys); + + // prepare for {adjancyMap}, a pair of neighbor indices and the correponding edge weights + int numNodes = keys.size(); + int numEdges = 0; + vector adjancyMap; // TODO: set is slow, but have to use it to remove duplicated edges + adjancyMap.resize(numNodes); + int index1, index2; + BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){ + index1 = dictionary[factor->key1.index]; + index2 = dictionary[factor->key2.index]; + if (index1 >= 0 && index2 >= 0) { // if both nodes are in the current graph, i.e. not a joint factor between frontal and separator + pair& adjancyMap1 = adjancyMap[index1]; + pair& adjancyMap2 = adjancyMap[index2]; + adjancyMap1.first .push_back(index2); + adjancyMap1.second.push_back(factor->weight); + adjancyMap2.first .push_back(index1); + adjancyMap2.second.push_back(factor->weight); + numEdges++; + } + } + + // prepare for {xadj}, {adjncy}, and {adjwgt} + *ptr_xadj = sharedInts(new int[numNodes+1]); + *ptr_adjncy = sharedInts(new int[numEdges*2]); + *ptr_adjwgt = sharedInts(new int[numEdges*2]); + sharedInts& xadj = *ptr_xadj; + sharedInts& adjncy = *ptr_adjncy; + sharedInts& adjwgt = *ptr_adjwgt; + int ind_xadj = 0, ind_adjncy = 0; + BOOST_FOREACH(const NeighborsInfo& info, adjancyMap) { + *(xadj.get() + ind_xadj) = ind_adjncy; + std::copy(info.first .begin(), info.first .end(), adjncy.get() + ind_adjncy); + std::copy(info.second.begin(), info.second.end(), adjwgt.get() + ind_adjncy); + assert(info.first.size() == info.second.size()); + ind_adjncy += info.first.size(); + ind_xadj ++; + } + if (ind_xadj != numNodes) throw std::runtime_error("prepareMetisGraph_: ind_xadj != numNodes"); + *(xadj.get() + ind_xadj) = ind_adjncy; + } + + /* ************************************************************************* */ + template + boost::optional separatorPartitionByMetis(const GenericGraph& graph, const vector& keys, WorkSpace& workspace, bool verbose) { + // create a metis graph + size_t numKeys = keys.size(); + if (verbose) cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << endl; + sharedInts xadj, adjncy, adjwgt; + prepareMetisGraph(graph, keys, workspace, &xadj, &adjncy, &adjwgt); + + // run ND on the graph + int sepsize; + sharedInts part; + boost::tie(sepsize, part) = separatorMetis(numKeys, xadj, adjncy, adjwgt, verbose); + if (!sepsize) return boost::optional(); + + // convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later we will have more submaps + MetisResult result; + result.C.reserve(sepsize); + result.A.reserve(numKeys - sepsize); + result.B.reserve(numKeys - sepsize); + int* ptr_part = part.get(); + vector::const_iterator itKey = keys.begin(); + vector::const_iterator itKeyLast = keys.end(); + while(itKey != itKeyLast) { + switch(*(ptr_part++)) { + case 0: result.A.push_back(*(itKey++)); break; + case 1: result.B.push_back(*(itKey++)); break; + case 2: result.C.push_back(*(itKey++)); break; + default: throw runtime_error("separatorPartitionByMetis: invalid results from Metis ND!"); + } + } + + if (verbose) { + cout << "total key: " << keys.size() + << " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " << result.C.size() + << "; sepsize from Metis = " << sepsize << endl; + throw runtime_error("separatorPartitionByMetis:stop for debug"); + } + + if(result.C.size() != sepsize) { + cout << "total key: " << keys.size() + << " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " << result.C.size() + << "; sepsize from Metis = " << sepsize << endl; + throw runtime_error("separatorPartitionByMetis: invalid sepsize from Metis ND!"); + } + + return boost::make_optional(result); + } + + /* ************************************************************************* */ + template + boost::optional edgePartitionByMetis(const GenericGraph& graph, const vector& keys, WorkSpace& workspace, bool verbose) { + + // a small hack for handling the camera1-camera2 case used in the unit tests + if (graph.size() == 1 && keys.size() == 2) { + MetisResult result; + result.A.push_back(keys.front()); + result.B.push_back(keys.back()); + return result; + } + + // create a metis graph + size_t numKeys = keys.size(); + if (verbose) cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << endl; + sharedInts xadj, adjncy, adjwgt; + prepareMetisGraph(graph, keys, workspace, &xadj, &adjncy, &adjwgt); + + // run metis on the graph + int edgecut; + sharedInts part; + boost::tie(edgecut, part) = edgeMetis(numKeys, xadj, adjncy, adjwgt, verbose); + + // convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later we will have more submaps + MetisResult result; + result.A.reserve(numKeys); + result.B.reserve(numKeys); + int* ptr_part = part.get(); + vector::const_iterator itKey = keys.begin(); + vector::const_iterator itKeyLast = keys.end(); + while(itKey != itKeyLast) { + if (*ptr_part != 0 && *ptr_part != 1) + cout << *ptr_part << "!!!" << endl; + switch(*(ptr_part++)) { + case 0: result.A.push_back(*(itKey++)); break; + case 1: result.B.push_back(*(itKey++)); break; + default: throw runtime_error("edgePartitionByMetis: invalid results from Metis ND!"); + } + } + + if (verbose) { + cout << "the size of two submaps in the reduced graph: " << result.A.size() << " " << result.B.size() << endl; + int edgeCut = 0; + + BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){ + int key1 = factor->key1.index; + int key2 = factor->key2.index; + // print keys and their subgraph assignment + cout << key1; + if (std::find(result.A.begin(), result.A.end(), key1) != result.A.end()) cout <<"A "; + if (std::find(result.B.begin(), result.B.end(), key1) != result.B.end()) cout <<"B "; + + cout << key2; + if (std::find(result.A.begin(), result.A.end(), key2) != result.A.end()) cout <<"A "; + if (std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) cout <<"B "; + cout << "weight " << factor->weight;; + + // find vertices that were assigned to sets A & B. Their edge will be cut + if ((std::find(result.A.begin(), result.A.end(), key1) != result.A.end() && + std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) || + (std::find(result.B.begin(), result.B.end(), key1) != result.B.end() && + std::find(result.A.begin(), result.A.end(), key2) != result.A.end())){ + edgeCut ++; + cout << " CUT "; + } + cout << endl; + } + cout << "edgeCut: " << edgeCut << endl; + } + + return boost::make_optional(result); + } + + /* ************************************************************************* */ + bool isLargerIsland(const vector& island1, const vector& island2) { + return island1.size() > island2.size(); + } + + /* ************************************************************************* */ + // debug functions + void printIsland(const vector& island) { + cout << "island: "; + BOOST_FOREACH(const size_t key, island) + cout << key << " "; + cout << endl; + } + + void printIslands(const list >& islands) { + BOOST_FOREACH(const vector& island, islands) + printIsland(island); + } + + void printNumCamerasLandmarks(const vector& keys, const vector& int2symbol) { + int numCamera = 0, numLandmark = 0; + BOOST_FOREACH(const size_t key, keys) + if (int2symbol[key].chr() == 'x') + numCamera++; + else + numLandmark++; + cout << "numCamera: " << numCamera << " numLandmark: " << numLandmark << endl; + } + + /* ************************************************************************* */ + template + void addLandmarkToPartitionResult(const GenericGraph& graph, const vector& landmarkKeys, + MetisResult& partitionResult, WorkSpace& workspace) { + + // set up cameras in the dictionary + std::vector& A = partitionResult.A; + std::vector& B = partitionResult.B; + std::vector& C = partitionResult.C; + std::vector& dictionary = workspace.dictionary; + std::fill(dictionary.begin(), dictionary.end(), -1); + BOOST_FOREACH(const size_t a, A) + dictionary[a] = 1; + BOOST_FOREACH(const size_t b, B) + dictionary[b] = 2; + if (!C.empty()) + throw runtime_error("addLandmarkToPartitionResult: C is not empty"); + + // set up landmarks + size_t i,j; + BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph) { + i = factor->key1.index; + j = factor->key2.index; + if (dictionary[j] == 0) // if the landmark is already in the separator, continue + continue; + else if (dictionary[j] == -1) + dictionary[j] = dictionary[i]; + else { + if (dictionary[j] != dictionary[i]) + dictionary[j] = 0; + } +// if (j == 67980) +// cout << "dictionary[67980]" << dictionary[j] << endl; + } + + BOOST_FOREACH(const size_t j, landmarkKeys) { + switch(dictionary[j]) { + case 0: C.push_back(j); break; + case 1: A.push_back(j); break; + case 2: B.push_back(j); break; + default: cout << j << ": " << dictionary[j] << endl; throw runtime_error("addLandmarkToPartitionResult: wrong status for landmark"); + } + } + } + +#define REDUCE_CAMERA_GRAPH + + /* ************************************************************************* */ + template + boost::optional findPartitoning(const GenericGraph& graph, const vector& keys, + WorkSpace& workspace, bool verbose, + const boost::optional >& int2symbol, const bool reduceGraph) { + boost::optional result; + GenericGraph reducedGraph; + vector keyToPartition; + vector cameraKeys, landmarkKeys; + if (reduceGraph) { + if (!int2symbol.is_initialized()) + throw std::invalid_argument("findSeparator: int2symbol must be valid!"); + + // find out all the landmark keys, which are to be eliminated + cameraKeys.reserve(keys.size()); + landmarkKeys.reserve(keys.size()); + BOOST_FOREACH(const size_t key, keys) { + if((*int2symbol)[key].chr() == 'x') + cameraKeys.push_back(key); + else + landmarkKeys.push_back(key); + } + + keyToPartition = cameraKeys; + workspace.prepareDictionary(keyToPartition); + const std::vector& dictionary = workspace.dictionary; + reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reducedGraph); + cout << "original graph: V" << keys.size() << ", E" << graph.size() << " --> reduced graph: V" << cameraKeys.size() << ", E" << reducedGraph.size() << endl; + result = edgePartitionByMetis(reducedGraph, keyToPartition, workspace, verbose); + } else // call Metis to partition the graph to A, B, C + result = separatorPartitionByMetis(graph, keys, workspace, verbose); + + if (!result.is_initialized()) { + cout << "metis failed!" << endl; + return 0; + } + + if (reduceGraph) { + addLandmarkToPartitionResult(graph, landmarkKeys, *result, workspace); + cout << "the separator size: " << result->C.size() << " landmarks" << endl; + } + + return result; + } + + /* ************************************************************************* */ + template + int findSeparator(const GenericGraph& graph, const vector& keys, + const int minNodesPerMap, WorkSpace& workspace, bool verbose, + const boost::optional >& int2symbol, const bool reduceGraph, + const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { + + boost::optional result = findPartitoning(graph, keys, workspace, verbose, int2symbol, reduceGraph); + + // find the island in A and B, and make them separated submaps + typedef vector Island; + list islands; + list islands_in_A = findIslands(graph, result->A, workspace, minNrConstraintsPerCamera, minNrConstraintsPerLandmark); + list islands_in_B = findIslands(graph, result->B, workspace, minNrConstraintsPerCamera, minNrConstraintsPerLandmark); + islands.insert(islands.end(), islands_in_A.begin(), islands_in_A.end()); + islands.insert(islands.end(), islands_in_B.begin(), islands_in_B.end()); + islands.sort(isLargerIsland); + size_t numIsland0 = islands.size(); + +#ifdef NDEBUG +// verbose = true; +// if (!int2symbol) throw std::invalid_argument("findSeparator: int2symbol is not set!"); +// cout << "sep size: " << result->C.size() << "; "; +// printNumCamerasLandmarks(result->C, *int2symbol); +// cout << "no. of island: " << islands.size() << "; "; +// cout << "island size: "; +// BOOST_FOREACH(const Island& island, islands) +// cout << island.size() << " "; +// cout << endl; + +// BOOST_FOREACH(const Island& island, islands) { +// printNumCamerasLandmarks(island, int2symbol); +// } +#endif + + // absorb small components into the separator + int oldSize = islands.size(); + while(true) { + if (islands.size() < 2) { + cout << "numIsland: " << numIsland0 << endl; + throw runtime_error("findSeparator: found fewer than 2 submaps!"); + } + + list::reference island = islands.back(); + if ((int)island.size() >= minNodesPerMap) break; + result->C.insert(result->C.end(), island.begin(), island.end()); + islands.pop_back(); + } + if (islands.size() != oldSize) + if (verbose) cout << oldSize << "-" << oldSize - islands.size() << " submap(s);\t" << endl; + else + if (verbose) cout << oldSize << " submap(s);\t" << endl; + + // generate the node map + std::vector& partitionTable = workspace.partitionTable; + std::fill(partitionTable.begin(), partitionTable.end(), -1); + BOOST_FOREACH(const size_t key, result->C) + partitionTable[key] = 0; + int idx = 0; + BOOST_FOREACH(const Island& island, islands) { + idx++; + BOOST_FOREACH(const size_t key, island) { + partitionTable[key] = idx; + } + } + + return islands.size(); + } + +}} //namespace diff --git a/gtsam_unstable/partition/FindSeparator.h b/gtsam_unstable/partition/FindSeparator.h new file mode 100644 index 000000000..44bc24b9e --- /dev/null +++ b/gtsam_unstable/partition/FindSeparator.h @@ -0,0 +1,43 @@ +/* + * FindSeparator.h + * + * Created on: Nov 23, 2010 + * Author: nikai + * Description: find the separator of bisectioning for a given graph + */ + +#include +#include +#include +#include + +#include "PartitionWorkSpace.h" + +namespace gtsam { namespace partition { + +// typedef std::map PartitionTable; // from the key to the partition: 0 - separator, > 1: submap id + + /** the metis Nest dissection result */ + struct MetisResult { + std::vector A, B; // frontals + std::vector C; // separator + }; + + /** + * use Metis library to partition, return the size of separator and the optional partition table + * the size of dictionary mush be equal to the number of variables in the original graph (the largest one) + */ + template + boost::optional separatorPartitionByMetis(const GenericGraph& graph, const std::vector& keys, + WorkSpace& workspace, bool verbose); + + /** + * return the number of submaps and the parition table of the partitioned graph (**stored in workspace.partitionTable**). + * return 0 if failed Note that the original output of Metis is 0,1 for submap, and 2 for the separator. + */ + template + int findSeparator(const GenericGraph& graph, const std::vector& keys, + const int minNodesPerMap, WorkSpace& workspace, bool verbose, const boost::optional >& int2symbol, + const bool reduceGraph, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark); + +}} //namespace diff --git a/gtsam_unstable/partition/GenericGraph.cpp b/gtsam_unstable/partition/GenericGraph.cpp new file mode 100644 index 000000000..611186c12 --- /dev/null +++ b/gtsam_unstable/partition/GenericGraph.cpp @@ -0,0 +1,476 @@ +/* + * GenericGraph2D.cpp + * + * Created on: Nov 23, 2010 + * Author: nikai + * Description: generic graph types used in partitioning + */ +#include +#include +#include +#include + +#include + +#include "GenericGraph.h" + +using namespace std; + +namespace gtsam { namespace partition { + + /** + * Note: Need to be able to handle a graph with factors that involve variables not in the given {keys} + */ + list > findIslands(const GenericGraph2D& graph, const vector& keys, WorkSpace& workspace, + const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) + { + typedef pair IntPair; + typedef list FactorList; + typedef map Connections; + + // create disjoin set forest + int numNodes = keys.size(); + DSFVector dsf(workspace.dsf, keys); + + FactorList factors(graph.begin(), graph.end()); + size_t i, nrFactors = factors.size(); + FactorList::iterator itEnd; + workspace.prepareDictionary(keys); + while (nrFactors) { + Connections connections; + bool succeed = false; + itEnd = factors.end(); + list toErase; + for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) { + + // remove invalid factors + GenericNode2D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2; + if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) { + toErase.push_back(itFactor); nrFactors--; continue; + } + + size_t label1 = dsf.findSet(key1.index); + size_t label2 = dsf.findSet(key2.index); + if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; } + + // merge two trees if the connection is strong enough, otherwise cache it + // an odometry factor always merges two islands + if (key1.type == NODE_POSE_2D && key2.type == NODE_POSE_2D) { + toErase.push_back(itFactor); nrFactors--; + dsf.makeUnionInPlace(label1, label2); + succeed = true; + break; + } + + // single landmark island only need one measurement + if ((dsf.isSingleton(label1)==1 && key1.type == NODE_LANDMARK_2D) || + (dsf.isSingleton(label2)==1 && key2.type == NODE_LANDMARK_2D)) { + toErase.push_back(itFactor); nrFactors--; + dsf.makeUnionInPlace(label1, label2); + succeed = true; + break; + } + + // stack the current factor with the cached constraint + IntPair labels = (label1 < label2) ? make_pair(label1, label2) : make_pair(label2, label1); + Connections::iterator itCached = connections.find(labels); + if (itCached == connections.end()) { + connections.insert(make_pair(labels, itFactor)); + continue; + } else { + GenericNode2D key21 = (*itCached->second)->key1, key22 = (*itCached->second)->key2; + // if observe the same landmark, we can not merge, abandon the current factor + if ((key1.index == key21.index && key1.type == NODE_LANDMARK_2D) || + (key1.index == key22.index && key1.type == NODE_LANDMARK_2D) || + (key2.index == key21.index && key2.type == NODE_LANDMARK_2D) || + (key2.index == key22.index && key2.type == NODE_LANDMARK_2D)) { + toErase.push_back(itFactor); nrFactors--; + continue; + } else { + toErase.push_back(itFactor); nrFactors--; + toErase.push_back(itCached->second); nrFactors--; + dsf.makeUnionInPlace(label1, label2); + connections.erase(itCached); + succeed = true; + break; + } + } + } + + // erase unused factors + BOOST_FOREACH(const FactorList::iterator& it, toErase) + factors.erase(it); + + if (!succeed) break; + } + + list > islands; + map > arrays = dsf.arrays(); + size_t key; vector array; + BOOST_FOREACH(boost::tie(key, array), arrays) + islands.push_back(array); + return islands; + } + + + /* ************************************************************************* */ + void print(const GenericGraph2D& graph, const std::string name) { + cout << name << endl; + BOOST_FOREACH(const sharedGenericFactor2D& factor_, graph) + cout << factor_->key1.index << " " << factor_->key2.index << endl; + } + + /* ************************************************************************* */ + void print(const GenericGraph3D& graph, const std::string name) { + cout << name << endl; + BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) + cout << factor_->key1.index << " " << factor_->key2.index << " (" << + factor_->key1.type << ", " << factor_->key2.type <<")" << endl; + } + + /* ************************************************************************* */ + // create disjoin set forest + DSFVector createDSF(const GenericGraph3D& graph, const vector& keys, const WorkSpace& workspace) { + DSFVector dsf(workspace.dsf, keys); + typedef list FactorList; + + FactorList factors(graph.begin(), graph.end()); + size_t i, nrFactors = factors.size(); + FactorList::iterator itEnd; + while (nrFactors) { + + bool succeed = false; + itEnd = factors.end(); + list toErase; + for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) { + + // remove invalid factors + if (graph.size() == 178765) cout << "kai21" << endl; + GenericNode3D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2; + if (graph.size() == 178765) cout << "kai21: " << key1.index << " " << key2.index << endl; + if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) { + toErase.push_back(itFactor); nrFactors--; continue; + } + + if (graph.size() == 178765) cout << "kai22" << endl; + size_t label1 = dsf.findSet(key1.index); + size_t label2 = dsf.findSet(key2.index); + if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; } + + if (graph.size() == 178765) cout << "kai23" << endl; + // merge two trees if the connection is strong enough, otherwise cache it + // an odometry factor always merges two islands + if ((key1.type == NODE_POSE_3D && key2.type == NODE_LANDMARK_3D) || + (key1.type == NODE_POSE_3D && key2.type == NODE_POSE_3D)) { + toErase.push_back(itFactor); nrFactors--; + dsf.makeUnionInPlace(label1, label2); + succeed = true; + break; + } + + if (graph.size() == 178765) cout << "kai24" << endl; + + + } + + // erase unused factors + BOOST_FOREACH(const FactorList::iterator& it, toErase) + factors.erase(it); + + if (!succeed) break; + } + return dsf; + } + + /* ************************************************************************* */ + // first check the type of the key (pose or landmark), and then check whether it is singular + inline bool isSingular(const set& singularCameras, const set& singularLandmarks, const GenericNode3D& node) { + switch(node.type) { + case NODE_POSE_3D: + return singularCameras.find(node.index) != singularCameras.end(); break; + case NODE_LANDMARK_3D: + return singularLandmarks.find(node.index) != singularLandmarks.end(); break; + default: + throw runtime_error("unrecognized key type!"); + } + } + + /* ************************************************************************* */ + void findSingularCamerasLandmarks(const GenericGraph3D& graph, const WorkSpace& workspace, + const vector& isCamera, const vector& isLandmark, + set& singularCameras, set& singularLandmarks, vector& nrConstraints, + bool& foundSingularCamera, bool& foundSingularLandmark, + const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { + + // compute the constraint number per camera + std::fill(nrConstraints.begin(), nrConstraints.end(), 0); + BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { + const int& key1 = factor_->key1.index; + const int& key2 = factor_->key2.index; + if (workspace.dictionary[key1] != -1 && workspace.dictionary[key2] != -1 && + !isSingular(singularCameras, singularLandmarks, factor_->key1) && + !isSingular(singularCameras, singularLandmarks, factor_->key2)) { + nrConstraints[key1]++; + nrConstraints[key2]++; + + // a single pose constraint is sufficient for stereo, so we add 2 to the counter + // for a total of 3, i.e. the same as 3 landmarks fully constraining the camera + if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){ + nrConstraints[key1]+=2; + nrConstraints[key2]+=2; + } + } + } + + // find singular cameras and landmarks + foundSingularCamera = false; + foundSingularLandmark = false; + for (int i=0; i > findIslands(const GenericGraph3D& graph, const vector& keys, WorkSpace& workspace, + const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { + + // create disjoint set forest + workspace.prepareDictionary(keys); + DSFVector dsf = createDSF(graph, keys, workspace); + + const bool verbose = false; + bool foundSingularCamera = true; + bool foundSingularLandmark = true; + + list > islands; + set singularCameras, singularLandmarks; + vector isCamera(workspace.dictionary.size(), false); + vector isLandmark(workspace.dictionary.size(), false); + + // check the constraint number of every variable + // find the camera and landmark keys + BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { + //assert(factor_->key2.type == NODE_LANDMARK_3D); // only VisualSLAM should come here, not StereoSLAM + if (workspace.dictionary[factor_->key1.index] != -1) { + if (factor_->key1.type == NODE_POSE_3D) + isCamera[factor_->key1.index] = true; + else + isLandmark[factor_->key1.index] = true; + } + if (workspace.dictionary[factor_->key2.index] != -1) + if (factor_->key2.type == NODE_POSE_3D) + isCamera[factor_->key2.index] = true; + else + isLandmark[factor_->key2.index] = true; + } + + vector nrConstraints(workspace.dictionary.size(), 0); + // iterate until all singular variables have been removed. Removing a singular variable + // can cause another to become singular, so this will probably run several times + while (foundSingularCamera || foundSingularLandmark) { + findSingularCamerasLandmarks(graph, workspace, isCamera, isLandmark, // input + singularCameras, singularLandmarks, nrConstraints, // output + foundSingularCamera, foundSingularLandmark, // output + minNrConstraintsPerCamera, minNrConstraintsPerLandmark); // input + } + + // add singular variables directly as islands + if (!singularCameras.empty()) { + if (verbose) cout << "singular cameras:"; + BOOST_FOREACH(const size_t i, singularCameras) { + islands.push_back(vector(1, i)); // <--------------------------- + if (verbose) cout << i << " "; + } + if (verbose) cout << endl; + } + if (!singularLandmarks.empty()) { + if (verbose) cout << "singular landmarks:"; + BOOST_FOREACH(const size_t i, singularLandmarks) { + islands.push_back(vector(1, i)); // <--------------------------- + if (verbose) cout << i << " "; + } + if (verbose) cout << endl; + } + + + // regenerating islands + map > labelIslands = dsf.arrays(); + size_t label; vector island; + BOOST_FOREACH(boost::tie(label, island), labelIslands) { + vector filteredIsland; // remove singular cameras from array + filteredIsland.reserve(island.size()); + BOOST_FOREACH(const size_t key, island) { + if ((isCamera[key] && singularCameras.find(key) == singularCameras.end()) || // not singular + (isLandmark[key] && singularLandmarks.find(key) == singularLandmarks.end()) || // not singular + (!isCamera[key] && !isLandmark[key])) { // the key is not involved in any factor, so the type is undertermined + filteredIsland.push_back(key); + } + } + islands.push_back(filteredIsland); + } + + // sanity check + int nrKeys = 0; + BOOST_FOREACH(const vector& island, islands) + nrKeys += island.size(); + if (nrKeys != keys.size()) { + cout << nrKeys << " vs " << keys.size() << endl; + throw runtime_error("findIslands: the number of keys is inconsistent!"); + } + + + if (verbose) cout << "found " << islands.size() << " islands!" << endl; + return islands; + } + + /* ************************************************************************* */ + // return the number of intersection between two **sorted** landmark vectors + inline int getNrCommonLandmarks(const vector& landmarks1, const vector& landmarks2){ + int i1 = 0, i2 = 0; + int nrCommonLandmarks = 0; + while (i1 < landmarks1.size() && i2 < landmarks2.size()) { + if (landmarks1[i1] < landmarks2[i2]) + i1 ++; + else if (landmarks1[i1] > landmarks2[i2]) + i2 ++; + else { + i1++; i2++; + nrCommonLandmarks ++; + } + } + return nrCommonLandmarks; + } + + /* ************************************************************************* */ + void reduceGenericGraph(const GenericGraph3D& graph, const std::vector& cameraKeys, const std::vector& landmarkKeys, + const std::vector& dictionary, GenericGraph3D& reducedGraph) { + + typedef size_t CameraKey; + typedef pair CameraPair; + typedef size_t LandmarkKey; + // get a mapping from each landmark to its connected cameras + vector > cameraToLandmarks(dictionary.size()); + // for odometry xi-xj where i cameraToCamera(dictionary.size(), -1); + size_t key_i, key_j; + BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { + if (factor_->key1.type == NODE_POSE_3D) { + if (factor_->key2.type == NODE_LANDMARK_3D) {// projection factor + cameraToLandmarks[factor_->key1.index].push_back(factor_->key2.index); + } + else { // odometry factor + if (factor_->key1.index < factor_->key2.index) { + key_i = factor_->key1.index; + key_j = factor_->key2.index; + } else { + key_i = factor_->key2.index; + key_j = factor_->key1.index; + } + cameraToCamera[key_i] = key_j; + } + } + } + + // sort the landmark keys for the late getNrCommonLandmarks call + BOOST_FOREACH(vector &landmarks, cameraToLandmarks){ + if (!landmarks.empty()) + std::sort(landmarks.begin(), landmarks.end()); + } + + // generate the reduced graph + reducedGraph.clear(); + int factorIndex = 0; + int camera1, camera2, nrTotalConstraints; + bool hasOdometry; + for (int i1=0; i1 0 || hasOdometry) { + nrTotalConstraints = 2 * nrCommonLandmarks + (hasOdometry ? 6 : 0); + reducedGraph.push_back(boost::make_shared(camera1, camera2, + factorIndex++, NODE_POSE_3D, NODE_POSE_3D, nrTotalConstraints)); + } + } + } + } + + /* ************************************************************************* */ + void checkSingularity(const GenericGraph3D& graph, const std::vector& frontals, + WorkSpace& workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { + workspace.prepareDictionary(frontals); + vector nrConstraints(workspace.dictionary.size(), 0); + + // summarize the constraint number + const vector& dictionary = workspace.dictionary; + vector isValidCamera(workspace.dictionary.size(), false); + vector isValidLandmark(workspace.dictionary.size(), false); + BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { + assert(factor_->key1.type == NODE_POSE_3D); + //assert(factor_->key2.type == NODE_LANDMARK_3D); + const size_t& key1 = factor_->key1.index; + const size_t& key2 = factor_->key2.index; + if (dictionary[key1] == -1 || dictionary[key2] == -1) + continue; + + isValidCamera[key1] = true; + if(factor_->key2.type == NODE_LANDMARK_3D) + isValidLandmark[key2] = true; + else + isValidCamera[key2] = true; + + nrConstraints[key1]++; + nrConstraints[key2]++; + + // a single pose constraint is sufficient for stereo, so we add 2 to the counter + // for a total of 3, i.e. the same as 3 landmarks fully constraining the camera + if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){ + nrConstraints[key1]+=2; + nrConstraints[key2]+=2; + } + } + + // find the minimum constraint for cameras and landmarks + size_t minFoundConstraintsPerCamera = 10000; + size_t minFoundConstraintsPerLandmark = 10000; + + for (int i=0; i +#include +#include +#include + +#include "PartitionWorkSpace.h" + +namespace gtsam { namespace partition { + + /*************************************************** + * 2D generic factors and their factor graph + ***************************************************/ + enum GenericNode2DType { NODE_POSE_2D, NODE_LANDMARK_2D }; + + /** the index of the node and the type of the node */ + struct GenericNode2D { + std::size_t index; + GenericNode2DType type; + GenericNode2D (const std::size_t& index_in, const GenericNode2DType& type_in) : index(index_in), type(type_in) {} + }; + + /** a factor always involves two nodes/variables for now */ + struct GenericFactor2D { + GenericNode2D key1; + GenericNode2D key2; + int index; // the factor index in the original nonlinear factor graph + int weight; // the weight of the edge + GenericFactor2D(const size_t index1, const GenericNode2DType type1, const size_t index2, const GenericNode2DType type2, const int index_ = -1, const int weight_ = 1) + : key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {} + GenericFactor2D(const size_t index1, const char type1, const size_t index2, const char type2, const int index_ = -1, const int weight_ = 1) + : key1(index1, type1 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), + key2(index2, type2 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_), weight(weight_) {} + }; + + /** graph is a collection of factors */ + typedef boost::shared_ptr sharedGenericFactor2D; + typedef std::vector GenericGraph2D; + + /** merge nodes in DSF using constraints captured by the given graph */ + std::list > findIslands(const GenericGraph2D& graph, const std::vector& keys, WorkSpace& workspace, + const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark); + + /** eliminate the sensors from generic graph */ + inline void reduceGenericGraph(const GenericGraph2D& graph, const std::vector& cameraKeys, const std::vector& landmarkKeys, + const std::vector& dictionary, GenericGraph2D& reducedGraph) { + throw std::runtime_error("reduceGenericGraph 2d not implemented"); + } + + /** check whether the 2D graph is singular (under constrained) , Dummy function for 2D */ + inline void checkSingularity(const GenericGraph2D& graph, const std::vector& frontals, + WorkSpace& workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { return; } + + /** print the graph **/ + void print(const GenericGraph2D& graph, const std::string name = "GenericGraph2D"); + + /*************************************************** + * 3D generic factors and their factor graph + ***************************************************/ + enum GenericNode3DType { NODE_POSE_3D, NODE_LANDMARK_3D }; + +// const int minNrConstraintsPerCamera = 7; +// const int minNrConstraintsPerLandmark = 2; + + /** the index of the node and the type of the node */ + struct GenericNode3D { + std::size_t index; + GenericNode3DType type; + GenericNode3D (const std::size_t& index_in, const GenericNode3DType& type_in) : index(index_in), type(type_in) {} + }; + + /** a factor always involves two nodes/variables for now */ + struct GenericFactor3D { + GenericNode3D key1; + GenericNode3D key2; + int index; // the index in the entire graph, 0-based + int weight; // the weight of the edge + GenericFactor3D() :key1(-1, NODE_POSE_3D), key2(-1, NODE_LANDMARK_3D), index(-1), weight(1) {} + GenericFactor3D(const size_t index1, const size_t index2, const int index_ = -1, + const GenericNode3DType type1 = NODE_POSE_3D, const GenericNode3DType type2 = NODE_LANDMARK_3D, const int weight_ = 1) + : key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {} + }; + + /** graph is a collection of factors */ + typedef boost::shared_ptr sharedGenericFactor3D; + typedef std::vector GenericGraph3D; + + /** merge nodes in DSF using constraints captured by the given graph */ + std::list > findIslands(const GenericGraph3D& graph, const std::vector& keys, WorkSpace& workspace, + const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark); + + /** eliminate the sensors from generic graph */ + void reduceGenericGraph(const GenericGraph3D& graph, const std::vector& cameraKeys, const std::vector& landmarkKeys, + const std::vector& dictionary, GenericGraph3D& reducedGraph); + + /** check whether the 3D graph is singular (under constrained) */ + void checkSingularity(const GenericGraph3D& graph, const std::vector& frontals, + WorkSpace& workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark); + + + /** print the graph **/ + void print(const GenericGraph3D& graph, const std::string name = "GenericGraph3D"); + + /*************************************************** + * unary generic factors and their factor graph + ***************************************************/ + /** a factor involves a single variable */ + struct GenericUnaryFactor { + GenericNode2D key; + int index; // the factor index in the original nonlinear factor graph + GenericUnaryFactor(const size_t key_, const GenericNode2DType type_, const int index_ = -1) + : key(key_, type_), index(index_) {} + GenericUnaryFactor(const size_t key_, const char type_, const int index_ = -1) + : key(key_, type_ == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_) {} + }; + + /** graph is a collection of factors */ + typedef boost::shared_ptr sharedGenericUnaryFactor; + typedef std::vector GenericUnaryGraph; + + /*************************************************** + * utility functions + ***************************************************/ + inline bool hasCommonCamera(const std::set& cameras1, const std::set& cameras2) { + if (cameras1.empty() || cameras2.empty()) + throw std::invalid_argument("hasCommonCamera: the input camera set is empty!"); + std::set::const_iterator it1 = cameras1.begin(); + std::set::const_iterator it2 = cameras2.begin(); + while (it1 != cameras1.end() && it2 != cameras2.end()) { + if (*it1 == *it2) + return true; + else if (*it1 < *it2) + it1++; + else + it2++; + } + return false; + } + +}} // namespace diff --git a/gtsam_unstable/partition/NestedDissection-inl.h b/gtsam_unstable/partition/NestedDissection-inl.h new file mode 100644 index 000000000..e8f3fa34a --- /dev/null +++ b/gtsam_unstable/partition/NestedDissection-inl.h @@ -0,0 +1,253 @@ +/* + * NestedDissection-inl.h + * + * Created on: Nov 27, 2010 + * Author: nikai + * Description: + */ + +#pragma once + +#include + +#include "partition/FindSeparator-inl.h" +#include "OrderedSymbols.h" +#include "NestedDissection.h" + +using namespace std; + +namespace gtsam { namespace partition { + + /* ************************************************************************* */ + template + NestedDissection::NestedDissection( + const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose) : + fg_(fg), ordering_(ordering){ + GenericUnaryGraph unaryFactors; + GenericGraph gfg; + boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering); + + // build reverse mapping from integer to symbol + int numNodes = ordering.size(); + int2symbol_.resize(numNodes); + Ordering::const_iterator it = ordering.begin(), itLast = ordering.end(); + while(it != itLast) + int2symbol_[it->second] = (it++)->first; + + vector keys; + keys.reserve(numNodes); + for(int i=0; i(), numNodeStopPartition, minNodesPerMap, boost::shared_ptr(), workspace, verbose); + } + + /* ************************************************************************* */ + template + NestedDissection::NestedDissection( + const NLG& fg, const Ordering& ordering, const boost::shared_ptr& cuts, const bool verbose) : fg_(fg), ordering_(ordering){ + GenericUnaryGraph unaryFactors; + GenericGraph gfg; + boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering); + + // build reverse mapping from integer to symbol + int numNodes = ordering.size(); + int2symbol_.resize(numNodes); + Ordering::const_iterator it = ordering.begin(), itLast = ordering.end(); + while(it != itLast) + int2symbol_[it->second] = (it++)->first; + + vector keys; + keys.reserve(numNodes); + for(int i=0; i(), cuts, boost::shared_ptr(), workspace, verbose); + } + + /* ************************************************************************* */ + template + boost::shared_ptr NestedDissection::makeSubNLG( + const NLG& fg, const vector& frontals, const vector& sep, const boost::shared_ptr& parent) const { + OrderedSymbols frontalKeys; + BOOST_FOREACH(const size_t index, frontals) + frontalKeys.push_back(int2symbol_[index]); + + UnorderedSymbols sepKeys; + BOOST_FOREACH(const size_t index, sep) + sepKeys.insert(int2symbol_[index]); + + return boost::make_shared(fg, frontalKeys, sepKeys, parent); + } + + /* ************************************************************************* */ + template + void NestedDissection::processFactor( + const typename GenericGraph::value_type& factor, const std::vector& partitionTable, // input + vector& frontalFactors, NLG& sepFactors, vector >& childSeps, // output factor graphs + typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques + list sep_; // the separator variables involved in the current factor + int partition1 = partitionTable[factor->key1.index]; + int partition2 = partitionTable[factor->key2.index]; + if (partition1 <= 0 && partition2 <= 0) { // is a factor in the current clique + sepFactors.push_back(fg_[factor->index]); + } + else if (partition1 > 0 && partition2 > 0 && partition1 != partition2) { // is a weeklink (factor between two child cliques) + weeklinks.push_back(fg_[factor->index]); + } + else if (partition1 > 0 && partition2 > 0 && partition1 == partition2) { // is a local factor in one of the child cliques + frontalFactors[partition1 - 1].push_back(factor); + } + else { // is a joint factor in the child clique (involving varaibles in the current clique) + if (partition1 > 0 && partition2 <= 0) { + frontalFactors[partition1 - 1].push_back(factor); + childSeps[partition1 - 1].insert(factor->key2.index); + } else if (partition1 <= 0 && partition2 > 0) { + frontalFactors[partition2 - 1].push_back(factor); + childSeps[partition2 - 1].insert(factor->key1.index); + } else + throw runtime_error("processFactor: unexpected entries in the partition table!"); + } + } + + /* ************************************************************************* */ + /** + * given a factor graph and its partition {nodeMap}, split the factors between the child cliques ({frontalFactors}) + * and the current clique ({sepFactors}). Also split the variables between the child cliques ({childFrontals}) + * and the current clique ({localFrontals}). Those separator variables involved in {frontalFactors} are put into + * the correspoding ordering in {childSeps}. + */ + // TODO: frontalFactors and localFrontals should be generated in findSeparator + template + void NestedDissection::partitionFactorsAndVariables( + const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, const std::vector& keys, //input + const std::vector& partitionTable, const int numSubmaps, // input + vector& frontalFactors, vector& frontalUnaryFactors, NLG& sepFactors, // output factor graphs + vector >& childFrontals, vector >& childSeps, vector& localFrontals, // output sub-orderings + typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques + + // make three lists of variables A, B, and C + int partition; + childFrontals.resize(numSubmaps); + BOOST_FOREACH(const size_t key, keys){ + partition = partitionTable[key]; + switch (partition) { + case -1: break; // the separator of the separator variables + case 0: localFrontals.push_back(key); break; // the separator variables + default: childFrontals[partition-1].push_back(key); // the frontal variables + } + } + + // group the factors to {frontalFactors} and {sepFactors},and find the joint variables + vector > childSeps_; + childSeps_.resize(numSubmaps); + childSeps.reserve(numSubmaps); + frontalFactors.resize(numSubmaps); + frontalUnaryFactors.resize(numSubmaps); + BOOST_FOREACH(typename GenericGraph::value_type factor, fg) + processFactor(factor, partitionTable, frontalFactors, sepFactors, childSeps_, weeklinks); + BOOST_FOREACH(const set& childSep, childSeps_) + childSeps.push_back(vector(childSep.begin(), childSep.end())); + + // add unary factor to the current cluster or pass it to one of the child clusters + BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) { + partition = partitionTable[unaryFactor_->key.index]; + if (!partition) sepFactors.push_back(fg_[unaryFactor_->index]); + else frontalUnaryFactors[partition-1].push_back(unaryFactor_); + } + } + + /* ************************************************************************* */ + template + NLG NestedDissection::collectOriginalFactors( + const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const { + NLG sepFactors; + typename GenericGraph::const_iterator it = gfg.begin(), itLast = gfg.end(); + while(it!=itLast) sepFactors.push_back(fg_[(*it++)->index]); + BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) + sepFactors.push_back(fg_[unaryFactor_->index]); + return sepFactors; + } + + /* ************************************************************************* */ + template + boost::shared_ptr NestedDissection::recursivePartition( + const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector& frontals, const vector& sep, + const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const { + + // if no split needed + NLG sepFactors; // factors that should remain in the current cluster + if (frontals.size() <= numNodeStopPartition || gfg.size() <= numNodeStopPartition) { + sepFactors = collectOriginalFactors(gfg, unaryFactors); + return makeSubNLG(sepFactors, frontals, sep, parent); + } + + // find the nested dissection separator + int numSubmaps = findSeparator(gfg, frontals, minNodesPerMap, workspace, verbose, int2symbol_, NLG::reduceGraph(), + NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark()); + partition::PartitionTable& partitionTable = workspace.partitionTable; + if (numSubmaps == 0) throw runtime_error("recursivePartition: get zero submap after ND!"); + + // split the factors between child cliques and the current clique + vector frontalFactors; vector frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks; + vector localFrontals; vector > childFrontals, childSeps; + partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps, + frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks); + + // make a new cluster + boost::shared_ptr current = makeSubNLG(sepFactors, localFrontals, sep, parent); + current->setWeeklinks(weeklinks); + + // check whether all the submaps are fully constrained + for (int i=0; i child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i], + numNodeStopPartition, minNodesPerMap, current, workspace, verbose); + current->addChild(child); + } + + return current; + } + + /* ************************************************************************* */ + template + boost::shared_ptr NestedDissection::recursivePartition( + const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector& frontals, const vector& sep, + const boost::shared_ptr& cuts, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const { + + // if there is no need to cut any more + NLG sepFactors; // factors that should remain in the current cluster + if (!cuts.get()) { + sepFactors = collectOriginalFactors(gfg, unaryFactors); + return makeSubNLG(sepFactors, frontals, sep, parent); + } + + // retrieve the current partitioning info + int numSubmaps = 2; + partition::PartitionTable& partitionTable = cuts->partitionTable; + + // split the factors between child cliques and the current clique + vector frontalFactors; vector frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks; + vector localFrontals; vector > childFrontals, childSeps; + partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps, + frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks); + + // make a new cluster + boost::shared_ptr current = makeSubNLG(sepFactors, localFrontals, sep, parent); + current->setWeeklinks(weeklinks); + + // create child clusters + for (int i=0; i<2; i++) { + boost::shared_ptr child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i], + cuts->children.empty() ? boost::shared_ptr() : cuts->children[i], current, workspace, verbose); + current->addChild(child); + } + return current; + } +}} //namespace diff --git a/gtsam_unstable/partition/NestedDissection.h b/gtsam_unstable/partition/NestedDissection.h new file mode 100644 index 000000000..3c294be64 --- /dev/null +++ b/gtsam_unstable/partition/NestedDissection.h @@ -0,0 +1,69 @@ +/* + * NestedDissection.h + * + * Created on: Nov 27, 2010 + * Author: nikai + * Description: apply nested dissection algorithm to the factor graph + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { namespace partition { + + /** + * Apply nested dissection algorithm to nonlinear factor graphs + */ + template + class NestedDissection { + public: + typedef boost::shared_ptr sharedSubNLG; + + private: + NLG fg_; // the original nonlinear factor graph + Ordering ordering_; // the variable ordering in the nonlinear factor graph + std::vector int2symbol_; // the mapping from integer key to symbol + sharedSubNLG root_; // the root of generated cluster tree + + public: + sharedSubNLG root() const { return root_; } + + public: + /* constructor with post-determined partitoning*/ + NestedDissection(const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose = false); + + /* constructor with pre-determined cuts*/ + NestedDissection(const NLG& fg, const Ordering& ordering, const boost::shared_ptr& cuts, const bool verbose = false); + + private: + /* convert generic subgraph to nonlinear subgraph */ + sharedSubNLG makeSubNLG(const NLG& fg, const std::vector& frontals, const std::vector& sep, const boost::shared_ptr& parent) const; + + void processFactor(const typename GenericGraph::value_type& factor, const std::vector& partitionTable, // input + std::vector& frontalFactors, NLG& sepFactors, std::vector >& childSeps, // output factor graphs + typename SubNLG::Weeklinks& weeklinks) const; + + /* recursively partition the generic graph */ + void partitionFactorsAndVariables( + const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, + const std::vector& keys, const std::vector& partitionTable, const int numSubmaps, // input + std::vector& frontalFactors, vector& frontalUnaryFactors, NLG& sepFactors, // output factor graphs + std::vector >& childFrontals, std::vector >& childSeps, std::vector& localFrontals, // output sub-orderings + typename SubNLG::Weeklinks& weeklinks) const; + + NLG collectOriginalFactors(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const; + + /* recursively partition the generic graph */ + sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector& frontals, const std::vector& sep, + const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const; + + /* recursively partition the generic graph */ + sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector& frontals, const std::vector& sep, + const boost::shared_ptr& cuts, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const; + + }; + +}} //namespace diff --git a/gtsam_unstable/partition/PartitionWorkSpace.h b/gtsam_unstable/partition/PartitionWorkSpace.h new file mode 100644 index 000000000..021c1cdcc --- /dev/null +++ b/gtsam_unstable/partition/PartitionWorkSpace.h @@ -0,0 +1,43 @@ +/* + * PartitionWorkSpace.h + * + * Created on: Nov 24, 2010 + * Author: nikai + * Description: a preallocated memory space used in partitioning + */ + +#pragma once + +#include +#include + +namespace gtsam { namespace partition { + + typedef std::vector PartitionTable; + + // the work space, preallocated memory + struct WorkSpace { + std::vector dictionary; // a mapping from the integer key in the original graph to 0-based index in the subgraph, useful when handling a subset of keys and graphs + boost::shared_ptr > dsf; // a block memory pre-allocated for DSFVector + PartitionTable partitionTable; // a mapping from a key to the submap index, 0 means the separator, i means the ith submap + + // constructor + WorkSpace(const size_t numNodes) : dictionary(numNodes,0), dsf(new std::vector(numNodes, 0)), partitionTable(numNodes, -1) { } + + // set up dictionary: -1: no such key, none-zero: the corresponding 0-based index + inline void prepareDictionary(const std::vector& keys) { + int index = 0; + std::fill(dictionary.begin(), dictionary.end(), -1); + std::vector::const_iterator it=keys.begin(), itLast=keys.end(); + while(it!=itLast) dictionary[*(it++)] = index++; + } + }; + + + // manually defined cuts + struct Cuts { + PartitionTable partitionTable; + std::vector > children; + }; + +}} // namespace diff --git a/gtsam_unstable/partition/testFindSeparator.cpp b/gtsam_unstable/partition/testFindSeparator.cpp new file mode 100644 index 000000000..8c3306458 --- /dev/null +++ b/gtsam_unstable/partition/testFindSeparator.cpp @@ -0,0 +1,222 @@ +/* + * testFindSeparator.cpp + * + * Created on: Nov 23, 2010 + * Author: nikai + * Description: unit tests for FindSeparator + */ + +#include // for operator += +#include // for operator += +#include // for operator += +using namespace boost::assign; +#include +#include +#include + +#include "partition/FindSeparator-inl.h" +#include "partition/GenericGraph.h" + +using namespace std; +using namespace gtsam; +using namespace gtsam::partition; + +/* ************************************************************************* */ +// x0 - x1 - x2 +// l3 l4 +TEST ( Partition, separatorPartitionByMetis ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(0, NODE_POSE_2D, 3, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(0, NODE_POSE_2D, 1, NODE_POSE_2D)); + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + std::vector keys; keys += 0, 1, 2, 3, 4; + + WorkSpace workspace(5); + boost::optional actual = separatorPartitionByMetis(graph, keys, workspace, false); + CHECK(actual.is_initialized()); + vector A_expected; A_expected += 0, 3; // frontal + vector B_expected; B_expected += 2, 4; // frontal + vector C_expected; C_expected += 1; // separator + CHECK(A_expected == actual->A); + CHECK(B_expected == actual->B); + CHECK(C_expected == actual->C); +} + +/* ************************************************************************* */ +// x1 - x2 - x3, variable not used x0, x4, l7 +// l5 l6 +TEST ( Partition, separatorPartitionByMetis2 ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 6, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + std::vector keys; keys += 1, 2, 3, 5, 6; + WorkSpace workspace(8); + boost::optional actual = separatorPartitionByMetis(graph, keys, workspace, false); + CHECK(actual.is_initialized()); + vector A_expected; A_expected += 1, 5; // frontal + vector B_expected; B_expected += 3, 6; // frontal + vector C_expected; C_expected += 2; // separator + CHECK(A_expected == actual->A); + CHECK(B_expected == actual->B); + CHECK(C_expected == actual->C); +} + +/* ************************************************************************* */ +// x0 - x2 - x3 - x5 +TEST ( Partition, edgePartitionByMetis ) +{ + GenericGraph3D graph; + graph.push_back(boost::make_shared(0, 2, 0, NODE_POSE_3D, NODE_POSE_3D)); + graph.push_back(boost::make_shared(2, 3, 1, NODE_POSE_3D, NODE_POSE_3D)); + graph.push_back(boost::make_shared(3, 5, 2, NODE_POSE_3D, NODE_POSE_3D)); + std::vector keys; keys += 0, 2, 3, 5; + + WorkSpace workspace(6); + boost::optional actual = edgePartitionByMetis(graph, keys, workspace, false); + CHECK(actual.is_initialized()); + vector A_expected; A_expected += 0, 2; // frontal + vector B_expected; B_expected += 3, 5; // frontal + vector C_expected; // separator +// BOOST_FOREACH(const size_t a, actual->A) +// cout << a << " "; +// cout << endl; +// BOOST_FOREACH(const size_t b, actual->B) +// cout << b << " "; +// cout << endl; + + CHECK(A_expected == actual->A || A_expected == actual->B); + CHECK(B_expected == actual->B || B_expected == actual->A); + CHECK(C_expected == actual->C); +} + +/* ************************************************************************* */ +// x0 - x2 - x3 - x5 - x6 +TEST ( Partition, edgePartitionByMetis2 ) +{ + GenericGraph3D graph; + graph.push_back(boost::make_shared(0, 2, 0, NODE_POSE_3D, NODE_POSE_3D, 1)); + graph.push_back(boost::make_shared(2, 3, 1, NODE_POSE_3D, NODE_POSE_3D, 1)); + graph.push_back(boost::make_shared(3, 5, 2, NODE_POSE_3D, NODE_POSE_3D, 20)); + graph.push_back(boost::make_shared(5, 6, 3, NODE_POSE_3D, NODE_POSE_3D, 1)); + std::vector keys; keys += 0, 2, 3, 5, 6; + + WorkSpace workspace(6); + boost::optional actual = edgePartitionByMetis(graph, keys, workspace, false); + CHECK(actual.is_initialized()); + vector A_expected; A_expected += 0, 2; // frontal + vector B_expected; B_expected += 3, 5, 6; // frontal + vector C_expected; // separator + CHECK(A_expected == actual->A); + CHECK(B_expected == actual->B); + CHECK(C_expected == actual->C); +} + + +/* ************************************************************************* */ +// x0 - x1 - x2 +// l3 l4 +TEST ( Partition, findSeparator ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(0, NODE_POSE_2D, 3, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(0, NODE_POSE_2D, 1, NODE_POSE_2D)); + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + std::vector keys; keys += 0, 1, 2, 3, 4; + + WorkSpace workspace(5); + int minNodesPerMap = -1; + bool reduceGraph = false; + int numSubmaps = findSeparator(graph, keys, minNodesPerMap, workspace, false, boost::none, reduceGraph); + LONGS_EQUAL(2, numSubmaps); + LONGS_EQUAL(5, workspace.partitionTable.size()); + LONGS_EQUAL(1, workspace.partitionTable[0]); + LONGS_EQUAL(0, workspace.partitionTable[1]); + LONGS_EQUAL(2, workspace.partitionTable[2]); + LONGS_EQUAL(1, workspace.partitionTable[3]); + LONGS_EQUAL(2, workspace.partitionTable[4]); +} + +/* ************************************************************************* */ +// x1 - x2 - x3, variable not used x0, x4, l7 +// l5 l6 +TEST ( Partition, findSeparator2 ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 6, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + std::vector keys; keys += 1, 2, 3, 5, 6; + + WorkSpace workspace(8); + int minNodesPerMap = -1; + bool reduceGraph = false; + int numSubmaps = findSeparator(graph, keys, minNodesPerMap, workspace, false, boost::none, reduceGraph); + LONGS_EQUAL(2, numSubmaps); + LONGS_EQUAL(8, workspace.partitionTable.size()); + LONGS_EQUAL(-1,workspace.partitionTable[0]); + LONGS_EQUAL(1, workspace.partitionTable[1]); + LONGS_EQUAL(0, workspace.partitionTable[2]); + LONGS_EQUAL(2, workspace.partitionTable[3]); + LONGS_EQUAL(-1,workspace.partitionTable[4]); + LONGS_EQUAL(1, workspace.partitionTable[5]); + LONGS_EQUAL(2, workspace.partitionTable[6]); + LONGS_EQUAL(-1,workspace.partitionTable[7]); +} + +/* ************************************************************************* */ +// l1-l8 l9-l16 l17-l24 +// / | / \ | \ +// x25 x26 x27 x28 +TEST ( Partition, findSeparator3_with_reduced_camera ) +{ + GenericGraph3D graph; + for (int j=1; j<=8; j++) + graph.push_back(boost::make_shared(25, j)); + for (int j=1; j<=16; j++) + graph.push_back(boost::make_shared(26, j)); + for (int j=9; j<=24; j++) + graph.push_back(boost::make_shared(27, j)); + for (int j=17; j<=24; j++) + graph.push_back(boost::make_shared(28, j)); + + std::vector keys; + for(int i=1; i<=28; i++) + keys.push_back(i); + + vector int2symbol; + int2symbol.push_back(Symbol('x',0)); // dummy + for(int i=1; i<=24; i++) + int2symbol.push_back(Symbol('l',i)); + int2symbol.push_back(Symbol('x',25)); + int2symbol.push_back(Symbol('x',26)); + int2symbol.push_back(Symbol('x',27)); + int2symbol.push_back(Symbol('x',28)); + + WorkSpace workspace(29); + bool reduceGraph = true; + int numIsland = findSeparator(graph, keys, 3, workspace, false, int2symbol, reduceGraph); + LONGS_EQUAL(2, numIsland); + + partition::PartitionTable& partitionTable = workspace.partitionTable; + for (int j=1; j<=8; j++) + LONGS_EQUAL(1, partitionTable[j]); + for (int j=9; j<=16; j++) + LONGS_EQUAL(0, partitionTable[j]); + for (int j=17; j<=24; j++) + LONGS_EQUAL(2, partitionTable[j]); + LONGS_EQUAL(1, partitionTable[25]); + LONGS_EQUAL(1, partitionTable[26]); + LONGS_EQUAL(2, partitionTable[27]); + LONGS_EQUAL(2, partitionTable[28]); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/partition/testGenericGraph.cpp b/gtsam_unstable/partition/testGenericGraph.cpp new file mode 100644 index 000000000..e8e9988bc --- /dev/null +++ b/gtsam_unstable/partition/testGenericGraph.cpp @@ -0,0 +1,254 @@ +/* + * testGenericGraph.cpp + * + * Created on: Nov 23, 2010 + * Author: nikai + * Description: unit tests for generic graph + */ + +#include +#include // for operator += +#include // for operator += +#include // for operator += +using namespace boost::assign; +#include +#include +#include + +#include "partition/GenericGraph.h" + +using namespace std; +using namespace gtsam; +using namespace gtsam::partition; + +/* ************************************************************************* */ +/** + * l7 l9 + * / | \ / | + * x1 -x2-x3 - l8 - x4- x5-x6 + */ +TEST ( GenerciGraph, findIslands ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(5, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(6, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); + + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_POSE_2D)); + graph.push_back(boost::make_shared(5, NODE_POSE_2D, 6, NODE_POSE_2D)); + std::vector keys; keys += 1, 2, 3, 4, 5, 6, 7, 8, 9; + + WorkSpace workspace(10); // from 0 to 9 + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(2, islands.size()); + vector island1; island1 += 1, 2, 3, 7, 8; + vector island2; island2 += 4, 5, 6, 9; + CHECK(island1 == islands.front()); + CHECK(island2 == islands.back()); +} + +/* ************************************************************************* */ +/** + * l7 l8 + * / / | X | \ + * x1 -x2-x3 x4- x5-x6 + */ +TEST( GenerciGraph, findIslands2 ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(5, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(6, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_POSE_2D)); + graph.push_back(boost::make_shared(5, NODE_POSE_2D, 6, NODE_POSE_2D)); + std::vector keys; keys += 1, 2, 3, 4, 5, 6, 7, 8; + + WorkSpace workspace(15); // from 0 to 8, but testing over-allocation here + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(1, islands.size()); + vector island1; island1 += 1, 2, 3, 4, 5, 6, 7, 8; + CHECK(island1 == islands.front()); +} + +/* ************************************************************************* */ +/** + * x1 - l5 + * x2 - x3 - x4 - l6 + */ +TEST ( GenerciGraph, findIslands3 ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 6, NODE_LANDMARK_2D)); + + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 4, NODE_POSE_2D)); + std::vector keys; keys += 1, 2, 3, 4, 5, 6; + + WorkSpace workspace(7); // from 0 to 9 + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(2, islands.size()); + vector island1; island1 += 1, 5; + vector island2; island2 += 2, 3, 4, 6; + CHECK(island1 == islands.front()); + CHECK(island2 == islands.back()); +} + +/* ************************************************************************* */ +/** + * x3 - l4 - x7 + */ +TEST ( GenerciGraph, findIslands4 ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(7, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + std::vector keys; keys += 3, 4, 7; + + WorkSpace workspace(8); // from 0 to 7 + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(2, islands.size()); + vector island1; island1 += 3, 4; + vector island2; island2 += 7; + CHECK(island1 == islands.front()); + CHECK(island2 == islands.back()); +} + +/* ************************************************************************* */ +/** + * x1 - l5 - x2 + * | / \ | + * x3 x4 + */ +TEST ( GenerciGraph, findIslands5 ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 4, NODE_POSE_2D)); + + std::vector keys; keys += 1, 2, 3, 4, 5; + + WorkSpace workspace(6); // from 0 to 5 + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(2, islands.size()); + vector island1; island1 += 1, 3, 5; + vector island2; island2 += 2, 4; + CHECK(island1 == islands.front()); + CHECK(island2 == islands.back()); +} + +/* ************************************************************************* */ +/** + * l3 l4 l5 l6 + * \ | / \ / + * x1 x2 + */ +TEST ( GenerciGraph, reduceGenericGraph ) +{ + GenericGraph3D graph; + graph.push_back(boost::make_shared(1, 3)); + graph.push_back(boost::make_shared(1, 4)); + graph.push_back(boost::make_shared(1, 5)); + graph.push_back(boost::make_shared(2, 5)); + graph.push_back(boost::make_shared(2, 6)); + + std::vector cameraKeys, landmarkKeys; + cameraKeys.push_back(1); + cameraKeys.push_back(2); + landmarkKeys.push_back(3); + landmarkKeys.push_back(4); + landmarkKeys.push_back(5); + landmarkKeys.push_back(6); + + std::vector dictionary; + dictionary.resize(7, -1); // from 0 to 6 + dictionary[1] = 0; + dictionary[2] = 1; + + GenericGraph3D reduced; + std::map > cameraToLandmarks; + reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced); + LONGS_EQUAL(1, reduced.size()); + LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index); +} + +/* ************************************************************************* */ +/** + * l3 l4 l5 l6 + * \ | / \ / + * x1 x2 - x7 + */ +TEST ( GenericGraph, reduceGenericGraph2 ) +{ + GenericGraph3D graph; + graph.push_back(boost::make_shared(1, 3, 0, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(1, 4, 1, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(1, 5, 2, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(2, 5, 3, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(2, 6, 4, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(2, 7, 5, NODE_POSE_3D, NODE_POSE_3D)); + + std::vector cameraKeys, landmarkKeys; + cameraKeys.push_back(1); + cameraKeys.push_back(2); + cameraKeys.push_back(7); + landmarkKeys.push_back(3); + landmarkKeys.push_back(4); + landmarkKeys.push_back(5); + landmarkKeys.push_back(6); + + std::vector dictionary; + dictionary.resize(8, -1); // from 0 to 7 + dictionary[1] = 0; + dictionary[2] = 1; + dictionary[7] = 6; + + GenericGraph3D reduced; + std::map > cameraToLandmarks; + reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced); + LONGS_EQUAL(2, reduced.size()); + LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index); + LONGS_EQUAL(2, reduced[1]->key1.index); LONGS_EQUAL(7, reduced[1]->key2.index); +} + +/* ************************************************************************* */ +TEST ( GenerciGraph, hasCommonCamera ) +{ + std::set cameras1; cameras1 += 1, 2, 3, 4, 5; + std::set cameras2; cameras2 += 8, 7, 6, 5; + bool actual = hasCommonCamera(cameras1, cameras2); + CHECK(actual); +} + +/* ************************************************************************* */ +TEST ( GenerciGraph, hasCommonCamera2 ) +{ + std::set cameras1; cameras1 += 1, 3, 5, 7; + std::set cameras2; cameras2 += 2, 4, 6, 8, 10; + bool actual = hasCommonCamera(cameras1, cameras2); + CHECK(!actual); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/partition/testNestedDissection.cpp b/gtsam_unstable/partition/testNestedDissection.cpp new file mode 100644 index 000000000..906178914 --- /dev/null +++ b/gtsam_unstable/partition/testNestedDissection.cpp @@ -0,0 +1,339 @@ +/* + * testNestedDissection.cpp + * + * Created on: Nov 29, 2010 + * Author: nikai + * Description: unit tests for NestedDissection + */ + +#include // for operator += +#include // for operator += +#include // for operator += +using namespace boost::assign; +#include +#include +#include + +#include "SubmapPlanarSLAM.h" +#include "SubmapVisualSLAM.h" +#include "SubmapExamples.h" +#include "SubmapExamples3D.h" +#include "GenericGraph.h" +#include "NonlinearTSAM.h" +#include "partition/NestedDissection-inl.h" + +using namespace std; +using namespace gtsam; +using namespace gtsam::partition; + +/* ************************************************************************* */ +// x1 - x2 +// \ / +// l1 +TEST ( NestedDissection, oneIsland ) +{ + using namespace submapPlanarSLAM; + typedef TSAM2D::SubNLG SubNLG; + Graph fg; + fg.addOdometry(1, 2, Pose2(), odoNoise); + fg.addBearingRange(1, 1, Rot2(), 0., bearingRangeNoise); + fg.addBearingRange(2, 1, Rot2(), 0., bearingRangeNoise); + fg.addPoseConstraint(1, Pose2()); + + Ordering ordering; ordering += x1, x2, l1; + + int numNodeStopPartition = 1e3; + int minNodesPerMap = 1e3; + NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); + LONGS_EQUAL(4, nd.root()->size()); + LONGS_EQUAL(3, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->children().size()); +} + +/* ************************************************************************* */ +// x1\ /x4 +// | x3 | +// x2/ \x5 +TEST ( NestedDissection, TwoIslands ) +{ + using namespace submapPlanarSLAM; + typedef TSAM2D::SubNLG SubNLG; + Graph fg; + fg.addOdometry(1, 2, Pose2(), odoNoise); + fg.addOdometry(1, 3, Pose2(), odoNoise); + fg.addOdometry(2, 3, Pose2(), odoNoise); + fg.addOdometry(3, 4, Pose2(), odoNoise); + fg.addOdometry(4, 5, Pose2(), odoNoise); + fg.addOdometry(3, 5, Pose2(), odoNoise); + fg.addPoseConstraint(1, Pose2()); + fg.addPoseConstraint(4, Pose2()); + Ordering ordering; ordering += x1, x2, x3, x4, x5; + + int numNodeStopPartition = 2; + int minNodesPerMap = 1; + NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); + // root submap + LONGS_EQUAL(0, nd.root()->size()); + LONGS_EQUAL(1, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps + + // the 1st submap + LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); + LONGS_EQUAL(4, nd.root()->children()[0]->size()); + + // the 2nd submap + LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); + LONGS_EQUAL(4, nd.root()->children()[1]->size()); +} + +/* ************************************************************************* */ +// x1\ /x4 +// x3 +// x2/ \x5 +TEST ( NestedDissection, FourIslands ) +{ + using namespace submapPlanarSLAM; + typedef TSAM2D::SubNLG SubNLG; + Graph fg; + fg.addOdometry(1, 3, Pose2(), odoNoise); + fg.addOdometry(2, 3, Pose2(), odoNoise); + fg.addOdometry(3, 4, Pose2(), odoNoise); + fg.addOdometry(3, 5, Pose2(), odoNoise); + fg.addPoseConstraint(1, Pose2()); + fg.addPoseConstraint(4, Pose2()); + Ordering ordering; ordering += x1, x2, x3, x4, x5; + + int numNodeStopPartition = 2; + int minNodesPerMap = 1; + NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); + LONGS_EQUAL(0, nd.root()->size()); + LONGS_EQUAL(1, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(4, nd.root()->children().size()); // 4 leaf submaps + + // the 1st submap + LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); + LONGS_EQUAL(2, nd.root()->children()[0]->size()); + + // the 2nd submap + LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); + LONGS_EQUAL(2, nd.root()->children()[1]->size()); + + // the 3rd submap + LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); + LONGS_EQUAL(1, nd.root()->children()[2]->size()); + + // the 4th submap + LONGS_EQUAL(1, nd.root()->children()[3]->frontal().size()); + LONGS_EQUAL(1, nd.root()->children()[3]->size()); +} + +/* ************************************************************************* */ +// x1\ /x3 +// | x2 | +// l6/ \x4 +// | +// x5 +TEST ( NestedDissection, weekLinks ) +{ + using namespace submapPlanarSLAM; + typedef TSAM2D::SubNLG SubNLG; + Graph fg; + fg.addOdometry(1, 2, Pose2(), odoNoise); + fg.addOdometry(2, 3, Pose2(), odoNoise); + fg.addOdometry(2, 4, Pose2(), odoNoise); + fg.addOdometry(3, 4, Pose2(), odoNoise); + fg.addBearingRange(1, 6, Rot2(), 0., bearingRangeNoise); + fg.addBearingRange(2, 6, Rot2(), 0., bearingRangeNoise); + fg.addBearingRange(5, 6, Rot2(), 0., bearingRangeNoise); + fg.addPoseConstraint(1, Pose2()); + fg.addPoseConstraint(4, Pose2()); + fg.addPoseConstraint(5, Pose2()); + Ordering ordering; ordering += x1, x2, x3, x4, x5, l6; + + int numNodeStopPartition = 2; + int minNodesPerMap = 1; + NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); + LONGS_EQUAL(0, nd.root()->size()); // one weeklink + LONGS_EQUAL(1, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(3, nd.root()->children().size()); // 4 leaf submaps + LONGS_EQUAL(1, nd.root()->weeklinks().size()); + + // the 1st submap + LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); // x3 and x4 + LONGS_EQUAL(4, nd.root()->children()[0]->size()); + + // the 2nd submap + LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); // x1 and l6 + LONGS_EQUAL(4, nd.root()->children()[1]->size()); + // + // the 3rd submap + LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); // x5 + LONGS_EQUAL(1, nd.root()->children()[2]->size()); +} + +/* ************************************************************************* */ +/** + * l1 l2 l3 + * | X | X | + * x0 - x1 - x2 + * | X | X | + * l4 l5 l6 + */ +TEST ( NestedDissection, manual_cuts ) +{ + using namespace submapPlanarSLAM; + typedef partition::Cuts Cuts; + typedef TSAM2D::SubNLG SubNLG; + typedef partition::PartitionTable PartitionTable; + Graph fg; + fg.addOdometry(x0, x1, Pose2(1.0, 0, 0), odoNoise); + fg.addOdometry(x1, x2, Pose2(1.0, 0, 0), odoNoise); + + fg.addBearingRange(x0, l1, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x0, l4, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x0, l2, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x0, l5, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise); + + fg.addBearingRange(x1, l1, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x1, l2, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x1, l3, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x1, l4, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x1, l5, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x1, l6, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise); + + fg.addBearingRange(x2, l2, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x2, l5, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x2, l3, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x2, l6, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise); + + fg.addPrior(x0, Pose2(0.1, 0, 0), priorNoise); + + // generate ordering + Ordering ordering; ordering += x0, x1, x2, l1, l2, l3, l4, l5, l6; + + // define cuts + boost::shared_ptr cuts(new Cuts()); + cuts->partitionTable = PartitionTable(9, -1); PartitionTable* p = &cuts->partitionTable; + //x0 x1 x2 l1 l2 l3 l4 l5 l6 + (*p)[0]=1; (*p)[1]=0; (*p)[2]=2; (*p)[3]=1; (*p)[4]=0; (*p)[5]=2; (*p)[6]=1; (*p)[7]=0; (*p)[8]=2; + + cuts->children.push_back(boost::shared_ptr(new Cuts())); + cuts->children[0]->partitionTable = PartitionTable(9, -1); p = &cuts->children[0]->partitionTable; + //x0 x1 x2 l1 l2 l3 l4 l5 l6 + (*p)[0]=0; (*p)[1]=-1; (*p)[2]=-1; (*p)[3]=1; (*p)[4]=-1; (*p)[5]=-1; (*p)[6]=2; (*p)[7]=-1; (*p)[8]=-1; + + cuts->children.push_back(boost::shared_ptr(new Cuts())); + cuts->children[1]->partitionTable = PartitionTable(9, -1); p = &cuts->children[1]->partitionTable; + //x0 x1 x2 l1 l2 l3 l4 l5 l6 + (*p)[0]=-1; (*p)[1]=-1; (*p)[2]=0; (*p)[3]=-1; (*p)[4]=-1; (*p)[5]=1; (*p)[6]=-1; (*p)[7]=-1; (*p)[8]=2; + + + // nested dissection + NestedDissection nd(fg, ordering, cuts); + LONGS_EQUAL(2, nd.root()->size()); + LONGS_EQUAL(3, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps + LONGS_EQUAL(0, nd.root()->weeklinks().size()); + + // the 1st submap + LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); // x0 + LONGS_EQUAL(4, nd.root()->children()[0]->size()); + LONGS_EQUAL(2, nd.root()->children()[0]->children().size()); + + // the 1-1st submap + LONGS_EQUAL(1, nd.root()->children()[0]->children()[0]->frontal().size()); // l1 + LONGS_EQUAL(2, nd.root()->children()[0]->children()[0]->size()); + + // the 1-2nd submap + LONGS_EQUAL(1, nd.root()->children()[0]->children()[1]->frontal().size()); // l4 + LONGS_EQUAL(2, nd.root()->children()[0]->children()[1]->size()); + + // the 2nd submap + LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); // x2 + LONGS_EQUAL(3, nd.root()->children()[1]->size()); + LONGS_EQUAL(2, nd.root()->children()[1]->children().size()); + + // the 2-1st submap + LONGS_EQUAL(1, nd.root()->children()[1]->children()[0]->frontal().size()); // l3 + LONGS_EQUAL(2, nd.root()->children()[1]->children()[0]->size()); + + // the 2-2nd submap + LONGS_EQUAL(1, nd.root()->children()[1]->children()[1]->frontal().size()); // l6 + LONGS_EQUAL(2, nd.root()->children()[1]->children()[1]->size()); + +} + +/* ************************************************************************* */ +// l1-l8 l9-16 l17-124 +// / | / \ | \ +// x0 x1 x2 x3 +TEST( NestedDissection, Graph3D) { + using namespace gtsam::submapVisualSLAM; + typedef TSAM3D::SubNLG SubNLG; + typedef partition::PartitionTable PartitionTable; + vector cameras; + cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-2., 0., 0.)))); + cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-1., 0., 0.)))); + cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 1., 0., 0.)))); + cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 2., 0., 0.)))); + + vector points; + for (int cube_index = 0; cube_index <= 3; cube_index++) { + Point3 center((cube_index-1) * 3, 0.5, 10.); + points.push_back(center + Point3(-0.5, -0.5, -0.5)); + points.push_back(center + Point3(-0.5, 0.5, -0.5)); + points.push_back(center + Point3( 0.5, 0.5, -0.5)); + points.push_back(center + Point3( 0.5, -0.5, -0.5)); + points.push_back(center + Point3(-0.5, -0.5, 0.5)); + points.push_back(center + Point3(-0.5, 0.5, 0.5)); + points.push_back(center + Point3( 0.5, 0.5, 0.5)); + points.push_back(center + Point3( 0.5, 0.5, 0.5)); + } + + Graph graph; + SharedDiagonal measurementNoise(gtsam::Vector_(2, 1., 1.)); + SharedDiagonal measurementZeroNoise(gtsam::Vector_(2, 0., 0.)); + for (int j=1; j<=8; j++) + graph.addMeasurement(0, j, cameras[0].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); + for (int j=1; j<=16; j++) + graph.addMeasurement(1, j, cameras[1].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); + for (int j=9; j<=24; j++) + graph.addMeasurement(2, j, cameras[2].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); + for (int j=17; j<=24; j++) + graph.addMeasurement(3, j, cameras[3].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); + + // make an easy ordering + Ordering ordering; ordering += x0, x1, x2, x3; + for (int j=1; j<=24; j++) + ordering += Symbol('l', j); + + // nested dissection + const int numNodeStopPartition = 10; + const int minNodesPerMap = 5; + NestedDissection nd(graph, ordering, numNodeStopPartition, minNodesPerMap); + + LONGS_EQUAL(0, nd.root()->size()); + LONGS_EQUAL(8, nd.root()->frontal().size()); // l9-l16 + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps + LONGS_EQUAL(0, nd.root()->weeklinks().size()); + + // the 1st submap + LONGS_EQUAL(10, nd.root()->children()[0]->frontal().size()); // x0, x1, l1-l8 + LONGS_EQUAL(24, nd.root()->children()[0]->size()); // 8 + 16 + LONGS_EQUAL(0, nd.root()->children()[0]->children().size()); + + // the 2nd submap + LONGS_EQUAL(10, nd.root()->children()[1]->frontal().size()); // x2, x3, l1-l8 + LONGS_EQUAL(24, nd.root()->children()[1]->size()); // 16 + 8 + LONGS_EQUAL(0, nd.root()->children()[1]->children().size()); +} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */