Parition builds partially. Computing edge separate is no longer a separate function in METIS. testFindSep fails
Andrew Melim
parent
f48b8e593c
commit
1ff9309533
|
|
@ -6,6 +6,7 @@ set (gtsam_unstable_subdirs
|
|||
discrete
|
||||
dynamics
|
||||
nonlinear
|
||||
partition
|
||||
slam
|
||||
)
|
||||
|
||||
|
|
@ -46,6 +47,7 @@ set(gtsam_unstable_srcs
|
|||
${discrete_srcs}
|
||||
${dynamics_srcs}
|
||||
${nonlinear_srcs}
|
||||
${partition_srcs}
|
||||
${slam_srcs}
|
||||
)
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,7 @@
|
|||
# Install headers
|
||||
file(GLOB partition_headers "*.h")
|
||||
install(FILES ${partition_headers} DESTINATION include/gtsam_unstable/parition)
|
||||
|
||||
set(ignore_test "tests/testNestedDissection.cpp")
|
||||
# Add all tests
|
||||
gtsamAddTestsGlob(partition_unstable "tests/*.cpp" "${ignore_test}" "gtsam_unstable;metis")
|
||||
|
|
@ -2,7 +2,9 @@
|
|||
* FindSeparator-inl.h
|
||||
*
|
||||
* Created on: Nov 23, 2010
|
||||
* Updated: Feb 20. 2014
|
||||
* Author: nikai
|
||||
* Author: Andrew Melim
|
||||
* Description: find the separator of bisectioning for a given graph
|
||||
*/
|
||||
|
||||
|
|
@ -13,6 +15,7 @@
|
|||
#include <boost/tuple/tuple.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/shared_array.hpp>
|
||||
#include <boost/timer.hpp>
|
||||
|
||||
extern "C" {
|
||||
#include <metis.h>
|
||||
|
|
@ -24,7 +27,7 @@ using namespace std;
|
|||
|
||||
namespace gtsam { namespace partition {
|
||||
|
||||
typedef boost::shared_array<idxtype> sharedInts;
|
||||
typedef boost::shared_array<idx_t> sharedInts;
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
|
|
@ -36,7 +39,7 @@ namespace gtsam { namespace partition {
|
|||
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
|
||||
idx_t 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
|
||||
|
|
@ -44,34 +47,37 @@ namespace gtsam { namespace partition {
|
|||
// set uniform weights on the vertices
|
||||
std::fill(vwgt, vwgt+n, 1);
|
||||
|
||||
timer TOTALTmr;
|
||||
// TODO: Fix at later time
|
||||
/*boost::timer::cpu_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);
|
||||
}
|
||||
TOTALTmr.start()
|
||||
}*/
|
||||
|
||||
// call metis parition routine
|
||||
METIS_NodeComputeSeparator(&n, xadj.get(), adjncy.get(), vwgt, adjwgt.get(),
|
||||
options, &sepsize, part_.get());
|
||||
// call metis parition routine OLD!!!!
|
||||
/*METIS_NodeComputeSeparator(&n, xadj.get(), adjncy.get(), vwgt, adjwgt.get(),
|
||||
options, &sepsize, part_.get());*/
|
||||
// new
|
||||
METIS_ComputeVertexSeparator(&n, xadj.get(), adjncy.get(),
|
||||
vwgt, options, &sepsize, part_.get());
|
||||
|
||||
if (verbose) {
|
||||
stoptimer(TOTALTmr);
|
||||
/*if (verbose) {
|
||||
boost::cpu_times const elapsed_times(timer.elapsed());
|
||||
printf("\nTiming Information --------------------------------------------------\n");
|
||||
printf(" Total: \t\t %7.3f\n", gettimer(TOTALTmr));
|
||||
printf(" Total: \t\t %7.3f\n", elapsed_times);
|
||||
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)
|
||||
/* ************************************************************************* *
|
||||
void modefied_EdgeComputeSeparator(int *nvtxs, idx_t *xadj, idx_t *adjncy, idx_t *vwgt,
|
||||
idx_t *adjwgt, int *options, int *edgecut, idx_t *part)
|
||||
{
|
||||
int i, j, tvwgt, tpwgts[2];
|
||||
GraphType graph;
|
||||
|
|
@ -80,7 +86,7 @@ namespace gtsam { namespace partition {
|
|||
SetUpGraph(&graph, OP_ONMETIS, *nvtxs, 1, xadj, adjncy, vwgt, adjwgt, 3);
|
||||
tvwgt = idxsum(*nvtxs, graph.vwgt);
|
||||
|
||||
if (options[0] == 0) { /* Use the default parameters */
|
||||
if (options[0] == 0) { // Use the default parameters
|
||||
ctrl.CType = ONMETIS_CTYPE;
|
||||
ctrl.IType = ONMETIS_ITYPE;
|
||||
ctrl.RType = ONMETIS_RTYPE;
|
||||
|
|
@ -104,9 +110,9 @@ namespace gtsam { namespace partition {
|
|||
|
||||
AllocateWorkSpace(&ctrl, &graph, 2);
|
||||
|
||||
/*============================================================
|
||||
* Perform the bisection
|
||||
*============================================================*/
|
||||
//============================================================
|
||||
// Perform the bisection
|
||||
//============================================================
|
||||
tpwgts[0] = tvwgt/2;
|
||||
tpwgts[1] = tvwgt-tpwgts[0];
|
||||
|
||||
|
|
@ -128,11 +134,11 @@ namespace gtsam { namespace partition {
|
|||
* 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
|
||||
idx_t 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
|
||||
|
|
@ -140,7 +146,8 @@ namespace gtsam { namespace partition {
|
|||
// set uniform weights on the vertices
|
||||
std::fill(vwgt, vwgt+n, 1);
|
||||
|
||||
timer TOTALTmr;
|
||||
TODO: Fix later
|
||||
boost::timer TOTALTmr;
|
||||
if (verbose) {
|
||||
printf("**********************************************************************\n");
|
||||
printf("Graph Information ---------------------------------------------------\n");
|
||||
|
|
@ -160,6 +167,7 @@ namespace gtsam { namespace partition {
|
|||
modefied_EdgeComputeSeparator(&n, xadj.get(), adjncy.get(), vwgt, adjwgt.get(),
|
||||
options, &edgecut, part_.get());
|
||||
|
||||
|
||||
if (verbose) {
|
||||
stoptimer(TOTALTmr);
|
||||
printf("\nTiming Information --------------------------------------------------\n");
|
||||
|
|
@ -279,7 +287,7 @@ namespace gtsam { namespace partition {
|
|||
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) {
|
||||
|
||||
|
|
@ -456,7 +464,7 @@ namespace gtsam { namespace partition {
|
|||
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);
|
||||
//result = edgePartitionByMetis(reducedGraph, keyToPartition, workspace, verbose);
|
||||
} else // call Metis to partition the graph to A, B, C
|
||||
result = separatorPartitionByMetis(graph, keys, workspace, verbose);
|
||||
|
||||
|
|
@ -521,10 +529,12 @@ namespace gtsam { namespace partition {
|
|||
result->C.insert(result->C.end(), island.begin(), island.end());
|
||||
islands.pop_back();
|
||||
}
|
||||
if (islands.size() != oldSize)
|
||||
if (islands.size() != oldSize){
|
||||
if (verbose) cout << oldSize << "-" << oldSize - islands.size() << " submap(s);\t" << endl;
|
||||
else
|
||||
}
|
||||
else{
|
||||
if (verbose) cout << oldSize << " submap(s);\t" << endl;
|
||||
}
|
||||
|
||||
// generate the node map
|
||||
std::vector<int>& partitionTable = workspace.partitionTable;
|
||||
|
|
|
|||
|
|
@ -9,7 +9,8 @@
|
|||
#include <map>
|
||||
#include <vector>
|
||||
#include <boost/optional.hpp>
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
|
||||
#include "PartitionWorkSpace.h"
|
||||
|
||||
|
|
|
|||
|
|
@ -1,222 +0,0 @@
|
|||
/*
|
||||
* 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);}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -1,254 +0,0 @@
|
|||
/*
|
||||
* 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);}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -1,339 +0,0 @@
|
|||
/*
|
||||
* 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);}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -14,8 +14,8 @@ using namespace boost::assign;
|
|||
#include <boost/make_shared.hpp>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include "partition/FindSeparator-inl.h"
|
||||
#include "partition/GenericGraph.h"
|
||||
#include <gtsam_unstable/partition/FindSeparator-inl.h>
|
||||
#include <gtsam_unstable/partition/GenericGraph.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -66,7 +66,7 @@ TEST ( Partition, separatorPartitionByMetis2 )
|
|||
CHECK(C_expected == actual->C);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/* *************************************************************************
|
||||
// x0 - x2 - x3 - x5
|
||||
TEST ( Partition, edgePartitionByMetis )
|
||||
{
|
||||
|
|
@ -94,7 +94,7 @@ TEST ( Partition, edgePartitionByMetis )
|
|||
CHECK(C_expected == actual->C);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
// x0 - x2 - x3 - x5 - x6
|
||||
TEST ( Partition, edgePartitionByMetis2 )
|
||||
{
|
||||
|
|
@ -114,8 +114,7 @@ TEST ( Partition, edgePartitionByMetis2 )
|
|||
CHECK(A_expected == actual->A);
|
||||
CHECK(B_expected == actual->B);
|
||||
CHECK(C_expected == actual->C);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// x0 - x1 - x2
|
||||
|
|
@ -132,7 +131,8 @@ TEST ( Partition, findSeparator )
|
|||
WorkSpace workspace(5);
|
||||
int minNodesPerMap = -1;
|
||||
bool reduceGraph = false;
|
||||
int numSubmaps = findSeparator<GenericGraph2D>(graph, keys, minNodesPerMap, workspace, false, boost::none, reduceGraph);
|
||||
int numSubmaps = findSeparator<GenericGraph2D>(graph, keys, minNodesPerMap, workspace,
|
||||
false, boost::none, reduceGraph, 0, 0);
|
||||
LONGS_EQUAL(2, numSubmaps);
|
||||
LONGS_EQUAL(5, workspace.partitionTable.size());
|
||||
LONGS_EQUAL(1, workspace.partitionTable[0]);
|
||||
|
|
@ -157,7 +157,8 @@ TEST ( Partition, findSeparator2 )
|
|||
WorkSpace workspace(8);
|
||||
int minNodesPerMap = -1;
|
||||
bool reduceGraph = false;
|
||||
int numSubmaps = findSeparator<GenericGraph2D>(graph, keys, minNodesPerMap, workspace, false, boost::none, reduceGraph);
|
||||
int numSubmaps = findSeparator<GenericGraph2D>(graph, keys, minNodesPerMap, workspace,
|
||||
false, boost::none, reduceGraph, 0, 0);
|
||||
LONGS_EQUAL(2, numSubmaps);
|
||||
LONGS_EQUAL(8, workspace.partitionTable.size());
|
||||
LONGS_EQUAL(-1,workspace.partitionTable[0]);
|
||||
|
|
@ -170,7 +171,7 @@ TEST ( Partition, findSeparator2 )
|
|||
LONGS_EQUAL(-1,workspace.partitionTable[7]);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/* *************************************************************************
|
||||
// l1-l8 l9-l16 l17-l24
|
||||
// / | / \ | \
|
||||
// x25 x26 x27 x28
|
||||
|
|
@ -201,7 +202,7 @@ TEST ( Partition, findSeparator3_with_reduced_camera )
|
|||
|
||||
WorkSpace workspace(29);
|
||||
bool reduceGraph = true;
|
||||
int numIsland = findSeparator(graph, keys, 3, workspace, false, int2symbol, reduceGraph);
|
||||
int numIsland = findSeparator(graph, keys, 3, workspace, false, int2symbol, reduceGraph, 0, 0);
|
||||
LONGS_EQUAL(2, numIsland);
|
||||
|
||||
partition::PartitionTable& partitionTable = workspace.partitionTable;
|
||||
|
|
|
|||
|
|
@ -15,7 +15,7 @@ using namespace boost::assign;
|
|||
#include <boost/make_shared.hpp>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include "partition/GenericGraph.h"
|
||||
#include <gtsam_unstable/partition/GenericGraph.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
|
|||
Loading…
Reference in New Issue