255 lines
9.9 KiB
C++
255 lines
9.9 KiB
C++
/*
|
|
* 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 <gtsam_unstable/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);}
|
|
/* ************************************************************************* */
|