53 lines
1.4 KiB
C++
53 lines
1.4 KiB
C++
/*
|
|
* testGraph.cpp
|
|
*
|
|
* Created on: Jan 12, 2010
|
|
* Author: nikai
|
|
* Description: unit test for graph-inl.h
|
|
*/
|
|
|
|
#include <iostream>
|
|
#include <boost/shared_ptr.hpp>
|
|
#include <boost/assign/std/list.hpp>
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
#include "Pose2Graph.h"
|
|
#include "graph-inl.h"
|
|
|
|
using namespace std;
|
|
using namespace boost;
|
|
using namespace gtsam;
|
|
|
|
/* ************************************************************************* */
|
|
TEST( Graph, composePoses )
|
|
{
|
|
Pose2Graph graph;
|
|
Matrix cov = eye(3);
|
|
graph.push_back(boost::shared_ptr<Pose2Factor>(new Pose2Factor("x1", "x2", Pose2(2.0, 0.0, 0.0), cov)));
|
|
graph.push_back(boost::shared_ptr<Pose2Factor>(new Pose2Factor("x2", "x3", Pose2(3.0, 0.0, 0.0), cov)));
|
|
|
|
map<string, string> tree;
|
|
tree.insert(make_pair("x1", "x2"));
|
|
tree.insert(make_pair("x2", "x2"));
|
|
tree.insert(make_pair("x3", "x2"));
|
|
|
|
Pose2 rootPose(3.0, 0.0, 0.0);
|
|
|
|
Pose2Config actual = composePoses<Pose2Graph, Pose2Factor, Pose2, Pose2Config>(graph, tree, rootPose);
|
|
Pose2Config expected;
|
|
expected.insert("x1", Pose2(1.0, 0.0, 0.0));
|
|
expected.insert("x2", Pose2(3.0, 0.0, 0.0));
|
|
expected.insert("x3", Pose2(6.0, 0.0, 0.0));
|
|
|
|
LONGS_EQUAL(3, actual.size());
|
|
CHECK(assert_equal(expected, actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() {
|
|
TestResult tr;
|
|
return TestRegistry::runAllTests(tr);
|
|
}
|
|
/* ************************************************************************* */
|