80 lines
2.2 KiB
C++
80 lines
2.2 KiB
C++
/*
|
|
* testConstrainedNonlinearFactorGraph.cpp
|
|
*
|
|
* Created on: Aug 10, 2009
|
|
* Author: Alex Cunningham
|
|
*/
|
|
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
#include "ConstrainedNonlinearFactorGraph.h"
|
|
#include "smallExample.h"
|
|
|
|
using namespace gtsam;
|
|
|
|
typedef boost::shared_ptr<NonlinearFactor> shared;
|
|
|
|
TEST( ConstrainedNonlinearFactorGraph, equals )
|
|
{
|
|
ConstrainedNonlinearFactorGraph fg = createConstrainedNonlinearFactorGraph();
|
|
ConstrainedNonlinearFactorGraph fg2 = createConstrainedNonlinearFactorGraph();
|
|
CHECK( fg.equals(fg2) );
|
|
}
|
|
|
|
TEST( ConstrainedNonlinearFactorGraph, copy )
|
|
{
|
|
NonlinearFactorGraph nfg = createNonlinearFactorGraph();
|
|
ConstrainedNonlinearFactorGraph actual(nfg);
|
|
|
|
shared f1 = nfg[0];
|
|
shared f2 = nfg[1];
|
|
shared f3 = nfg[2];
|
|
shared f4 = nfg[3];
|
|
|
|
ConstrainedNonlinearFactorGraph expected;
|
|
expected.push_back(f1);
|
|
expected.push_back(f2);
|
|
expected.push_back(f3);
|
|
expected.push_back(f4);
|
|
|
|
CHECK(actual.equals(expected));
|
|
}
|
|
|
|
TEST( ConstrainedNonlinearFactorGraph, baseline )
|
|
{
|
|
// use existing examples
|
|
NonlinearFactorGraph nfg = createNonlinearFactorGraph();
|
|
ConstrainedNonlinearFactorGraph cfg(nfg);
|
|
|
|
FGConfig initial = createNoisyConfig();
|
|
ConstrainedLinearFactorGraph linearized = cfg.linearize(initial);
|
|
LinearFactorGraph lfg = createLinearFactorGraph();
|
|
ConstrainedLinearFactorGraph expected(lfg);
|
|
|
|
CHECK(expected.equals(linearized));
|
|
}
|
|
|
|
TEST( ConstrainedNonlinearFactorGraph, convert )
|
|
{
|
|
NonlinearFactorGraph expected = createNonlinearFactorGraph();
|
|
ConstrainedNonlinearFactorGraph cfg(expected);
|
|
NonlinearFactorGraph actual = cfg.convert();
|
|
CHECK(actual.equals(expected));
|
|
}
|
|
|
|
TEST( ConstrainedNonlinearFactorGraph, linearize_and_solve )
|
|
{
|
|
ConstrainedNonlinearFactorGraph nfg = createConstrainedNonlinearFactorGraph();
|
|
FGConfig lin = createConstrainedLinConfig();
|
|
ConstrainedLinearFactorGraph actual_lfg = nfg.linearize(lin);
|
|
FGConfig actual = actual_lfg.optimize(actual_lfg.getOrdering());
|
|
|
|
FGConfig expected = createConstrainedCorrectDelta();
|
|
CHECK(actual.equals(expected));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
|
/* ************************************************************************* */
|
|
|