83 lines
2.1 KiB
C++
83 lines
2.1 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<FGConfig> > shared;
|
|
typedef ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig>,FGConfig> TestGraph;
|
|
|
|
TEST( TestGraph, equals )
|
|
{
|
|
TestGraph fg = createConstrainedNonlinearFactorGraph();
|
|
TestGraph fg2 = createConstrainedNonlinearFactorGraph();
|
|
CHECK( fg.equals(fg2) );
|
|
}
|
|
|
|
TEST( TestGraph, copy )
|
|
{
|
|
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
|
|
TestGraph actual(nfg);
|
|
|
|
shared f1 = nfg[0];
|
|
shared f2 = nfg[1];
|
|
shared f3 = nfg[2];
|
|
shared f4 = nfg[3];
|
|
|
|
TestGraph expected;
|
|
expected.push_back(f1);
|
|
expected.push_back(f2);
|
|
expected.push_back(f3);
|
|
expected.push_back(f4);
|
|
|
|
CHECK(actual.equals(expected));
|
|
}
|
|
|
|
TEST( TestGraph, baseline )
|
|
{
|
|
// use existing examples
|
|
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
|
|
TestGraph cfg(nfg);
|
|
|
|
FGConfig initial = createNoisyConfig();
|
|
ConstrainedLinearFactorGraph linearized = cfg.linearize(initial);
|
|
LinearFactorGraph lfg = createLinearFactorGraph();
|
|
ConstrainedLinearFactorGraph expected(lfg);
|
|
|
|
CHECK(expected.equals(linearized));
|
|
}
|
|
|
|
/*
|
|
TEST( TestGraph, convert )
|
|
{
|
|
ExampleNonlinearFactorGraph expected = createNonlinearFactorGraph();
|
|
TestGraph cfg(expected);
|
|
ExampleNonlinearFactorGraph actual = cfg.convert();
|
|
CHECK(actual.equals(expected));
|
|
}
|
|
*/
|
|
|
|
TEST( TestGraph, linearize_and_solve )
|
|
{
|
|
TestGraph 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);}
|
|
/* ************************************************************************* */
|
|
|