From f48b8e593cad1f0e17be161b4ce4e3a6f1b65204 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Thu, 20 Feb 2014 12:27:58 -0500 Subject: [PATCH] Move tests to folder --- .../partition/tests/testFindSeparator.cpp | 222 ++++++++++++ .../partition/tests/testGenericGraph.cpp | 254 +++++++++++++ .../partition/tests/testNestedDissection.cpp | 339 ++++++++++++++++++ 3 files changed, 815 insertions(+) create mode 100644 gtsam_unstable/partition/tests/testFindSeparator.cpp create mode 100644 gtsam_unstable/partition/tests/testGenericGraph.cpp create mode 100644 gtsam_unstable/partition/tests/testNestedDissection.cpp diff --git a/gtsam_unstable/partition/tests/testFindSeparator.cpp b/gtsam_unstable/partition/tests/testFindSeparator.cpp new file mode 100644 index 000000000..8c3306458 --- /dev/null +++ b/gtsam_unstable/partition/tests/testFindSeparator.cpp @@ -0,0 +1,222 @@ +/* + * testFindSeparator.cpp + * + * Created on: Nov 23, 2010 + * Author: nikai + * Description: unit tests for FindSeparator + */ + +#include // for operator += +#include // for operator += +#include // for operator += +using namespace boost::assign; +#include +#include +#include + +#include "partition/FindSeparator-inl.h" +#include "partition/GenericGraph.h" + +using namespace std; +using namespace gtsam; +using namespace gtsam::partition; + +/* ************************************************************************* */ +// x0 - x1 - x2 +// l3 l4 +TEST ( Partition, separatorPartitionByMetis ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(0, NODE_POSE_2D, 3, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(0, NODE_POSE_2D, 1, NODE_POSE_2D)); + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + std::vector keys; keys += 0, 1, 2, 3, 4; + + WorkSpace workspace(5); + boost::optional actual = separatorPartitionByMetis(graph, keys, workspace, false); + CHECK(actual.is_initialized()); + vector A_expected; A_expected += 0, 3; // frontal + vector B_expected; B_expected += 2, 4; // frontal + vector C_expected; C_expected += 1; // separator + CHECK(A_expected == actual->A); + CHECK(B_expected == actual->B); + CHECK(C_expected == actual->C); +} + +/* ************************************************************************* */ +// x1 - x2 - x3, variable not used x0, x4, l7 +// l5 l6 +TEST ( Partition, separatorPartitionByMetis2 ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 6, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + std::vector keys; keys += 1, 2, 3, 5, 6; + WorkSpace workspace(8); + boost::optional actual = separatorPartitionByMetis(graph, keys, workspace, false); + CHECK(actual.is_initialized()); + vector A_expected; A_expected += 1, 5; // frontal + vector B_expected; B_expected += 3, 6; // frontal + vector C_expected; C_expected += 2; // separator + CHECK(A_expected == actual->A); + CHECK(B_expected == actual->B); + CHECK(C_expected == actual->C); +} + +/* ************************************************************************* */ +// x0 - x2 - x3 - x5 +TEST ( Partition, edgePartitionByMetis ) +{ + GenericGraph3D graph; + graph.push_back(boost::make_shared(0, 2, 0, NODE_POSE_3D, NODE_POSE_3D)); + graph.push_back(boost::make_shared(2, 3, 1, NODE_POSE_3D, NODE_POSE_3D)); + graph.push_back(boost::make_shared(3, 5, 2, NODE_POSE_3D, NODE_POSE_3D)); + std::vector keys; keys += 0, 2, 3, 5; + + WorkSpace workspace(6); + boost::optional actual = edgePartitionByMetis(graph, keys, workspace, false); + CHECK(actual.is_initialized()); + vector A_expected; A_expected += 0, 2; // frontal + vector B_expected; B_expected += 3, 5; // frontal + vector C_expected; // separator +// BOOST_FOREACH(const size_t a, actual->A) +// cout << a << " "; +// cout << endl; +// BOOST_FOREACH(const size_t b, actual->B) +// cout << b << " "; +// cout << endl; + + CHECK(A_expected == actual->A || A_expected == actual->B); + CHECK(B_expected == actual->B || B_expected == actual->A); + CHECK(C_expected == actual->C); +} + +/* ************************************************************************* */ +// x0 - x2 - x3 - x5 - x6 +TEST ( Partition, edgePartitionByMetis2 ) +{ + GenericGraph3D graph; + graph.push_back(boost::make_shared(0, 2, 0, NODE_POSE_3D, NODE_POSE_3D, 1)); + graph.push_back(boost::make_shared(2, 3, 1, NODE_POSE_3D, NODE_POSE_3D, 1)); + graph.push_back(boost::make_shared(3, 5, 2, NODE_POSE_3D, NODE_POSE_3D, 20)); + graph.push_back(boost::make_shared(5, 6, 3, NODE_POSE_3D, NODE_POSE_3D, 1)); + std::vector keys; keys += 0, 2, 3, 5, 6; + + WorkSpace workspace(6); + boost::optional actual = edgePartitionByMetis(graph, keys, workspace, false); + CHECK(actual.is_initialized()); + vector A_expected; A_expected += 0, 2; // frontal + vector B_expected; B_expected += 3, 5, 6; // frontal + vector C_expected; // separator + CHECK(A_expected == actual->A); + CHECK(B_expected == actual->B); + CHECK(C_expected == actual->C); +} + + +/* ************************************************************************* */ +// x0 - x1 - x2 +// l3 l4 +TEST ( Partition, findSeparator ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(0, NODE_POSE_2D, 3, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(0, NODE_POSE_2D, 1, NODE_POSE_2D)); + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + std::vector keys; keys += 0, 1, 2, 3, 4; + + WorkSpace workspace(5); + int minNodesPerMap = -1; + bool reduceGraph = false; + int numSubmaps = findSeparator(graph, keys, minNodesPerMap, workspace, false, boost::none, reduceGraph); + LONGS_EQUAL(2, numSubmaps); + LONGS_EQUAL(5, workspace.partitionTable.size()); + LONGS_EQUAL(1, workspace.partitionTable[0]); + LONGS_EQUAL(0, workspace.partitionTable[1]); + LONGS_EQUAL(2, workspace.partitionTable[2]); + LONGS_EQUAL(1, workspace.partitionTable[3]); + LONGS_EQUAL(2, workspace.partitionTable[4]); +} + +/* ************************************************************************* */ +// x1 - x2 - x3, variable not used x0, x4, l7 +// l5 l6 +TEST ( Partition, findSeparator2 ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 6, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + std::vector keys; keys += 1, 2, 3, 5, 6; + + WorkSpace workspace(8); + int minNodesPerMap = -1; + bool reduceGraph = false; + int numSubmaps = findSeparator(graph, keys, minNodesPerMap, workspace, false, boost::none, reduceGraph); + LONGS_EQUAL(2, numSubmaps); + LONGS_EQUAL(8, workspace.partitionTable.size()); + LONGS_EQUAL(-1,workspace.partitionTable[0]); + LONGS_EQUAL(1, workspace.partitionTable[1]); + LONGS_EQUAL(0, workspace.partitionTable[2]); + LONGS_EQUAL(2, workspace.partitionTable[3]); + LONGS_EQUAL(-1,workspace.partitionTable[4]); + LONGS_EQUAL(1, workspace.partitionTable[5]); + LONGS_EQUAL(2, workspace.partitionTable[6]); + LONGS_EQUAL(-1,workspace.partitionTable[7]); +} + +/* ************************************************************************* */ +// l1-l8 l9-l16 l17-l24 +// / | / \ | \ +// x25 x26 x27 x28 +TEST ( Partition, findSeparator3_with_reduced_camera ) +{ + GenericGraph3D graph; + for (int j=1; j<=8; j++) + graph.push_back(boost::make_shared(25, j)); + for (int j=1; j<=16; j++) + graph.push_back(boost::make_shared(26, j)); + for (int j=9; j<=24; j++) + graph.push_back(boost::make_shared(27, j)); + for (int j=17; j<=24; j++) + graph.push_back(boost::make_shared(28, j)); + + std::vector keys; + for(int i=1; i<=28; i++) + keys.push_back(i); + + vector int2symbol; + int2symbol.push_back(Symbol('x',0)); // dummy + for(int i=1; i<=24; i++) + int2symbol.push_back(Symbol('l',i)); + int2symbol.push_back(Symbol('x',25)); + int2symbol.push_back(Symbol('x',26)); + int2symbol.push_back(Symbol('x',27)); + int2symbol.push_back(Symbol('x',28)); + + WorkSpace workspace(29); + bool reduceGraph = true; + int numIsland = findSeparator(graph, keys, 3, workspace, false, int2symbol, reduceGraph); + LONGS_EQUAL(2, numIsland); + + partition::PartitionTable& partitionTable = workspace.partitionTable; + for (int j=1; j<=8; j++) + LONGS_EQUAL(1, partitionTable[j]); + for (int j=9; j<=16; j++) + LONGS_EQUAL(0, partitionTable[j]); + for (int j=17; j<=24; j++) + LONGS_EQUAL(2, partitionTable[j]); + LONGS_EQUAL(1, partitionTable[25]); + LONGS_EQUAL(1, partitionTable[26]); + LONGS_EQUAL(2, partitionTable[27]); + LONGS_EQUAL(2, partitionTable[28]); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/partition/tests/testGenericGraph.cpp b/gtsam_unstable/partition/tests/testGenericGraph.cpp new file mode 100644 index 000000000..e8e9988bc --- /dev/null +++ b/gtsam_unstable/partition/tests/testGenericGraph.cpp @@ -0,0 +1,254 @@ +/* + * testGenericGraph.cpp + * + * Created on: Nov 23, 2010 + * Author: nikai + * Description: unit tests for generic graph + */ + +#include +#include // for operator += +#include // for operator += +#include // for operator += +using namespace boost::assign; +#include +#include +#include + +#include "partition/GenericGraph.h" + +using namespace std; +using namespace gtsam; +using namespace gtsam::partition; + +/* ************************************************************************* */ +/** + * l7 l9 + * / | \ / | + * x1 -x2-x3 - l8 - x4- x5-x6 + */ +TEST ( GenerciGraph, findIslands ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(5, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(6, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); + + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_POSE_2D)); + graph.push_back(boost::make_shared(5, NODE_POSE_2D, 6, NODE_POSE_2D)); + std::vector keys; keys += 1, 2, 3, 4, 5, 6, 7, 8, 9; + + WorkSpace workspace(10); // from 0 to 9 + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(2, islands.size()); + vector island1; island1 += 1, 2, 3, 7, 8; + vector island2; island2 += 4, 5, 6, 9; + CHECK(island1 == islands.front()); + CHECK(island2 == islands.back()); +} + +/* ************************************************************************* */ +/** + * l7 l8 + * / / | X | \ + * x1 -x2-x3 x4- x5-x6 + */ +TEST( GenerciGraph, findIslands2 ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(5, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(6, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_POSE_2D)); + graph.push_back(boost::make_shared(5, NODE_POSE_2D, 6, NODE_POSE_2D)); + std::vector keys; keys += 1, 2, 3, 4, 5, 6, 7, 8; + + WorkSpace workspace(15); // from 0 to 8, but testing over-allocation here + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(1, islands.size()); + vector island1; island1 += 1, 2, 3, 4, 5, 6, 7, 8; + CHECK(island1 == islands.front()); +} + +/* ************************************************************************* */ +/** + * x1 - l5 + * x2 - x3 - x4 - l6 + */ +TEST ( GenerciGraph, findIslands3 ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 6, NODE_LANDMARK_2D)); + + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 4, NODE_POSE_2D)); + std::vector keys; keys += 1, 2, 3, 4, 5, 6; + + WorkSpace workspace(7); // from 0 to 9 + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(2, islands.size()); + vector island1; island1 += 1, 5; + vector island2; island2 += 2, 3, 4, 6; + CHECK(island1 == islands.front()); + CHECK(island2 == islands.back()); +} + +/* ************************************************************************* */ +/** + * x3 - l4 - x7 + */ +TEST ( GenerciGraph, findIslands4 ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(7, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + std::vector keys; keys += 3, 4, 7; + + WorkSpace workspace(8); // from 0 to 7 + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(2, islands.size()); + vector island1; island1 += 3, 4; + vector island2; island2 += 7; + CHECK(island1 == islands.front()); + CHECK(island2 == islands.back()); +} + +/* ************************************************************************* */ +/** + * x1 - l5 - x2 + * | / \ | + * x3 x4 + */ +TEST ( GenerciGraph, findIslands5 ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 4, NODE_POSE_2D)); + + std::vector keys; keys += 1, 2, 3, 4, 5; + + WorkSpace workspace(6); // from 0 to 5 + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(2, islands.size()); + vector island1; island1 += 1, 3, 5; + vector island2; island2 += 2, 4; + CHECK(island1 == islands.front()); + CHECK(island2 == islands.back()); +} + +/* ************************************************************************* */ +/** + * l3 l4 l5 l6 + * \ | / \ / + * x1 x2 + */ +TEST ( GenerciGraph, reduceGenericGraph ) +{ + GenericGraph3D graph; + graph.push_back(boost::make_shared(1, 3)); + graph.push_back(boost::make_shared(1, 4)); + graph.push_back(boost::make_shared(1, 5)); + graph.push_back(boost::make_shared(2, 5)); + graph.push_back(boost::make_shared(2, 6)); + + std::vector cameraKeys, landmarkKeys; + cameraKeys.push_back(1); + cameraKeys.push_back(2); + landmarkKeys.push_back(3); + landmarkKeys.push_back(4); + landmarkKeys.push_back(5); + landmarkKeys.push_back(6); + + std::vector dictionary; + dictionary.resize(7, -1); // from 0 to 6 + dictionary[1] = 0; + dictionary[2] = 1; + + GenericGraph3D reduced; + std::map > cameraToLandmarks; + reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced); + LONGS_EQUAL(1, reduced.size()); + LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index); +} + +/* ************************************************************************* */ +/** + * l3 l4 l5 l6 + * \ | / \ / + * x1 x2 - x7 + */ +TEST ( GenericGraph, reduceGenericGraph2 ) +{ + GenericGraph3D graph; + graph.push_back(boost::make_shared(1, 3, 0, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(1, 4, 1, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(1, 5, 2, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(2, 5, 3, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(2, 6, 4, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(2, 7, 5, NODE_POSE_3D, NODE_POSE_3D)); + + std::vector cameraKeys, landmarkKeys; + cameraKeys.push_back(1); + cameraKeys.push_back(2); + cameraKeys.push_back(7); + landmarkKeys.push_back(3); + landmarkKeys.push_back(4); + landmarkKeys.push_back(5); + landmarkKeys.push_back(6); + + std::vector dictionary; + dictionary.resize(8, -1); // from 0 to 7 + dictionary[1] = 0; + dictionary[2] = 1; + dictionary[7] = 6; + + GenericGraph3D reduced; + std::map > cameraToLandmarks; + reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced); + LONGS_EQUAL(2, reduced.size()); + LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index); + LONGS_EQUAL(2, reduced[1]->key1.index); LONGS_EQUAL(7, reduced[1]->key2.index); +} + +/* ************************************************************************* */ +TEST ( GenerciGraph, hasCommonCamera ) +{ + std::set cameras1; cameras1 += 1, 2, 3, 4, 5; + std::set cameras2; cameras2 += 8, 7, 6, 5; + bool actual = hasCommonCamera(cameras1, cameras2); + CHECK(actual); +} + +/* ************************************************************************* */ +TEST ( GenerciGraph, hasCommonCamera2 ) +{ + std::set cameras1; cameras1 += 1, 3, 5, 7; + std::set cameras2; cameras2 += 2, 4, 6, 8, 10; + bool actual = hasCommonCamera(cameras1, cameras2); + CHECK(!actual); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/partition/tests/testNestedDissection.cpp b/gtsam_unstable/partition/tests/testNestedDissection.cpp new file mode 100644 index 000000000..906178914 --- /dev/null +++ b/gtsam_unstable/partition/tests/testNestedDissection.cpp @@ -0,0 +1,339 @@ +/* + * testNestedDissection.cpp + * + * Created on: Nov 29, 2010 + * Author: nikai + * Description: unit tests for NestedDissection + */ + +#include // for operator += +#include // for operator += +#include // for operator += +using namespace boost::assign; +#include +#include +#include + +#include "SubmapPlanarSLAM.h" +#include "SubmapVisualSLAM.h" +#include "SubmapExamples.h" +#include "SubmapExamples3D.h" +#include "GenericGraph.h" +#include "NonlinearTSAM.h" +#include "partition/NestedDissection-inl.h" + +using namespace std; +using namespace gtsam; +using namespace gtsam::partition; + +/* ************************************************************************* */ +// x1 - x2 +// \ / +// l1 +TEST ( NestedDissection, oneIsland ) +{ + using namespace submapPlanarSLAM; + typedef TSAM2D::SubNLG SubNLG; + Graph fg; + fg.addOdometry(1, 2, Pose2(), odoNoise); + fg.addBearingRange(1, 1, Rot2(), 0., bearingRangeNoise); + fg.addBearingRange(2, 1, Rot2(), 0., bearingRangeNoise); + fg.addPoseConstraint(1, Pose2()); + + Ordering ordering; ordering += x1, x2, l1; + + int numNodeStopPartition = 1e3; + int minNodesPerMap = 1e3; + NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); + LONGS_EQUAL(4, nd.root()->size()); + LONGS_EQUAL(3, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->children().size()); +} + +/* ************************************************************************* */ +// x1\ /x4 +// | x3 | +// x2/ \x5 +TEST ( NestedDissection, TwoIslands ) +{ + using namespace submapPlanarSLAM; + typedef TSAM2D::SubNLG SubNLG; + Graph fg; + fg.addOdometry(1, 2, Pose2(), odoNoise); + fg.addOdometry(1, 3, Pose2(), odoNoise); + fg.addOdometry(2, 3, Pose2(), odoNoise); + fg.addOdometry(3, 4, Pose2(), odoNoise); + fg.addOdometry(4, 5, Pose2(), odoNoise); + fg.addOdometry(3, 5, Pose2(), odoNoise); + fg.addPoseConstraint(1, Pose2()); + fg.addPoseConstraint(4, Pose2()); + Ordering ordering; ordering += x1, x2, x3, x4, x5; + + int numNodeStopPartition = 2; + int minNodesPerMap = 1; + NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); + // root submap + LONGS_EQUAL(0, nd.root()->size()); + LONGS_EQUAL(1, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps + + // the 1st submap + LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); + LONGS_EQUAL(4, nd.root()->children()[0]->size()); + + // the 2nd submap + LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); + LONGS_EQUAL(4, nd.root()->children()[1]->size()); +} + +/* ************************************************************************* */ +// x1\ /x4 +// x3 +// x2/ \x5 +TEST ( NestedDissection, FourIslands ) +{ + using namespace submapPlanarSLAM; + typedef TSAM2D::SubNLG SubNLG; + Graph fg; + fg.addOdometry(1, 3, Pose2(), odoNoise); + fg.addOdometry(2, 3, Pose2(), odoNoise); + fg.addOdometry(3, 4, Pose2(), odoNoise); + fg.addOdometry(3, 5, Pose2(), odoNoise); + fg.addPoseConstraint(1, Pose2()); + fg.addPoseConstraint(4, Pose2()); + Ordering ordering; ordering += x1, x2, x3, x4, x5; + + int numNodeStopPartition = 2; + int minNodesPerMap = 1; + NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); + LONGS_EQUAL(0, nd.root()->size()); + LONGS_EQUAL(1, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(4, nd.root()->children().size()); // 4 leaf submaps + + // the 1st submap + LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); + LONGS_EQUAL(2, nd.root()->children()[0]->size()); + + // the 2nd submap + LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); + LONGS_EQUAL(2, nd.root()->children()[1]->size()); + + // the 3rd submap + LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); + LONGS_EQUAL(1, nd.root()->children()[2]->size()); + + // the 4th submap + LONGS_EQUAL(1, nd.root()->children()[3]->frontal().size()); + LONGS_EQUAL(1, nd.root()->children()[3]->size()); +} + +/* ************************************************************************* */ +// x1\ /x3 +// | x2 | +// l6/ \x4 +// | +// x5 +TEST ( NestedDissection, weekLinks ) +{ + using namespace submapPlanarSLAM; + typedef TSAM2D::SubNLG SubNLG; + Graph fg; + fg.addOdometry(1, 2, Pose2(), odoNoise); + fg.addOdometry(2, 3, Pose2(), odoNoise); + fg.addOdometry(2, 4, Pose2(), odoNoise); + fg.addOdometry(3, 4, Pose2(), odoNoise); + fg.addBearingRange(1, 6, Rot2(), 0., bearingRangeNoise); + fg.addBearingRange(2, 6, Rot2(), 0., bearingRangeNoise); + fg.addBearingRange(5, 6, Rot2(), 0., bearingRangeNoise); + fg.addPoseConstraint(1, Pose2()); + fg.addPoseConstraint(4, Pose2()); + fg.addPoseConstraint(5, Pose2()); + Ordering ordering; ordering += x1, x2, x3, x4, x5, l6; + + int numNodeStopPartition = 2; + int minNodesPerMap = 1; + NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); + LONGS_EQUAL(0, nd.root()->size()); // one weeklink + LONGS_EQUAL(1, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(3, nd.root()->children().size()); // 4 leaf submaps + LONGS_EQUAL(1, nd.root()->weeklinks().size()); + + // the 1st submap + LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); // x3 and x4 + LONGS_EQUAL(4, nd.root()->children()[0]->size()); + + // the 2nd submap + LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); // x1 and l6 + LONGS_EQUAL(4, nd.root()->children()[1]->size()); + // + // the 3rd submap + LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); // x5 + LONGS_EQUAL(1, nd.root()->children()[2]->size()); +} + +/* ************************************************************************* */ +/** + * l1 l2 l3 + * | X | X | + * x0 - x1 - x2 + * | X | X | + * l4 l5 l6 + */ +TEST ( NestedDissection, manual_cuts ) +{ + using namespace submapPlanarSLAM; + typedef partition::Cuts Cuts; + typedef TSAM2D::SubNLG SubNLG; + typedef partition::PartitionTable PartitionTable; + Graph fg; + fg.addOdometry(x0, x1, Pose2(1.0, 0, 0), odoNoise); + fg.addOdometry(x1, x2, Pose2(1.0, 0, 0), odoNoise); + + fg.addBearingRange(x0, l1, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x0, l4, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x0, l2, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x0, l5, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise); + + fg.addBearingRange(x1, l1, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x1, l2, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x1, l3, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x1, l4, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x1, l5, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x1, l6, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise); + + fg.addBearingRange(x2, l2, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x2, l5, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x2, l3, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x2, l6, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise); + + fg.addPrior(x0, Pose2(0.1, 0, 0), priorNoise); + + // generate ordering + Ordering ordering; ordering += x0, x1, x2, l1, l2, l3, l4, l5, l6; + + // define cuts + boost::shared_ptr cuts(new Cuts()); + cuts->partitionTable = PartitionTable(9, -1); PartitionTable* p = &cuts->partitionTable; + //x0 x1 x2 l1 l2 l3 l4 l5 l6 + (*p)[0]=1; (*p)[1]=0; (*p)[2]=2; (*p)[3]=1; (*p)[4]=0; (*p)[5]=2; (*p)[6]=1; (*p)[7]=0; (*p)[8]=2; + + cuts->children.push_back(boost::shared_ptr(new Cuts())); + cuts->children[0]->partitionTable = PartitionTable(9, -1); p = &cuts->children[0]->partitionTable; + //x0 x1 x2 l1 l2 l3 l4 l5 l6 + (*p)[0]=0; (*p)[1]=-1; (*p)[2]=-1; (*p)[3]=1; (*p)[4]=-1; (*p)[5]=-1; (*p)[6]=2; (*p)[7]=-1; (*p)[8]=-1; + + cuts->children.push_back(boost::shared_ptr(new Cuts())); + cuts->children[1]->partitionTable = PartitionTable(9, -1); p = &cuts->children[1]->partitionTable; + //x0 x1 x2 l1 l2 l3 l4 l5 l6 + (*p)[0]=-1; (*p)[1]=-1; (*p)[2]=0; (*p)[3]=-1; (*p)[4]=-1; (*p)[5]=1; (*p)[6]=-1; (*p)[7]=-1; (*p)[8]=2; + + + // nested dissection + NestedDissection nd(fg, ordering, cuts); + LONGS_EQUAL(2, nd.root()->size()); + LONGS_EQUAL(3, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps + LONGS_EQUAL(0, nd.root()->weeklinks().size()); + + // the 1st submap + LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); // x0 + LONGS_EQUAL(4, nd.root()->children()[0]->size()); + LONGS_EQUAL(2, nd.root()->children()[0]->children().size()); + + // the 1-1st submap + LONGS_EQUAL(1, nd.root()->children()[0]->children()[0]->frontal().size()); // l1 + LONGS_EQUAL(2, nd.root()->children()[0]->children()[0]->size()); + + // the 1-2nd submap + LONGS_EQUAL(1, nd.root()->children()[0]->children()[1]->frontal().size()); // l4 + LONGS_EQUAL(2, nd.root()->children()[0]->children()[1]->size()); + + // the 2nd submap + LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); // x2 + LONGS_EQUAL(3, nd.root()->children()[1]->size()); + LONGS_EQUAL(2, nd.root()->children()[1]->children().size()); + + // the 2-1st submap + LONGS_EQUAL(1, nd.root()->children()[1]->children()[0]->frontal().size()); // l3 + LONGS_EQUAL(2, nd.root()->children()[1]->children()[0]->size()); + + // the 2-2nd submap + LONGS_EQUAL(1, nd.root()->children()[1]->children()[1]->frontal().size()); // l6 + LONGS_EQUAL(2, nd.root()->children()[1]->children()[1]->size()); + +} + +/* ************************************************************************* */ +// l1-l8 l9-16 l17-124 +// / | / \ | \ +// x0 x1 x2 x3 +TEST( NestedDissection, Graph3D) { + using namespace gtsam::submapVisualSLAM; + typedef TSAM3D::SubNLG SubNLG; + typedef partition::PartitionTable PartitionTable; + vector cameras; + cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-2., 0., 0.)))); + cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-1., 0., 0.)))); + cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 1., 0., 0.)))); + cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 2., 0., 0.)))); + + vector points; + for (int cube_index = 0; cube_index <= 3; cube_index++) { + Point3 center((cube_index-1) * 3, 0.5, 10.); + points.push_back(center + Point3(-0.5, -0.5, -0.5)); + points.push_back(center + Point3(-0.5, 0.5, -0.5)); + points.push_back(center + Point3( 0.5, 0.5, -0.5)); + points.push_back(center + Point3( 0.5, -0.5, -0.5)); + points.push_back(center + Point3(-0.5, -0.5, 0.5)); + points.push_back(center + Point3(-0.5, 0.5, 0.5)); + points.push_back(center + Point3( 0.5, 0.5, 0.5)); + points.push_back(center + Point3( 0.5, 0.5, 0.5)); + } + + Graph graph; + SharedDiagonal measurementNoise(gtsam::Vector_(2, 1., 1.)); + SharedDiagonal measurementZeroNoise(gtsam::Vector_(2, 0., 0.)); + for (int j=1; j<=8; j++) + graph.addMeasurement(0, j, cameras[0].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); + for (int j=1; j<=16; j++) + graph.addMeasurement(1, j, cameras[1].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); + for (int j=9; j<=24; j++) + graph.addMeasurement(2, j, cameras[2].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); + for (int j=17; j<=24; j++) + graph.addMeasurement(3, j, cameras[3].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); + + // make an easy ordering + Ordering ordering; ordering += x0, x1, x2, x3; + for (int j=1; j<=24; j++) + ordering += Symbol('l', j); + + // nested dissection + const int numNodeStopPartition = 10; + const int minNodesPerMap = 5; + NestedDissection nd(graph, ordering, numNodeStopPartition, minNodesPerMap); + + LONGS_EQUAL(0, nd.root()->size()); + LONGS_EQUAL(8, nd.root()->frontal().size()); // l9-l16 + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps + LONGS_EQUAL(0, nd.root()->weeklinks().size()); + + // the 1st submap + LONGS_EQUAL(10, nd.root()->children()[0]->frontal().size()); // x0, x1, l1-l8 + LONGS_EQUAL(24, nd.root()->children()[0]->size()); // 8 + 16 + LONGS_EQUAL(0, nd.root()->children()[0]->children().size()); + + // the 2nd submap + LONGS_EQUAL(10, nd.root()->children()[1]->frontal().size()); // x2, x3, l1-l8 + LONGS_EQUAL(24, nd.root()->children()[1]->size()); // 16 + 8 + LONGS_EQUAL(0, nd.root()->children()[1]->children().size()); +} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */