172 lines
3.8 KiB
C++
172 lines
3.8 KiB
C++
/*
|
|
* testTupleConfig.cpp
|
|
*
|
|
* Created on: Jan 13, 2010
|
|
* Author: richard
|
|
*/
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
#include <stdexcept>
|
|
|
|
#define GTSAM_MAGIC_KEY
|
|
|
|
#include <Pose2.h>
|
|
#include <Point2.h>
|
|
|
|
#include "Vector.h"
|
|
#include "Key.h"
|
|
#include "VectorConfig.h"
|
|
#include "TupleConfig-inl.h"
|
|
|
|
using namespace gtsam;
|
|
using namespace std;
|
|
|
|
typedef TypedSymbol<Pose2, 'x'> PoseKey;
|
|
typedef TypedSymbol<Point2, 'l'> PointKey;
|
|
typedef PairConfig<PoseKey, Pose2, PointKey, Point2> Config;
|
|
|
|
/* ************************************************************************* */
|
|
TEST( PairConfig, insert_equals1 )
|
|
{
|
|
Pose2 x1(1,2,3), x2(6,7,8);
|
|
Point2 l1(4,5), l2(9,10);
|
|
|
|
Config expected;
|
|
expected.insert(1, x1);
|
|
expected.insert(2, x2);
|
|
expected.insert(1, l1);
|
|
expected.insert(2, l2);
|
|
|
|
Config actual;
|
|
actual.insert(1, x1);
|
|
actual.insert(2, x2);
|
|
actual.insert(1, l1);
|
|
actual.insert(2, l2);
|
|
|
|
CHECK(assert_equal(expected,actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( PairConfig, insert_equals2 )
|
|
{
|
|
Pose2 x1(1,2,3), x2(6,7,8);
|
|
Point2 l1(4,5), l2(9,10);
|
|
|
|
Config cfg1;
|
|
cfg1.insert(1, x1);
|
|
cfg1.insert(2, x2);
|
|
cfg1.insert(1, l1);
|
|
cfg1.insert(2, l2);
|
|
|
|
Config cfg2;
|
|
cfg2.insert(1, x1);
|
|
cfg2.insert(2, x2);
|
|
cfg2.insert(1, l1);
|
|
|
|
CHECK(!cfg1.equals(cfg2));
|
|
|
|
cfg2.insert(2, Point2(9,11));
|
|
|
|
CHECK(!cfg1.equals(cfg2));
|
|
}
|
|
|
|
///* ************************************************************************* */
|
|
//TEST( PairConfig, insert_duplicate )
|
|
//{
|
|
// Pose2 x1(1,2,3), x2(6,7,8);
|
|
// Point2 l1(4,5), l2(9,10);
|
|
//
|
|
// Config cfg1;
|
|
// cfg1.insert(1, x1);
|
|
// cfg1.insert(2, x2);
|
|
// cfg1.insert(1, l1);
|
|
// cfg1.insert(2, l2);
|
|
// cfg1.insert(2, l1);
|
|
//
|
|
// CHECK(assert_equal(l2, cfg1[PointKey(2)]));
|
|
// CHECK(cfg1.size() == 4);
|
|
// CHECK(cfg1.dim() == 10);
|
|
//}
|
|
|
|
|
|
/* ************************************************************************* */
|
|
TEST( PairConfig, size_dim )
|
|
{
|
|
Pose2 x1(1,2,3), x2(6,7,8);
|
|
Point2 l1(4,5), l2(9,10);
|
|
|
|
Config cfg1;
|
|
cfg1.insert(1, x1);
|
|
cfg1.insert(2, x2);
|
|
cfg1.insert(1, l1);
|
|
cfg1.insert(2, l2);
|
|
|
|
CHECK(cfg1.size() == 4);
|
|
CHECK(cfg1.dim() == 10);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(PairConfig, at)
|
|
{
|
|
Pose2 x1(1,2,3), x2(6,7,8);
|
|
Point2 l1(4,5), l2(9,10);
|
|
|
|
Config cfg1;
|
|
cfg1.insert(1, x1);
|
|
cfg1.insert(2, x2);
|
|
cfg1.insert(1, l1);
|
|
cfg1.insert(2, l2);
|
|
|
|
CHECK(assert_equal(x1, cfg1[PoseKey(1)]));
|
|
CHECK(assert_equal(x2, cfg1[PoseKey(2)]));
|
|
CHECK(assert_equal(l1, cfg1[PointKey(1)]));
|
|
CHECK(assert_equal(l2, cfg1[PointKey(2)]));
|
|
|
|
bool caught = false;
|
|
try {
|
|
cfg1[PoseKey(3)];
|
|
} catch(invalid_argument e) {
|
|
caught = true;
|
|
}
|
|
CHECK(caught);
|
|
|
|
caught = false;
|
|
try {
|
|
cfg1[PointKey(3)];
|
|
} catch(invalid_argument e) {
|
|
caught = true;
|
|
}
|
|
CHECK(caught);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(PairConfig, expmap)
|
|
{
|
|
Pose2 x1(1,2,3), x2(6,7,8);
|
|
Point2 l1(4,5), l2(9,10);
|
|
|
|
Config cfg1;
|
|
cfg1.insert(1, x1);
|
|
cfg1.insert(2, x2);
|
|
cfg1.insert(1, l1);
|
|
cfg1.insert(2, l2);
|
|
|
|
VectorConfig increment;
|
|
increment.insert("x1", Vector_(3, 1.0, 1.1, 1.2));
|
|
increment.insert("x2", Vector_(3, 1.3, 1.4, 1.5));
|
|
increment.insert("l1", Vector_(2, 1.0, 1.1));
|
|
increment.insert("l2", Vector_(2, 1.3, 1.4));
|
|
|
|
Config expected;
|
|
expected.insert(1, expmap(x1, Vector_(3, 1.0, 1.1, 1.2)));
|
|
expected.insert(2, expmap(x2, Vector_(3, 1.3, 1.4, 1.5)));
|
|
expected.insert(1, Point2(5.0, 6.1));
|
|
expected.insert(2, Point2(10.3, 11.4));
|
|
|
|
CHECK(assert_equal(expected, expmap(cfg1, increment)));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
|
/* ************************************************************************* */
|