234 lines
8.7 KiB
C++
234 lines
8.7 KiB
C++
/*
|
|
* testFindSeparator.cpp
|
|
*
|
|
* Created on: Nov 23, 2010
|
|
* Author: nikai
|
|
* Description: unit tests for FindSeparator
|
|
*/
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
#include <gtsam_unstable/partition/FindSeparator-inl.h>
|
|
#include <gtsam_unstable/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(std::make_shared<GenericFactor2D>(0, NODE_POSE_2D, 3, NODE_LANDMARK_2D));
|
|
graph.push_back(std::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 4, NODE_LANDMARK_2D));
|
|
graph.push_back(std::make_shared<GenericFactor2D>(0, NODE_POSE_2D, 1, NODE_POSE_2D));
|
|
graph.push_back(std::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
|
|
std::vector<size_t> keys{0, 1, 2, 3, 4};
|
|
|
|
WorkSpace workspace(5);
|
|
std::optional<MetisResult> actual = separatorPartitionByMetis<GenericGraph2D>(graph, keys,
|
|
workspace, true);
|
|
|
|
CHECK(actual.has_value());
|
|
vector<size_t> A_expected{0, 3}; // frontal
|
|
vector<size_t> B_expected{2, 4}; // frontal
|
|
vector<size_t> 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(std::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
|
|
graph.push_back(std::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 6, NODE_LANDMARK_2D));
|
|
graph.push_back(std::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
|
|
graph.push_back(std::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
|
|
std::vector<size_t> keys{1, 2, 3, 5, 6};
|
|
|
|
WorkSpace workspace(8);
|
|
std::optional<MetisResult> actual = separatorPartitionByMetis<GenericGraph2D>(graph, keys,
|
|
workspace, true);
|
|
|
|
CHECK(actual.has_value());
|
|
vector<size_t> A_expected{1, 5}; // frontal
|
|
vector<size_t> B_expected{3, 6}; // frontal
|
|
vector<size_t> C_expected{2}; // separator
|
|
CHECK(A_expected == actual->A);
|
|
CHECK(B_expected == actual->B);
|
|
CHECK(C_expected == actual->C);
|
|
}
|
|
|
|
/* *************************************************************************/
|
|
// x0 - x1 - x2 - x3
|
|
TEST ( Partition, edgePartitionByMetis )
|
|
{
|
|
GenericGraph3D graph;
|
|
graph.push_back(std::make_shared<GenericFactor3D>(0, 1, 0, NODE_POSE_3D, NODE_POSE_3D));
|
|
graph.push_back(std::make_shared<GenericFactor3D>(1, 2, 1, NODE_POSE_3D, NODE_POSE_3D));
|
|
graph.push_back(std::make_shared<GenericFactor3D>(2, 3, 2, NODE_POSE_3D, NODE_POSE_3D));
|
|
std::vector<size_t> keys{0, 1, 2, 3};
|
|
|
|
WorkSpace workspace(6);
|
|
std::optional<MetisResult> actual = edgePartitionByMetis<GenericGraph3D>(graph, keys,
|
|
workspace, true);
|
|
|
|
CHECK(actual.has_value());
|
|
vector<size_t> A_expected{0, 1}; // frontal
|
|
vector<size_t> B_expected{2, 3}; // frontal
|
|
vector<size_t> C_expected; // separator
|
|
// for(const size_t a: actual->A)
|
|
// cout << a << " ";
|
|
// cout << endl;
|
|
// for(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 - x1 - x2 - x3 - x4
|
|
TEST ( Partition, edgePartitionByMetis2 )
|
|
{
|
|
GenericGraph3D graph;
|
|
graph.push_back(std::make_shared<GenericFactor3D>(0, 1, 0, NODE_POSE_3D, NODE_POSE_3D, 1));
|
|
graph.push_back(std::make_shared<GenericFactor3D>(1, 2, 1, NODE_POSE_3D, NODE_POSE_3D, 1));
|
|
graph.push_back(std::make_shared<GenericFactor3D>(2, 3, 2, NODE_POSE_3D, NODE_POSE_3D, 20));
|
|
graph.push_back(std::make_shared<GenericFactor3D>(3, 4, 3, NODE_POSE_3D, NODE_POSE_3D, 1));
|
|
//QNX Testing: fix tiebreaker to match
|
|
#if !defined(__QNX__)
|
|
std::vector<size_t> keys{0, 1, 2, 3, 4};
|
|
#else
|
|
//Anything where 2 is before 0 will work.
|
|
std::vector<size_t> keys{2, 0, 3, 1, 4};
|
|
#endif
|
|
|
|
WorkSpace workspace(6);
|
|
std::optional<MetisResult> actual = edgePartitionByMetis<GenericGraph3D>(graph, keys,
|
|
workspace, true);
|
|
CHECK(actual.has_value());
|
|
vector<size_t> A_expected{0, 1}; // frontal
|
|
vector<size_t> B_expected{2, 3, 4}; // 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(std::make_shared<GenericFactor2D>(0, NODE_POSE_2D, 3, NODE_LANDMARK_2D));
|
|
graph.push_back(std::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 4, NODE_LANDMARK_2D));
|
|
graph.push_back(std::make_shared<GenericFactor2D>(0, NODE_POSE_2D, 1, NODE_POSE_2D));
|
|
graph.push_back(std::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
|
|
std::vector<size_t> keys{0, 1, 2, 3, 4};
|
|
|
|
WorkSpace workspace(5);
|
|
int minNodesPerMap = -1;
|
|
bool reduceGraph = false;
|
|
int numSubmaps = findSeparator<GenericGraph2D>(graph, keys, minNodesPerMap, workspace,
|
|
false, {}, reduceGraph, 0, 0);
|
|
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(std::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
|
|
graph.push_back(std::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 6, NODE_LANDMARK_2D));
|
|
graph.push_back(std::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
|
|
graph.push_back(std::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
|
|
std::vector<size_t> keys{1, 2, 3, 5, 6};
|
|
|
|
WorkSpace workspace(8);
|
|
int minNodesPerMap = -1;
|
|
bool reduceGraph = false;
|
|
int numSubmaps = findSeparator<GenericGraph2D>(graph, keys, minNodesPerMap, workspace,
|
|
false, {}, reduceGraph, 0, 0);
|
|
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(std::make_shared<GenericFactor3D>(25, j));
|
|
for (int j=1; j<=16; j++)
|
|
graph.push_back(std::make_shared<GenericFactor3D>(26, j));
|
|
for (int j=9; j<=24; j++)
|
|
graph.push_back(std::make_shared<GenericFactor3D>(27, j));
|
|
for (int j=17; j<=24; j++)
|
|
graph.push_back(std::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, 0, 0);
|
|
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);}
|
|
/* ************************************************************************* */
|