Move partition over
Andrew Melim
parent
a6ff1eb9ec
commit
1d9aa38a62
|
|
@ -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 <stdexcept>
|
||||
#include <iostream>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/shared_array.hpp>
|
||||
|
||||
extern "C" {
|
||||
#include <metis.h>
|
||||
}
|
||||
|
||||
#include "FindSeparator.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam { namespace partition {
|
||||
|
||||
typedef boost::shared_array<idxtype> 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<int, sharedInts> 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<int, sharedInts> 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 <class GenericGraph>
|
||||
void prepareMetisGraph(const GenericGraph& graph, const vector<size_t>& keys, WorkSpace& workspace,
|
||||
sharedInts* ptr_xadj, sharedInts* ptr_adjncy, sharedInts* ptr_adjwgt) {
|
||||
|
||||
typedef int Weight;
|
||||
typedef vector<int> Weights;
|
||||
typedef vector<int> Neighbors;
|
||||
typedef pair<Neighbors, Weights> NeighborsInfo;
|
||||
|
||||
// set up dictionary
|
||||
std::vector<int>& 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<NeighborsInfo> 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<Neighbors, Weights>& adjancyMap1 = adjancyMap[index1];
|
||||
pair<Neighbors, Weights>& 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<class GenericGraph>
|
||||
boost::optional<MetisResult> separatorPartitionByMetis(const GenericGraph& graph, const vector<size_t>& 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<GenericGraph>(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<MetisResult>();
|
||||
|
||||
// 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<size_t>::const_iterator itKey = keys.begin();
|
||||
vector<size_t>::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<MetisResult >(result);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class GenericGraph>
|
||||
boost::optional<MetisResult> edgePartitionByMetis(const GenericGraph& graph, const vector<size_t>& 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<GenericGraph>(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<size_t>::const_iterator itKey = keys.begin();
|
||||
vector<size_t>::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<MetisResult >(result);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool isLargerIsland(const vector<size_t>& island1, const vector<size_t>& island2) {
|
||||
return island1.size() > island2.size();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// debug functions
|
||||
void printIsland(const vector<size_t>& island) {
|
||||
cout << "island: ";
|
||||
BOOST_FOREACH(const size_t key, island)
|
||||
cout << key << " ";
|
||||
cout << endl;
|
||||
}
|
||||
|
||||
void printIslands(const list<vector<size_t> >& islands) {
|
||||
BOOST_FOREACH(const vector<size_t>& island, islands)
|
||||
printIsland(island);
|
||||
}
|
||||
|
||||
void printNumCamerasLandmarks(const vector<size_t>& keys, const vector<Symbol>& 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<class GenericGraph>
|
||||
void addLandmarkToPartitionResult(const GenericGraph& graph, const vector<size_t>& landmarkKeys,
|
||||
MetisResult& partitionResult, WorkSpace& workspace) {
|
||||
|
||||
// set up cameras in the dictionary
|
||||
std::vector<size_t>& A = partitionResult.A;
|
||||
std::vector<size_t>& B = partitionResult.B;
|
||||
std::vector<size_t>& C = partitionResult.C;
|
||||
std::vector<int>& 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<class GenericGraph>
|
||||
boost::optional<MetisResult> findPartitoning(const GenericGraph& graph, const vector<size_t>& keys,
|
||||
WorkSpace& workspace, bool verbose,
|
||||
const boost::optional<vector<Symbol> >& int2symbol, const bool reduceGraph) {
|
||||
boost::optional<MetisResult> result;
|
||||
GenericGraph reducedGraph;
|
||||
vector<size_t> keyToPartition;
|
||||
vector<size_t> 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<int>& 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<class GenericGraph>
|
||||
int findSeparator(const GenericGraph& graph, const vector<size_t>& keys,
|
||||
const int minNodesPerMap, WorkSpace& workspace, bool verbose,
|
||||
const boost::optional<vector<Symbol> >& int2symbol, const bool reduceGraph,
|
||||
const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) {
|
||||
|
||||
boost::optional<MetisResult> result = findPartitoning(graph, keys, workspace, verbose, int2symbol, reduceGraph);
|
||||
|
||||
// find the island in A and B, and make them separated submaps
|
||||
typedef vector<size_t> Island;
|
||||
list<Island> islands;
|
||||
list<Island> islands_in_A = findIslands(graph, result->A, workspace, minNrConstraintsPerCamera, minNrConstraintsPerLandmark);
|
||||
list<Island> 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<Island>::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<int>& 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
|
||||
|
|
@ -0,0 +1,43 @@
|
|||
/*
|
||||
* FindSeparator.h
|
||||
*
|
||||
* Created on: Nov 23, 2010
|
||||
* Author: nikai
|
||||
* Description: find the separator of bisectioning for a given graph
|
||||
*/
|
||||
|
||||
#include <map>
|
||||
#include <vector>
|
||||
#include <boost/optional.hpp>
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
|
||||
#include "PartitionWorkSpace.h"
|
||||
|
||||
namespace gtsam { namespace partition {
|
||||
|
||||
// typedef std::map<size_t, size_t> PartitionTable; // from the key to the partition: 0 - separator, > 1: submap id
|
||||
|
||||
/** the metis Nest dissection result */
|
||||
struct MetisResult {
|
||||
std::vector<size_t> A, B; // frontals
|
||||
std::vector<size_t> 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<class GenericGraph>
|
||||
boost::optional<MetisResult> separatorPartitionByMetis(const GenericGraph& graph, const std::vector<size_t>& 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<class GenericGraph>
|
||||
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);
|
||||
|
||||
}} //namespace
|
||||
|
|
@ -0,0 +1,476 @@
|
|||
/*
|
||||
* GenericGraph2D.cpp
|
||||
*
|
||||
* Created on: Nov 23, 2010
|
||||
* Author: nikai
|
||||
* Description: generic graph types used in partitioning
|
||||
*/
|
||||
#include <iostream>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
#include <gtsam/base/DSFVector.h>
|
||||
|
||||
#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<vector<size_t> > findIslands(const GenericGraph2D& graph, const vector<size_t>& keys, WorkSpace& workspace,
|
||||
const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark)
|
||||
{
|
||||
typedef pair<int, int> IntPair;
|
||||
typedef list<sharedGenericFactor2D> FactorList;
|
||||
typedef map<IntPair, FactorList::iterator> 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<FactorList::iterator> 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<vector<size_t> > islands;
|
||||
map<size_t, vector<size_t> > arrays = dsf.arrays();
|
||||
size_t key; vector<size_t> 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<size_t>& keys, const WorkSpace& workspace) {
|
||||
DSFVector dsf(workspace.dsf, keys);
|
||||
typedef list<sharedGenericFactor3D> 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<FactorList::iterator> 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<size_t>& singularCameras, const set<size_t>& 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<bool>& isCamera, const vector<bool>& isLandmark,
|
||||
set<size_t>& singularCameras, set<size_t>& singularLandmarks, vector<int>& 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<nrConstraints.size(); i++) {
|
||||
if (isCamera[i] && nrConstraints[i] < minNrConstraintsPerCamera &&
|
||||
singularCameras.find(i) == singularCameras.end()) {
|
||||
singularCameras.insert(i);
|
||||
foundSingularCamera = true;
|
||||
}
|
||||
if (isLandmark[i] && nrConstraints[i] < minNrConstraintsPerLandmark &&
|
||||
singularLandmarks.find(i) == singularLandmarks.end()) {
|
||||
singularLandmarks.insert(i);
|
||||
foundSingularLandmark = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
list<vector<size_t> > findIslands(const GenericGraph3D& graph, const vector<size_t>& 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<vector<size_t> > islands;
|
||||
set<size_t> singularCameras, singularLandmarks;
|
||||
vector<bool> isCamera(workspace.dictionary.size(), false);
|
||||
vector<bool> 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<int> 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<size_t>(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<size_t>(1, i)); // <---------------------------
|
||||
if (verbose) cout << i << " ";
|
||||
}
|
||||
if (verbose) cout << endl;
|
||||
}
|
||||
|
||||
|
||||
// regenerating islands
|
||||
map<size_t, vector<size_t> > labelIslands = dsf.arrays();
|
||||
size_t label; vector<size_t> island;
|
||||
BOOST_FOREACH(boost::tie(label, island), labelIslands) {
|
||||
vector<size_t> 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<size_t>& 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<size_t>& landmarks1, const vector<size_t>& 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<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys,
|
||||
const std::vector<int>& dictionary, GenericGraph3D& reducedGraph) {
|
||||
|
||||
typedef size_t CameraKey;
|
||||
typedef pair<CameraKey, CameraKey> CameraPair;
|
||||
typedef size_t LandmarkKey;
|
||||
// get a mapping from each landmark to its connected cameras
|
||||
vector<vector<LandmarkKey> > cameraToLandmarks(dictionary.size());
|
||||
// for odometry xi-xj where i<j, we always store cameraToCamera[i] = j, otherwise equal to -1 if no odometry
|
||||
vector<int> 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<LandmarkKey> &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<cameraKeys.size()-1; ++i1) {
|
||||
for (int i2=i1+1; i2<cameraKeys.size(); ++i2) {
|
||||
camera1 = cameraKeys[i1];
|
||||
camera2 = cameraKeys[i2];
|
||||
int nrCommonLandmarks = getNrCommonLandmarks(cameraToLandmarks[camera1], cameraToLandmarks[camera2]);
|
||||
hasOdometry = cameraToCamera[camera1] == camera2;
|
||||
if (nrCommonLandmarks > 0 || hasOdometry) {
|
||||
nrTotalConstraints = 2 * nrCommonLandmarks + (hasOdometry ? 6 : 0);
|
||||
reducedGraph.push_back(boost::make_shared<GenericFactor3D>(camera1, camera2,
|
||||
factorIndex++, NODE_POSE_3D, NODE_POSE_3D, nrTotalConstraints));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& frontals,
|
||||
WorkSpace& workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) {
|
||||
workspace.prepareDictionary(frontals);
|
||||
vector<size_t> nrConstraints(workspace.dictionary.size(), 0);
|
||||
|
||||
// summarize the constraint number
|
||||
const vector<int>& dictionary = workspace.dictionary;
|
||||
vector<bool> isValidCamera(workspace.dictionary.size(), false);
|
||||
vector<bool> 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<isValidCamera.size(); i++) {
|
||||
if (isValidCamera[i]) {
|
||||
minFoundConstraintsPerCamera = std::min(nrConstraints[i], minFoundConstraintsPerCamera);
|
||||
if (nrConstraints[i] < minNrConstraintsPerCamera)
|
||||
cout << "!!!!!!!!!!!!!!!!!!! camera with " << nrConstraints[i] << " constraint: " << i << endl;
|
||||
}
|
||||
|
||||
}
|
||||
for (int j=0; j<isValidLandmark.size(); j++) {
|
||||
if (isValidLandmark[j]) {
|
||||
minFoundConstraintsPerLandmark = std::min(nrConstraints[j], minFoundConstraintsPerLandmark);
|
||||
if (nrConstraints[j] < minNrConstraintsPerLandmark)
|
||||
cout << "!!!!!!!!!!!!!!!!!!! landmark with " << nrConstraints[j] << " constraint: " << j << endl;
|
||||
}
|
||||
}
|
||||
|
||||
// debug info
|
||||
BOOST_FOREACH(const size_t key, frontals) {
|
||||
if (isValidCamera[key] && nrConstraints[key] < minNrConstraintsPerCamera)
|
||||
cout << "singular camera:" << key << " with " << nrConstraints[key] << " constraints" << endl;
|
||||
}
|
||||
|
||||
if (minFoundConstraintsPerCamera < minNrConstraintsPerCamera)
|
||||
throw runtime_error("checkSingularity:minConstraintsPerCamera < " + minFoundConstraintsPerCamera);
|
||||
if (minFoundConstraintsPerLandmark < minNrConstraintsPerLandmark)
|
||||
throw runtime_error("checkSingularity:minConstraintsPerLandmark < " + minFoundConstraintsPerLandmark);
|
||||
}
|
||||
|
||||
}} // namespace
|
||||
|
|
@ -0,0 +1,149 @@
|
|||
/*
|
||||
* GenericGraph.h
|
||||
*
|
||||
* Created on: Nov 22, 2010
|
||||
* Author: nikai
|
||||
* Description: generic graph types used in partitioning
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <list>
|
||||
#include <vector>
|
||||
#include <stdexcept>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
#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<GenericFactor2D> sharedGenericFactor2D;
|
||||
typedef std::vector<sharedGenericFactor2D> GenericGraph2D;
|
||||
|
||||
/** merge nodes in DSF using constraints captured by the given graph */
|
||||
std::list<std::vector<size_t> > findIslands(const GenericGraph2D& graph, const std::vector<size_t>& keys, WorkSpace& workspace,
|
||||
const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark);
|
||||
|
||||
/** eliminate the sensors from generic graph */
|
||||
inline void reduceGenericGraph(const GenericGraph2D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys,
|
||||
const std::vector<int>& 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<size_t>& 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<GenericFactor3D> sharedGenericFactor3D;
|
||||
typedef std::vector<sharedGenericFactor3D> GenericGraph3D;
|
||||
|
||||
/** merge nodes in DSF using constraints captured by the given graph */
|
||||
std::list<std::vector<size_t> > findIslands(const GenericGraph3D& graph, const std::vector<size_t>& keys, WorkSpace& workspace,
|
||||
const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark);
|
||||
|
||||
/** eliminate the sensors from generic graph */
|
||||
void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys,
|
||||
const std::vector<int>& dictionary, GenericGraph3D& reducedGraph);
|
||||
|
||||
/** check whether the 3D graph is singular (under constrained) */
|
||||
void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& 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<GenericUnaryFactor> sharedGenericUnaryFactor;
|
||||
typedef std::vector<sharedGenericUnaryFactor> GenericUnaryGraph;
|
||||
|
||||
/***************************************************
|
||||
* utility functions
|
||||
***************************************************/
|
||||
inline bool hasCommonCamera(const std::set<size_t>& cameras1, const std::set<size_t>& cameras2) {
|
||||
if (cameras1.empty() || cameras2.empty())
|
||||
throw std::invalid_argument("hasCommonCamera: the input camera set is empty!");
|
||||
std::set<size_t>::const_iterator it1 = cameras1.begin();
|
||||
std::set<size_t>::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
|
||||
|
|
@ -0,0 +1,253 @@
|
|||
/*
|
||||
* NestedDissection-inl.h
|
||||
*
|
||||
* Created on: Nov 27, 2010
|
||||
* Author: nikai
|
||||
* Description:
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
#include "partition/FindSeparator-inl.h"
|
||||
#include "OrderedSymbols.h"
|
||||
#include "NestedDissection.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam { namespace partition {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
NestedDissection<NLG, SubNLG, GenericGraph>::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<size_t> keys;
|
||||
keys.reserve(numNodes);
|
||||
for(int i=0; i<ordering.size(); ++i)
|
||||
keys.push_back(i);
|
||||
|
||||
WorkSpace workspace(numNodes);
|
||||
root_ = recursivePartition(gfg, unaryFactors, keys, vector<size_t>(), numNodeStopPartition, minNodesPerMap, boost::shared_ptr<SubNLG>(), workspace, verbose);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
NestedDissection<NLG, SubNLG, GenericGraph>::NestedDissection(
|
||||
const NLG& fg, const Ordering& ordering, const boost::shared_ptr<Cuts>& 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<size_t> keys;
|
||||
keys.reserve(numNodes);
|
||||
for(int i=0; i<ordering.size(); ++i)
|
||||
keys.push_back(i);
|
||||
|
||||
WorkSpace workspace(numNodes);
|
||||
root_ = recursivePartition(gfg, unaryFactors, keys, vector<size_t>(), cuts, boost::shared_ptr<SubNLG>(), workspace, verbose);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::makeSubNLG(
|
||||
const NLG& fg, const vector<size_t>& frontals, const vector<size_t>& sep, const boost::shared_ptr<SubNLG>& 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<SubNLG>(fg, frontalKeys, sepKeys, parent);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
void NestedDissection<NLG, SubNLG, GenericGraph>::processFactor(
|
||||
const typename GenericGraph::value_type& factor, const std::vector<int>& partitionTable, // input
|
||||
vector<GenericGraph>& frontalFactors, NLG& sepFactors, vector<set<size_t> >& childSeps, // output factor graphs
|
||||
typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques
|
||||
list<size_t> 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 <class NLG, class SubNLG, class GenericGraph>
|
||||
void NestedDissection<NLG, SubNLG, GenericGraph>::partitionFactorsAndVariables(
|
||||
const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& keys, //input
|
||||
const std::vector<int>& partitionTable, const int numSubmaps, // input
|
||||
vector<GenericGraph>& frontalFactors, vector<GenericUnaryGraph>& frontalUnaryFactors, NLG& sepFactors, // output factor graphs
|
||||
vector<vector<size_t> >& childFrontals, vector<vector<size_t> >& childSeps, vector<size_t>& 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<set<size_t> > 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<size_t>& childSep, childSeps_)
|
||||
childSeps.push_back(vector<size_t>(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 <class NLG, class SubNLG, class GenericGraph>
|
||||
NLG NestedDissection<NLG, SubNLG, GenericGraph>::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 <class NLG, class SubNLG, class GenericGraph>
|
||||
boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::recursivePartition(
|
||||
const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector<size_t>& frontals, const vector<size_t>& sep,
|
||||
const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr<SubNLG>& 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<GenericGraph> frontalFactors; vector<GenericUnaryGraph> frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks;
|
||||
vector<size_t> localFrontals; vector<vector<size_t> > childFrontals, childSeps;
|
||||
partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps,
|
||||
frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks);
|
||||
|
||||
// make a new cluster
|
||||
boost::shared_ptr<SubNLG> current = makeSubNLG(sepFactors, localFrontals, sep, parent);
|
||||
current->setWeeklinks(weeklinks);
|
||||
|
||||
// check whether all the submaps are fully constrained
|
||||
for (int i=0; i<numSubmaps; i++) {
|
||||
checkSingularity(frontalFactors[i], childFrontals[i], workspace, NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark());
|
||||
}
|
||||
|
||||
// create child clusters
|
||||
for (int i=0; i<numSubmaps; i++) {
|
||||
boost::shared_ptr<SubNLG> child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i],
|
||||
numNodeStopPartition, minNodesPerMap, current, workspace, verbose);
|
||||
current->addChild(child);
|
||||
}
|
||||
|
||||
return current;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::recursivePartition(
|
||||
const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector<size_t>& frontals, const vector<size_t>& sep,
|
||||
const boost::shared_ptr<Cuts>& cuts, const boost::shared_ptr<SubNLG>& 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<GenericGraph> frontalFactors; vector<GenericUnaryGraph> frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks;
|
||||
vector<size_t> localFrontals; vector<vector<size_t> > childFrontals, childSeps;
|
||||
partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps,
|
||||
frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks);
|
||||
|
||||
// make a new cluster
|
||||
boost::shared_ptr<SubNLG> current = makeSubNLG(sepFactors, localFrontals, sep, parent);
|
||||
current->setWeeklinks(weeklinks);
|
||||
|
||||
// create child clusters
|
||||
for (int i=0; i<2; i++) {
|
||||
boost::shared_ptr<SubNLG> child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i],
|
||||
cuts->children.empty() ? boost::shared_ptr<Cuts>() : cuts->children[i], current, workspace, verbose);
|
||||
current->addChild(child);
|
||||
}
|
||||
return current;
|
||||
}
|
||||
}} //namespace
|
||||
|
|
@ -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 <vector>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
|
||||
namespace gtsam { namespace partition {
|
||||
|
||||
/**
|
||||
* Apply nested dissection algorithm to nonlinear factor graphs
|
||||
*/
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
class NestedDissection {
|
||||
public:
|
||||
typedef boost::shared_ptr<SubNLG> sharedSubNLG;
|
||||
|
||||
private:
|
||||
NLG fg_; // the original nonlinear factor graph
|
||||
Ordering ordering_; // the variable ordering in the nonlinear factor graph
|
||||
std::vector<Symbol> 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>& cuts, const bool verbose = false);
|
||||
|
||||
private:
|
||||
/* convert generic subgraph to nonlinear subgraph */
|
||||
sharedSubNLG makeSubNLG(const NLG& fg, const std::vector<size_t>& frontals, const std::vector<size_t>& sep, const boost::shared_ptr<SubNLG>& parent) const;
|
||||
|
||||
void processFactor(const typename GenericGraph::value_type& factor, const std::vector<int>& partitionTable, // input
|
||||
std::vector<GenericGraph>& frontalFactors, NLG& sepFactors, std::vector<std::set<size_t> >& 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<size_t>& keys, const std::vector<int>& partitionTable, const int numSubmaps, // input
|
||||
std::vector<GenericGraph>& frontalFactors, vector<GenericUnaryGraph>& frontalUnaryFactors, NLG& sepFactors, // output factor graphs
|
||||
std::vector<std::vector<size_t> >& childFrontals, std::vector<std::vector<size_t> >& childSeps, std::vector<size_t>& 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<size_t>& frontals, const std::vector<size_t>& sep,
|
||||
const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const;
|
||||
|
||||
/* recursively partition the generic graph */
|
||||
sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& frontals, const std::vector<size_t>& sep,
|
||||
const boost::shared_ptr<Cuts>& cuts, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const;
|
||||
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
|
|
@ -0,0 +1,43 @@
|
|||
/*
|
||||
* PartitionWorkSpace.h
|
||||
*
|
||||
* Created on: Nov 24, 2010
|
||||
* Author: nikai
|
||||
* Description: a preallocated memory space used in partitioning
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
namespace gtsam { namespace partition {
|
||||
|
||||
typedef std::vector<int> PartitionTable;
|
||||
|
||||
// the work space, preallocated memory
|
||||
struct WorkSpace {
|
||||
std::vector<int> 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<std::vector<size_t> > 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<size_t>(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<size_t>& keys) {
|
||||
int index = 0;
|
||||
std::fill(dictionary.begin(), dictionary.end(), -1);
|
||||
std::vector<size_t>::const_iterator it=keys.begin(), itLast=keys.end();
|
||||
while(it!=itLast) dictionary[*(it++)] = index++;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
// manually defined cuts
|
||||
struct Cuts {
|
||||
PartitionTable partitionTable;
|
||||
std::vector<boost::shared_ptr<Cuts> > children;
|
||||
};
|
||||
|
||||
}} // namespace
|
||||
|
|
@ -0,0 +1,222 @@
|
|||
/*
|
||||
* testFindSeparator.cpp
|
||||
*
|
||||
* Created on: Nov 23, 2010
|
||||
* Author: nikai
|
||||
* Description: unit tests for FindSeparator
|
||||
*/
|
||||
|
||||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
#include <boost/assign/std/set.hpp> // for operator +=
|
||||
#include <boost/assign/std/vector.hpp> // for operator +=
|
||||
using namespace boost::assign;
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#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<GenericFactor2D>(0, NODE_POSE_2D, 3, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 4, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(0, NODE_POSE_2D, 1, NODE_POSE_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
|
||||
std::vector<size_t> keys; keys += 0, 1, 2, 3, 4;
|
||||
|
||||
WorkSpace workspace(5);
|
||||
boost::optional<MetisResult> actual = separatorPartitionByMetis<GenericGraph2D>(graph, keys, workspace, false);
|
||||
CHECK(actual.is_initialized());
|
||||
vector<size_t> A_expected; A_expected += 0, 3; // frontal
|
||||
vector<size_t> B_expected; B_expected += 2, 4; // frontal
|
||||
vector<size_t> 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<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 6, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
|
||||
std::vector<size_t> keys; keys += 1, 2, 3, 5, 6;
|
||||
WorkSpace workspace(8);
|
||||
boost::optional<MetisResult> actual = separatorPartitionByMetis<GenericGraph2D>(graph, keys, workspace, false);
|
||||
CHECK(actual.is_initialized());
|
||||
vector<size_t> A_expected; A_expected += 1, 5; // frontal
|
||||
vector<size_t> B_expected; B_expected += 3, 6; // frontal
|
||||
vector<size_t> 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<GenericFactor3D>(0, 2, 0, NODE_POSE_3D, NODE_POSE_3D));
|
||||
graph.push_back(boost::make_shared<GenericFactor3D>(2, 3, 1, NODE_POSE_3D, NODE_POSE_3D));
|
||||
graph.push_back(boost::make_shared<GenericFactor3D>(3, 5, 2, NODE_POSE_3D, NODE_POSE_3D));
|
||||
std::vector<size_t> keys; keys += 0, 2, 3, 5;
|
||||
|
||||
WorkSpace workspace(6);
|
||||
boost::optional<MetisResult> actual = edgePartitionByMetis<GenericGraph3D>(graph, keys, workspace, false);
|
||||
CHECK(actual.is_initialized());
|
||||
vector<size_t> A_expected; A_expected += 0, 2; // frontal
|
||||
vector<size_t> B_expected; B_expected += 3, 5; // frontal
|
||||
vector<size_t> 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<GenericFactor3D>(0, 2, 0, NODE_POSE_3D, NODE_POSE_3D, 1));
|
||||
graph.push_back(boost::make_shared<GenericFactor3D>(2, 3, 1, NODE_POSE_3D, NODE_POSE_3D, 1));
|
||||
graph.push_back(boost::make_shared<GenericFactor3D>(3, 5, 2, NODE_POSE_3D, NODE_POSE_3D, 20));
|
||||
graph.push_back(boost::make_shared<GenericFactor3D>(5, 6, 3, NODE_POSE_3D, NODE_POSE_3D, 1));
|
||||
std::vector<size_t> keys; keys += 0, 2, 3, 5, 6;
|
||||
|
||||
WorkSpace workspace(6);
|
||||
boost::optional<MetisResult> actual = edgePartitionByMetis<GenericGraph3D>(graph, keys, workspace, false);
|
||||
CHECK(actual.is_initialized());
|
||||
vector<size_t> A_expected; A_expected += 0, 2; // frontal
|
||||
vector<size_t> B_expected; B_expected += 3, 5, 6; // frontal
|
||||
vector<size_t> 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<GenericFactor2D>(0, NODE_POSE_2D, 3, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 4, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(0, NODE_POSE_2D, 1, NODE_POSE_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
|
||||
std::vector<size_t> keys; keys += 0, 1, 2, 3, 4;
|
||||
|
||||
WorkSpace workspace(5);
|
||||
int minNodesPerMap = -1;
|
||||
bool reduceGraph = false;
|
||||
int numSubmaps = findSeparator<GenericGraph2D>(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<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 6, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
|
||||
std::vector<size_t> keys; keys += 1, 2, 3, 5, 6;
|
||||
|
||||
WorkSpace workspace(8);
|
||||
int minNodesPerMap = -1;
|
||||
bool reduceGraph = false;
|
||||
int numSubmaps = findSeparator<GenericGraph2D>(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<GenericFactor3D>(25, j));
|
||||
for (int j=1; j<=16; j++)
|
||||
graph.push_back(boost::make_shared<GenericFactor3D>(26, j));
|
||||
for (int j=9; j<=24; j++)
|
||||
graph.push_back(boost::make_shared<GenericFactor3D>(27, j));
|
||||
for (int j=17; j<=24; j++)
|
||||
graph.push_back(boost::make_shared<GenericFactor3D>(28, j));
|
||||
|
||||
std::vector<size_t> keys;
|
||||
for(int i=1; i<=28; i++)
|
||||
keys.push_back(i);
|
||||
|
||||
vector<Symbol> 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);}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -0,0 +1,254 @@
|
|||
/*
|
||||
* testGenericGraph.cpp
|
||||
*
|
||||
* Created on: Nov 23, 2010
|
||||
* Author: nikai
|
||||
* Description: unit tests for generic graph
|
||||
*/
|
||||
|
||||
#include <map>
|
||||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
#include <boost/assign/std/set.hpp> // for operator +=
|
||||
#include <boost/assign/std/vector.hpp> // for operator +=
|
||||
using namespace boost::assign;
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#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<GenericFactor2D>(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(6, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
|
||||
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_POSE_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 6, NODE_POSE_2D));
|
||||
std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6, 7, 8, 9;
|
||||
|
||||
WorkSpace workspace(10); // from 0 to 9
|
||||
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
|
||||
LONGS_EQUAL(2, islands.size());
|
||||
vector<size_t> island1; island1 += 1, 2, 3, 7, 8;
|
||||
vector<size_t> 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<GenericFactor2D>(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(6, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
|
||||
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_POSE_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 6, NODE_POSE_2D));
|
||||
std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6, 7, 8;
|
||||
|
||||
WorkSpace workspace(15); // from 0 to 8, but testing over-allocation here
|
||||
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
|
||||
LONGS_EQUAL(1, islands.size());
|
||||
vector<size_t> 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<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 6, NODE_LANDMARK_2D));
|
||||
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 4, NODE_POSE_2D));
|
||||
std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6;
|
||||
|
||||
WorkSpace workspace(7); // from 0 to 9
|
||||
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
|
||||
LONGS_EQUAL(2, islands.size());
|
||||
vector<size_t> island1; island1 += 1, 5;
|
||||
vector<size_t> 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<GenericFactor2D>(3, NODE_POSE_2D, 4, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(7, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
|
||||
std::vector<size_t> keys; keys += 3, 4, 7;
|
||||
|
||||
WorkSpace workspace(8); // from 0 to 7
|
||||
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
|
||||
LONGS_EQUAL(2, islands.size());
|
||||
vector<size_t> island1; island1 += 3, 4;
|
||||
vector<size_t> 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<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
|
||||
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 3, NODE_POSE_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 4, NODE_POSE_2D));
|
||||
|
||||
std::vector<size_t> keys; keys += 1, 2, 3, 4, 5;
|
||||
|
||||
WorkSpace workspace(6); // from 0 to 5
|
||||
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
|
||||
LONGS_EQUAL(2, islands.size());
|
||||
vector<size_t> island1; island1 += 1, 3, 5;
|
||||
vector<size_t> 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<GenericFactor3D>(1, 3));
|
||||
graph.push_back(boost::make_shared<GenericFactor3D>(1, 4));
|
||||
graph.push_back(boost::make_shared<GenericFactor3D>(1, 5));
|
||||
graph.push_back(boost::make_shared<GenericFactor3D>(2, 5));
|
||||
graph.push_back(boost::make_shared<GenericFactor3D>(2, 6));
|
||||
|
||||
std::vector<size_t> 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<int> dictionary;
|
||||
dictionary.resize(7, -1); // from 0 to 6
|
||||
dictionary[1] = 0;
|
||||
dictionary[2] = 1;
|
||||
|
||||
GenericGraph3D reduced;
|
||||
std::map<size_t, vector<size_t> > 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<GenericFactor3D>(1, 3, 0, NODE_POSE_3D, NODE_LANDMARK_3D));
|
||||
graph.push_back(boost::make_shared<GenericFactor3D>(1, 4, 1, NODE_POSE_3D, NODE_LANDMARK_3D));
|
||||
graph.push_back(boost::make_shared<GenericFactor3D>(1, 5, 2, NODE_POSE_3D, NODE_LANDMARK_3D));
|
||||
graph.push_back(boost::make_shared<GenericFactor3D>(2, 5, 3, NODE_POSE_3D, NODE_LANDMARK_3D));
|
||||
graph.push_back(boost::make_shared<GenericFactor3D>(2, 6, 4, NODE_POSE_3D, NODE_LANDMARK_3D));
|
||||
graph.push_back(boost::make_shared<GenericFactor3D>(2, 7, 5, NODE_POSE_3D, NODE_POSE_3D));
|
||||
|
||||
std::vector<size_t> 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<int> dictionary;
|
||||
dictionary.resize(8, -1); // from 0 to 7
|
||||
dictionary[1] = 0;
|
||||
dictionary[2] = 1;
|
||||
dictionary[7] = 6;
|
||||
|
||||
GenericGraph3D reduced;
|
||||
std::map<size_t, vector<size_t> > 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<size_t> cameras1; cameras1 += 1, 2, 3, 4, 5;
|
||||
std::set<size_t> cameras2; cameras2 += 8, 7, 6, 5;
|
||||
bool actual = hasCommonCamera(cameras1, cameras2);
|
||||
CHECK(actual);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST ( GenerciGraph, hasCommonCamera2 )
|
||||
{
|
||||
std::set<size_t> cameras1; cameras1 += 1, 3, 5, 7;
|
||||
std::set<size_t> cameras2; cameras2 += 2, 4, 6, 8, 10;
|
||||
bool actual = hasCommonCamera(cameras1, cameras2);
|
||||
CHECK(!actual);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -0,0 +1,339 @@
|
|||
/*
|
||||
* testNestedDissection.cpp
|
||||
*
|
||||
* Created on: Nov 29, 2010
|
||||
* Author: nikai
|
||||
* Description: unit tests for NestedDissection
|
||||
*/
|
||||
|
||||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
#include <boost/assign/std/set.hpp> // for operator +=
|
||||
#include <boost/assign/std/vector.hpp> // for operator +=
|
||||
using namespace boost::assign;
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#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<Graph, SubNLG, GenericGraph2D> 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<Graph, SubNLG, GenericGraph2D> 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<Graph, SubNLG, GenericGraph2D> 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<Graph, SubNLG, GenericGraph2D> 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> 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<Cuts>(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<Cuts>(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<Graph, SubNLG, GenericGraph2D> 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<GeneralCamera> 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<Point3> 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<Graph, SubNLG, GenericGraph3D> 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);}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue