/*
 * 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);}
/* ************************************************************************* */