235 lines
6.8 KiB
C++
235 lines
6.8 KiB
C++
/**
|
|
* @file testNonlinearFactor.cpp
|
|
* @brief Unit tests for Non-Linear Factor,
|
|
* create a non linear factor graph and a configuration for it and
|
|
* calculate the error for the factor.
|
|
* @author Christian Potthast
|
|
**/
|
|
|
|
/*STL/C++*/
|
|
#include <iostream>
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
#include "Matrix.h"
|
|
#include "smallExample.h"
|
|
#include "Simulated2DMeasurement.h"
|
|
#include "Pose2.h"
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared_nlf;
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearFactor, equals )
|
|
{
|
|
double sigma = 1.0;
|
|
|
|
// create two nonlinear2 factors
|
|
Vector z3(2); z3(0) = 0. ; z3(1) = -1.;
|
|
Simulated2DMeasurement f0(z3, sigma, "x1", "l1");
|
|
|
|
// measurement between x2 and l1
|
|
Vector z4(2); z4(0)= -1.5 ; z4(1) = -1.;
|
|
Simulated2DMeasurement f1(z4, sigma, "x2", "l1");
|
|
|
|
CHECK(assert_equal(f0,f0));
|
|
CHECK(f0.equals(f0));
|
|
CHECK(!f0.equals(f1));
|
|
CHECK(!f1.equals(f0));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearFactor, equals2 )
|
|
{
|
|
// create a non linear factor graph
|
|
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
|
|
|
|
// get two factors
|
|
shared_nlf f0 = fg[0], f1 = fg[1];
|
|
|
|
CHECK(f0->equals(*f0));
|
|
CHECK(!f0->equals(*f1));
|
|
CHECK(!f1->equals(*f0));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearFactor, NonlinearFactor )
|
|
{
|
|
// create a non linear factor graph
|
|
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
|
|
|
|
// create a configuration for the non linear factor graph
|
|
VectorConfig cfg = createNoisyConfig();
|
|
|
|
// get the factor "f1" from the factor graph
|
|
shared_nlf factor = fg[0];
|
|
|
|
// calculate the error_vector from the factor "f1"
|
|
Vector actual_e = factor->error_vector(cfg);
|
|
Vector e(2); e(0) = -0.1; e(1) = -0.1;
|
|
CHECK(assert_equal(e,actual_e));
|
|
|
|
// the expected value for the error from the factor
|
|
// error_vector / sigma = [0.1 0.1]/0.1 = [1;1]
|
|
// error = 0.5 * [1 1] * [1;1] = 1
|
|
double expected = 1.0;
|
|
|
|
// calculate the error from the factor "f1"
|
|
double actual = factor->error(cfg);
|
|
DOUBLES_EQUAL(expected,actual,0.00000001);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearFactor, linearize_f1 )
|
|
{
|
|
// Grab a non-linear factor
|
|
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
|
|
boost::shared_ptr<NonlinearFactor1> nlf =
|
|
boost::static_pointer_cast<NonlinearFactor1>(nfg[0]);
|
|
|
|
// We linearize at noisy config from SmallExample
|
|
VectorConfig c = createNoisyConfig();
|
|
GaussianFactor::shared_ptr actual = nlf->linearize(c);
|
|
|
|
GaussianFactorGraph lfg = createGaussianFactorGraph();
|
|
GaussianFactor::shared_ptr expected = lfg[0];
|
|
|
|
CHECK(expected->equals(*actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearFactor, linearize_f2 )
|
|
{
|
|
// Grab a non-linear factor
|
|
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
|
|
boost::shared_ptr<NonlinearFactor1> nlf =
|
|
boost::static_pointer_cast<NonlinearFactor1>(nfg[1]);
|
|
|
|
// We linearize at noisy config from SmallExample
|
|
VectorConfig c = createNoisyConfig();
|
|
GaussianFactor::shared_ptr actual = nlf->linearize(c);
|
|
|
|
GaussianFactorGraph lfg = createGaussianFactorGraph();
|
|
GaussianFactor::shared_ptr expected = lfg[1];
|
|
|
|
CHECK(expected->equals(*actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearFactor, linearize_f3 )
|
|
{
|
|
// Grab a non-linear factor
|
|
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
|
|
boost::shared_ptr<NonlinearFactor1> nlf =
|
|
boost::static_pointer_cast<NonlinearFactor1>(nfg[2]);
|
|
|
|
// We linearize at noisy config from SmallExample
|
|
VectorConfig c = createNoisyConfig();
|
|
GaussianFactor::shared_ptr actual = nlf->linearize(c);
|
|
|
|
GaussianFactorGraph lfg = createGaussianFactorGraph();
|
|
GaussianFactor::shared_ptr expected = lfg[2];
|
|
|
|
CHECK(expected->equals(*actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearFactor, linearize_f4 )
|
|
{
|
|
// Grab a non-linear factor
|
|
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
|
|
boost::shared_ptr<NonlinearFactor1> nlf =
|
|
boost::static_pointer_cast<NonlinearFactor1>(nfg[3]);
|
|
|
|
// We linearize at noisy config from SmallExample
|
|
VectorConfig c = createNoisyConfig();
|
|
GaussianFactor::shared_ptr actual = nlf->linearize(c);
|
|
|
|
GaussianFactorGraph lfg = createGaussianFactorGraph();
|
|
GaussianFactor::shared_ptr expected = lfg[3];
|
|
|
|
CHECK(expected->equals(*actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearFactor, size )
|
|
{
|
|
// create a non linear factor graph
|
|
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
|
|
|
|
// create a configuration for the non linear factor graph
|
|
VectorConfig cfg = createNoisyConfig();
|
|
|
|
// get some factors from the graph
|
|
shared_nlf factor1 = fg[0];
|
|
shared_nlf factor2 = fg[1];
|
|
shared_nlf factor3 = fg[2];
|
|
|
|
CHECK(factor1->size() == 1);
|
|
CHECK(factor2->size() == 2);
|
|
CHECK(factor3->size() == 2);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
#include "numericalDerivative.h"
|
|
|
|
Pose2 h(const Pose2& p1, const Pose2& p2) {
|
|
Pose2 dpose = p2 - p1;
|
|
return dpose.rotate(-p1.theta());
|
|
}
|
|
|
|
Matrix H1(const Pose2& p1, const Pose2& p2) {
|
|
double dx= p2.x()-p1.x();
|
|
double dy= p2.y()-p1.y();
|
|
double ct=cos(p1.theta());
|
|
double st=sin(p1.theta());
|
|
return Matrix_(3,3,
|
|
-ct, -st, -st*dx+ct*dy,
|
|
st, -ct, -ct*dx-st*dy,
|
|
0.0, 0.0, -1.0
|
|
);
|
|
}
|
|
|
|
Matrix H2(const Pose2& p1) {
|
|
double ct=cos(p1.theta());
|
|
double st=sin(p1.theta());
|
|
return Matrix_(3,3,
|
|
ct, st, 0.0,
|
|
-st, ct, 0.0,
|
|
0.0, 0.0, 1.0
|
|
);
|
|
}
|
|
|
|
TEST( PoseConstraintFactor2, testFunctions )
|
|
{
|
|
Pose2 p1(0.0, 6.0, 0.0);
|
|
Pose2 p2(0.101826, 6.111236, 0.011499);
|
|
|
|
//expected
|
|
Pose2 expectedh(0.101826, 0.111236, 0.011499);
|
|
Matrix expectedH1 = Matrix_(3,3,-1.0, 0.0, 0.111236, 0.0, -1.0, -0.101826, 0.0, 0.0, -1.0);
|
|
Matrix expectedH2 = Matrix_(3,3, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
|
|
|
|
// actual
|
|
Pose2 actualh = h(p1,p2);
|
|
Matrix actualH1 = H1(p1,p2);
|
|
Matrix actualH2 = H2(p1);
|
|
|
|
CHECK(assert_equal(expectedh,actualh));
|
|
CHECK(assert_equal(expectedH1,actualH1));
|
|
CHECK(assert_equal(expectedH2,actualH2));
|
|
|
|
Matrix numericalH1 = numericalDerivative21(h, p1, p2, 1e-5);
|
|
//CHECK(assert_equal(numericalH1,actualH1));
|
|
|
|
Matrix numericalH2 = numericalDerivative22(h, p1, p2, 1e-5);
|
|
CHECK(assert_equal(numericalH2,actualH2));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
|
/* ************************************************************************* */
|