112 lines
2.7 KiB
C++
112 lines
2.7 KiB
C++
/**
|
|
* @file testPlanarSLAM.cpp
|
|
* @authors Frank Dellaert
|
|
**/
|
|
|
|
#include <iostream>
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
#include "planarSLAM.h"
|
|
#include "BearingRangeFactor.h"
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
// some shared test values
|
|
static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4);
|
|
static Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
|
|
|
|
SharedGaussian
|
|
sigma(noiseModel::Isotropic::Sigma(1,0.1)),
|
|
sigma2(noiseModel::Isotropic::Sigma(2,0.1)),
|
|
I3(noiseModel::Unit::Create(3));
|
|
|
|
/* ************************************************************************* */
|
|
TEST( planarSLAM, BearingFactor )
|
|
{
|
|
// Create factor
|
|
Rot2 z = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
|
|
planarSLAM::Bearing factor(2, 3, z, sigma);
|
|
|
|
// create config
|
|
planarSLAM::Config c;
|
|
c.insert(2, x2);
|
|
c.insert(3, l3);
|
|
|
|
// Check error
|
|
Vector actual = factor.unwhitenedError(c);
|
|
CHECK(assert_equal(Vector_(1,-0.1),actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( planarSLAM, RangeFactor )
|
|
{
|
|
// Create factor
|
|
double z(sqrt(2) - 0.22); // h(x) - z = 0.22
|
|
planarSLAM::Range factor(2, 3, z, sigma);
|
|
|
|
// create config
|
|
planarSLAM::Config c;
|
|
c.insert(2, x2);
|
|
c.insert(3, l3);
|
|
|
|
// Check error
|
|
Vector actual = factor.unwhitenedError(c);
|
|
CHECK(assert_equal(Vector_(1,0.22),actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( planarSLAM, BearingRangeFactor )
|
|
{
|
|
// Create factor
|
|
Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
|
|
double b(sqrt(2) - 0.22); // h(x) - z = 0.22
|
|
planarSLAM::BearingRange factor(2, 3, r, b, sigma2);
|
|
|
|
// create config
|
|
planarSLAM::Config c;
|
|
c.insert(2, x2);
|
|
c.insert(3, l3);
|
|
|
|
// Check error
|
|
Vector actual = factor.unwhitenedError(c);
|
|
CHECK(assert_equal(Vector_(2,-0.1, 0.22),actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( planarSLAM, constructor )
|
|
{
|
|
// create config
|
|
planarSLAM::Config c;
|
|
c.insert(2, x2);
|
|
c.insert(3, x3);
|
|
c.insert(3, l3);
|
|
|
|
// create graph
|
|
planarSLAM::Graph G;
|
|
|
|
// Add pose constraint
|
|
G.addPoseConstraint(2, x2); // make it feasible :-)
|
|
|
|
// Add odometry
|
|
G.addOdometry(2, 3, Pose2(0, 0, M_PI_4), I3);
|
|
|
|
// Create bearing factor
|
|
Rot2 z1 = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
|
|
G.addBearing(2, 3, z1, sigma);
|
|
|
|
// Create range factor
|
|
double z2(sqrt(2) - 0.22); // h(x) - z = 0.22
|
|
G.addRange(2, 3, z2, sigma);
|
|
|
|
Vector expected = Vector_(8, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.1, 0.22);
|
|
CHECK(assert_equal(expected,G.unwhitenedError(c)));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() {
|
|
TestResult tr;
|
|
return TestRegistry::runAllTests(tr);
|
|
}
|
|
/* ************************************************************************* */
|