533 lines
		
	
	
		
			18 KiB
		
	
	
	
		
			C++
		
	
	
			
		
		
	
	
			533 lines
		
	
	
		
			18 KiB
		
	
	
	
		
			C++
		
	
	
/* ----------------------------------------------------------------------------
 | 
						|
 | 
						|
 * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
 | 
						|
 * Atlanta, Georgia 30332-0415
 | 
						|
 * All Rights Reserved
 | 
						|
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 | 
						|
 | 
						|
 * See LICENSE for the license information
 | 
						|
 | 
						|
 * -------------------------------------------------------------------------- */
 | 
						|
 | 
						|
/**
 | 
						|
 *  @file   testGaussianFactorGraphB.cpp
 | 
						|
 *  @brief  Unit tests for Linear Factor Graph
 | 
						|
 *  @author Christian Potthast
 | 
						|
 **/
 | 
						|
 | 
						|
#include <tests/smallExample.h>
 | 
						|
#include <gtsam/nonlinear/Symbol.h>
 | 
						|
#include <gtsam/linear/GaussianBayesNet.h>
 | 
						|
#include <gtsam/linear/GaussianSequentialSolver.h>
 | 
						|
#include <gtsam/linear/GaussianFactorGraph.h>
 | 
						|
#include <gtsam/inference/SymbolicFactorGraph.h>
 | 
						|
#include <gtsam/base/numericalDerivative.h>
 | 
						|
#include <gtsam/base/Matrix.h>
 | 
						|
#include <gtsam/base/Testable.h>
 | 
						|
 | 
						|
#include <CppUnitLite/TestHarness.h>
 | 
						|
 | 
						|
#include <boost/foreach.hpp>
 | 
						|
#include <boost/tuple/tuple.hpp>
 | 
						|
#include <boost/assign/std/list.hpp> // for operator +=
 | 
						|
#include <boost/assign/std/set.hpp> // for operator +=
 | 
						|
#include <boost/assign/std/vector.hpp> // for operator +=
 | 
						|
using namespace boost::assign;
 | 
						|
 | 
						|
#include <string.h>
 | 
						|
#include <iostream>
 | 
						|
 | 
						|
using namespace std;
 | 
						|
using namespace gtsam;
 | 
						|
using namespace example;
 | 
						|
 | 
						|
double tol=1e-5;
 | 
						|
 | 
						|
using symbol_shorthand::X;
 | 
						|
using symbol_shorthand::L;
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST( GaussianFactorGraph, equals ) {
 | 
						|
 | 
						|
  Ordering ordering; ordering += X(1),X(2),L(1);
 | 
						|
  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 | 
						|
  GaussianFactorGraph fg2 = createGaussianFactorGraph(ordering);
 | 
						|
  EXPECT(fg.equals(fg2));
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST( GaussianFactorGraph, error ) {
 | 
						|
  Ordering ordering; ordering += X(1),X(2),L(1);
 | 
						|
  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 | 
						|
  VectorValues cfg = createZeroDelta(ordering);
 | 
						|
 | 
						|
  // note the error is the same as in testNonlinearFactorGraph as a
 | 
						|
  // zero delta config in the linear graph is equivalent to noisy in
 | 
						|
  // non-linear, which is really linear under the hood
 | 
						|
  double actual = fg.error(cfg);
 | 
						|
  DOUBLES_EQUAL( 5.625, actual, 1e-9 );
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST( GaussianFactorGraph, eliminateOne_x1 )
 | 
						|
{
 | 
						|
  Ordering ordering; ordering += X(1),L(1),X(2);
 | 
						|
  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 | 
						|
 | 
						|
  GaussianConditional::shared_ptr conditional;
 | 
						|
  GaussianFactorGraph remaining;
 | 
						|
  boost::tie(conditional,remaining) = inference::eliminateOne(fg, 0, EliminateQR);
 | 
						|
 | 
						|
  // create expected Conditional Gaussian
 | 
						|
  Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
 | 
						|
  Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2);
 | 
						|
  GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma);
 | 
						|
 | 
						|
  EXPECT(assert_equal(expected,*conditional,tol));
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST( GaussianFactorGraph, eliminateOne_x2 )
 | 
						|
{
 | 
						|
  Ordering ordering; ordering += X(2),L(1),X(1);
 | 
						|
  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 | 
						|
  GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, 0, EliminateQR).first;
 | 
						|
 | 
						|
  // create expected Conditional Gaussian
 | 
						|
  double sig = 0.0894427;
 | 
						|
  Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
 | 
						|
  Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2);
 | 
						|
  GaussianConditional expected(ordering[X(2)],d,R11,ordering[L(1)],S12,ordering[X(1)],S13,sigma);
 | 
						|
 | 
						|
  EXPECT(assert_equal(expected,*actual,tol));
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST( GaussianFactorGraph, eliminateOne_l1 )
 | 
						|
{
 | 
						|
  Ordering ordering; ordering += L(1),X(1),X(2);
 | 
						|
  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 | 
						|
  GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, 0, EliminateQR).first;
 | 
						|
 | 
						|
  // create expected Conditional Gaussian
 | 
						|
  double sig = sqrt(2.0)/10.;
 | 
						|
  Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
 | 
						|
  Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
 | 
						|
  GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma);
 | 
						|
 | 
						|
  EXPECT(assert_equal(expected,*actual,tol));
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST( GaussianFactorGraph, eliminateOne_x1_fast )
 | 
						|
{
 | 
						|
  Ordering ordering; ordering += X(1),L(1),X(2);
 | 
						|
  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 | 
						|
  GaussianConditional::shared_ptr conditional;
 | 
						|
  GaussianFactorGraph remaining;
 | 
						|
  boost::tie(conditional,remaining) = inference::eliminateOne(fg, ordering[X(1)], EliminateQR);
 | 
						|
 | 
						|
  // create expected Conditional Gaussian
 | 
						|
  Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
 | 
						|
  Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2);
 | 
						|
  GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma);
 | 
						|
 | 
						|
  // Create expected remaining new factor
 | 
						|
  JacobianFactor expectedFactor(1, Matrix_(4,2,
 | 
						|
             4.714045207910318,                   0.,
 | 
						|
                             0.,   4.714045207910318,
 | 
						|
                             0.,                   0.,
 | 
						|
                             0.,                   0.),
 | 
						|
     2, Matrix_(4,2,
 | 
						|
           -2.357022603955159,                   0.,
 | 
						|
                            0.,  -2.357022603955159,
 | 
						|
            7.071067811865475,                   0.,
 | 
						|
                            0.,   7.071067811865475),
 | 
						|
     Vector_(4, -0.707106781186547, 0.942809041582063, 0.707106781186547, -1.414213562373094), noiseModel::Unit::Create(4));
 | 
						|
 | 
						|
  EXPECT(assert_equal(expected,*conditional,tol));
 | 
						|
  EXPECT(assert_equal((const GaussianFactor&)expectedFactor,*remaining.back(),tol));
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST( GaussianFactorGraph, eliminateOne_x2_fast )
 | 
						|
{
 | 
						|
  Ordering ordering; ordering += X(1),L(1),X(2);
 | 
						|
  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 | 
						|
  GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, ordering[X(2)], EliminateQR).first;
 | 
						|
 | 
						|
  // create expected Conditional Gaussian
 | 
						|
  double sig = 0.0894427;
 | 
						|
  Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
 | 
						|
  Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2);
 | 
						|
  GaussianConditional expected(ordering[X(2)],d,R11,ordering[X(1)],S13,ordering[L(1)],S12,sigma);
 | 
						|
 | 
						|
  EXPECT(assert_equal(expected,*actual,tol));
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST( GaussianFactorGraph, eliminateOne_l1_fast )
 | 
						|
{
 | 
						|
  Ordering ordering; ordering += X(1),L(1),X(2);
 | 
						|
  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 | 
						|
  GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, ordering[L(1)], EliminateQR).first;
 | 
						|
 | 
						|
  // create expected Conditional Gaussian
 | 
						|
  double sig = sqrt(2.0)/10.;
 | 
						|
  Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
 | 
						|
  Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
 | 
						|
  GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma);
 | 
						|
 | 
						|
  EXPECT(assert_equal(expected,*actual,tol));
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST( GaussianFactorGraph, eliminateAll )
 | 
						|
{
 | 
						|
	// create expected Chordal bayes Net
 | 
						|
	Matrix I = eye(2);
 | 
						|
 | 
						|
  Ordering ordering;
 | 
						|
  ordering += X(2),L(1),X(1);
 | 
						|
 | 
						|
	Vector d1 = Vector_(2, -0.1,-0.1);
 | 
						|
	GaussianBayesNet expected = simpleGaussian(ordering[X(1)],d1,0.1);
 | 
						|
 | 
						|
	double sig1 = 0.149071;
 | 
						|
	Vector d2 = Vector_(2, 0.0, 0.2)/sig1, sigma2 = ones(2);
 | 
						|
	push_front(expected,ordering[L(1)],d2, I/sig1,ordering[X(1)], (-1)*I/sig1,sigma2);
 | 
						|
 | 
						|
	double sig2 = 0.0894427;
 | 
						|
	Vector d3 = Vector_(2, 0.2, -0.14)/sig2, sigma3 = ones(2);
 | 
						|
	push_front(expected,ordering[X(2)],d3, I/sig2,ordering[L(1)], (-0.2)*I/sig2, ordering[X(1)], (-0.8)*I/sig2, sigma3);
 | 
						|
 | 
						|
	// Check one ordering
 | 
						|
	GaussianFactorGraph fg1 = createGaussianFactorGraph(ordering);
 | 
						|
	GaussianBayesNet actual = *GaussianSequentialSolver(fg1).eliminate();
 | 
						|
	EXPECT(assert_equal(expected,actual,tol));
 | 
						|
 | 
						|
  GaussianBayesNet actualQR = *GaussianSequentialSolver(fg1, true).eliminate();
 | 
						|
  EXPECT(assert_equal(expected,actualQR,tol));
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST( GaussianFactorGraph, copying )
 | 
						|
{
 | 
						|
  // Create a graph
 | 
						|
  Ordering ordering; ordering += X(2),L(1),X(1);
 | 
						|
  GaussianFactorGraph actual = createGaussianFactorGraph(ordering);
 | 
						|
 | 
						|
  // Copy the graph !
 | 
						|
  GaussianFactorGraph copy = actual;
 | 
						|
 | 
						|
  // now eliminate the copy
 | 
						|
  GaussianBayesNet actual1 = *GaussianSequentialSolver(copy).eliminate();
 | 
						|
 | 
						|
  // Create the same graph, but not by copying
 | 
						|
  GaussianFactorGraph expected = createGaussianFactorGraph(ordering);
 | 
						|
 | 
						|
  // and check that original is still the same graph
 | 
						|
  EXPECT(assert_equal(expected,actual));
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet )
 | 
						|
{
 | 
						|
  Ordering ord;
 | 
						|
  ord += X(2),L(1),X(1);
 | 
						|
  GaussianFactorGraph fg = createGaussianFactorGraph(ord);
 | 
						|
 | 
						|
  // render with a given ordering
 | 
						|
  GaussianBayesNet CBN = *GaussianSequentialSolver(fg).eliminate();
 | 
						|
 | 
						|
  // True GaussianFactorGraph
 | 
						|
  GaussianFactorGraph fg2(CBN);
 | 
						|
  GaussianBayesNet CBN2 = *GaussianSequentialSolver(fg2).eliminate();
 | 
						|
  EXPECT(assert_equal(CBN,CBN2));
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST( GaussianFactorGraph, getOrdering)
 | 
						|
{
 | 
						|
  Ordering original; original += L(1),X(1),X(2);
 | 
						|
  FactorGraph<IndexFactor> symbolic(createGaussianFactorGraph(original));
 | 
						|
  Permutation perm(*inference::PermutationCOLAMD(VariableIndex(symbolic)));
 | 
						|
  Ordering actual = original; actual.permuteWithInverse((*perm.inverse()));
 | 
						|
  Ordering expected; expected += L(1),X(2),X(1);
 | 
						|
  EXPECT(assert_equal(expected,actual));
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST( GaussianFactorGraph, optimize_Cholesky )
 | 
						|
{
 | 
						|
  // create an ordering
 | 
						|
  Ordering ord; ord += X(2),L(1),X(1);
 | 
						|
 | 
						|
  // create a graph
 | 
						|
	GaussianFactorGraph fg = createGaussianFactorGraph(ord);
 | 
						|
 | 
						|
	// optimize the graph
 | 
						|
	VectorValues actual = *GaussianSequentialSolver(fg, false).optimize();
 | 
						|
 | 
						|
	// verify
 | 
						|
	VectorValues expected = createCorrectDelta(ord);
 | 
						|
 | 
						|
  EXPECT(assert_equal(expected,actual));
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST( GaussianFactorGraph, optimize_QR )
 | 
						|
{
 | 
						|
  // create an ordering
 | 
						|
  Ordering ord; ord += X(2),L(1),X(1);
 | 
						|
 | 
						|
  // create a graph
 | 
						|
	GaussianFactorGraph fg = createGaussianFactorGraph(ord);
 | 
						|
 | 
						|
	// optimize the graph
 | 
						|
	VectorValues actual = *GaussianSequentialSolver(fg, true).optimize();
 | 
						|
 | 
						|
	// verify
 | 
						|
	VectorValues expected = createCorrectDelta(ord);
 | 
						|
 | 
						|
  EXPECT(assert_equal(expected,actual));
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST( GaussianFactorGraph, combine)
 | 
						|
{
 | 
						|
  // create an ordering
 | 
						|
  Ordering ord; ord += X(2),L(1),X(1);
 | 
						|
 | 
						|
  // create a test graph
 | 
						|
	GaussianFactorGraph fg1 = createGaussianFactorGraph(ord);
 | 
						|
 | 
						|
	// create another factor graph
 | 
						|
	GaussianFactorGraph fg2 = createGaussianFactorGraph(ord);
 | 
						|
 | 
						|
	// get sizes
 | 
						|
	size_t size1 = fg1.size();
 | 
						|
	size_t size2 = fg2.size();
 | 
						|
 | 
						|
	// combine them
 | 
						|
	fg1.combine(fg2);
 | 
						|
 | 
						|
	EXPECT(size1+size2 == fg1.size());
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST( GaussianFactorGraph, combine2)
 | 
						|
{
 | 
						|
  // create an ordering
 | 
						|
  Ordering ord; ord += X(2),L(1),X(1);
 | 
						|
 | 
						|
	// create a test graph
 | 
						|
	GaussianFactorGraph fg1 = createGaussianFactorGraph(ord);
 | 
						|
 | 
						|
	// create another factor graph
 | 
						|
	GaussianFactorGraph fg2 = createGaussianFactorGraph(ord);
 | 
						|
 | 
						|
	// get sizes
 | 
						|
	size_t size1 = fg1.size();
 | 
						|
	size_t size2 = fg2.size();
 | 
						|
 | 
						|
	// combine them
 | 
						|
	GaussianFactorGraph fg3 = GaussianFactorGraph::combine2(fg1, fg2);
 | 
						|
 | 
						|
	EXPECT(size1+size2 == fg3.size());
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
// print a vector of ints if needed for debugging
 | 
						|
void print(vector<int> v) {
 | 
						|
	for (size_t k = 0; k < v.size(); k++)
 | 
						|
		cout << v[k] << " ";
 | 
						|
	cout << endl;
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST(GaussianFactorGraph, createSmoother)
 | 
						|
{
 | 
						|
	GaussianFactorGraph fg1 = createSmoother(2).first;
 | 
						|
	LONGS_EQUAL(3,fg1.size());
 | 
						|
	GaussianFactorGraph fg2 = createSmoother(3).first;
 | 
						|
	LONGS_EQUAL(5,fg2.size());
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
double error(const VectorValues& x) {
 | 
						|
  // create an ordering
 | 
						|
  Ordering ord; ord += X(2),L(1),X(1);
 | 
						|
 | 
						|
	GaussianFactorGraph fg = createGaussianFactorGraph(ord);
 | 
						|
	return fg.error(x);
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST( GaussianFactorGraph, multiplication )
 | 
						|
{
 | 
						|
  // create an ordering
 | 
						|
  Ordering ord; ord += X(2),L(1),X(1);
 | 
						|
 | 
						|
  GaussianFactorGraph A = createGaussianFactorGraph(ord);
 | 
						|
  VectorValues x = createCorrectDelta(ord);
 | 
						|
  Errors actual = A * x;
 | 
						|
  Errors expected;
 | 
						|
  expected += Vector_(2,-1.0,-1.0);
 | 
						|
  expected += Vector_(2, 2.0,-1.0);
 | 
						|
  expected += Vector_(2, 0.0, 1.0);
 | 
						|
  expected += Vector_(2,-1.0, 1.5);
 | 
						|
	EXPECT(assert_equal(expected,actual));
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
// Extra test on elimination prompted by Michael's email to Frank 1/4/2010
 | 
						|
TEST( GaussianFactorGraph, elimination )
 | 
						|
{
 | 
						|
  Ordering ord;
 | 
						|
  ord += X(1), X(2);
 | 
						|
	// Create Gaussian Factor Graph
 | 
						|
	GaussianFactorGraph fg;
 | 
						|
	Matrix Ap = eye(1), An = eye(1) * -1;
 | 
						|
	Vector b = Vector_(1, 0.0);
 | 
						|
  SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,2.0);
 | 
						|
	fg.add(ord[X(1)], An, ord[X(2)], Ap, b, sigma);
 | 
						|
	fg.add(ord[X(1)], Ap, b, sigma);
 | 
						|
	fg.add(ord[X(2)], Ap, b, sigma);
 | 
						|
 | 
						|
	// Eliminate
 | 
						|
	GaussianBayesNet bayesNet = *GaussianSequentialSolver(fg).eliminate();
 | 
						|
 | 
						|
	// Check sigma
 | 
						|
	EXPECT_DOUBLES_EQUAL(1.0,bayesNet[ord[X(2)]]->get_sigmas()(0),1e-5);
 | 
						|
 | 
						|
	// Check matrix
 | 
						|
	Matrix R;Vector d;
 | 
						|
	boost::tie(R,d) = matrix(bayesNet);
 | 
						|
	Matrix expected = Matrix_(2,2,
 | 
						|
			0.707107,	-0.353553,
 | 
						|
			0.0,	 0.612372);
 | 
						|
	Matrix expected2 = Matrix_(2,2,
 | 
						|
			0.707107,	-0.353553,
 | 
						|
			0.0,	 -0.612372);
 | 
						|
	EXPECT(equal_with_abs_tol(expected, R, 1e-6) || equal_with_abs_tol(expected2, R, 1e-6));
 | 
						|
}
 | 
						|
 | 
						|
 /* ************************************************************************* */
 | 
						|
// Tests ported from ConstrainedGaussianFactorGraph
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST( GaussianFactorGraph, constrained_simple )
 | 
						|
{
 | 
						|
	// get a graph with a constraint in it
 | 
						|
	GaussianFactorGraph fg = createSimpleConstraintGraph();
 | 
						|
	EXPECT(hasConstraints(fg));
 | 
						|
 | 
						|
 | 
						|
	// eliminate and solve
 | 
						|
	VectorValues actual = *GaussianSequentialSolver(fg).optimize();
 | 
						|
 | 
						|
	// verify
 | 
						|
	VectorValues expected = createSimpleConstraintValues();
 | 
						|
	EXPECT(assert_equal(expected, actual));
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST( GaussianFactorGraph, constrained_single )
 | 
						|
{
 | 
						|
	// get a graph with a constraint in it
 | 
						|
	GaussianFactorGraph fg = createSingleConstraintGraph();
 | 
						|
	EXPECT(hasConstraints(fg));
 | 
						|
 | 
						|
	// eliminate and solve
 | 
						|
	VectorValues actual = *GaussianSequentialSolver(fg).optimize();
 | 
						|
 | 
						|
	// verify
 | 
						|
	VectorValues expected = createSingleConstraintValues();
 | 
						|
	EXPECT(assert_equal(expected, actual));
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST( GaussianFactorGraph, constrained_multi1 )
 | 
						|
{
 | 
						|
	// get a graph with a constraint in it
 | 
						|
	GaussianFactorGraph fg = createMultiConstraintGraph();
 | 
						|
	EXPECT(hasConstraints(fg));
 | 
						|
 | 
						|
	// eliminate and solve
 | 
						|
  VectorValues actual = *GaussianSequentialSolver(fg).optimize();
 | 
						|
 | 
						|
	// verify
 | 
						|
	VectorValues expected = createMultiConstraintValues();
 | 
						|
	EXPECT(assert_equal(expected, actual));
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
 | 
						|
static SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1);
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST(GaussianFactorGraph, replace)
 | 
						|
{
 | 
						|
  Ordering ord; ord += X(1),X(2),X(3),X(4),X(5),X(6);
 | 
						|
	SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0));
 | 
						|
 | 
						|
	GaussianFactorGraph::sharedFactor f1(new JacobianFactor(
 | 
						|
	    ord[X(1)], eye(3,3), ord[X(2)], eye(3,3), zero(3), noise));
 | 
						|
	GaussianFactorGraph::sharedFactor f2(new JacobianFactor(
 | 
						|
	    ord[X(2)], eye(3,3), ord[X(3)], eye(3,3), zero(3), noise));
 | 
						|
	GaussianFactorGraph::sharedFactor f3(new JacobianFactor(
 | 
						|
	    ord[X(3)], eye(3,3), ord[X(4)], eye(3,3), zero(3), noise));
 | 
						|
	GaussianFactorGraph::sharedFactor f4(new JacobianFactor(
 | 
						|
	    ord[X(5)], eye(3,3), ord[X(6)], eye(3,3), zero(3), noise));
 | 
						|
 | 
						|
	GaussianFactorGraph actual;
 | 
						|
	actual.push_back(f1);
 | 
						|
	actual.push_back(f2);
 | 
						|
	actual.push_back(f3);
 | 
						|
	actual.replace(0, f4);
 | 
						|
 | 
						|
	GaussianFactorGraph expected;
 | 
						|
	expected.push_back(f4);
 | 
						|
	expected.push_back(f2);
 | 
						|
	expected.push_back(f3);
 | 
						|
 | 
						|
	EXPECT(assert_equal(expected, actual));
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST(GaussianFactorGraph, createSmoother2)
 | 
						|
{
 | 
						|
  using namespace example;
 | 
						|
  GaussianFactorGraph fg2;
 | 
						|
  Ordering ordering;
 | 
						|
  boost::tie(fg2,ordering) = createSmoother(3);
 | 
						|
  LONGS_EQUAL(5,fg2.size());
 | 
						|
 | 
						|
  // eliminate
 | 
						|
  vector<Index> x3var; x3var.push_back(ordering[X(3)]);
 | 
						|
  vector<Index> x1var; x1var.push_back(ordering[X(1)]);
 | 
						|
  GaussianBayesNet p_x3 = *GaussianSequentialSolver(
 | 
						|
      *GaussianSequentialSolver(fg2).jointFactorGraph(x3var)).eliminate();
 | 
						|
  GaussianBayesNet p_x1 = *GaussianSequentialSolver(
 | 
						|
      *GaussianSequentialSolver(fg2).jointFactorGraph(x1var)).eliminate();
 | 
						|
  CHECK(assert_equal(*p_x1.back(),*p_x3.front())); // should be the same because of symmetry
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
TEST(GaussianFactorGraph, hasConstraints)
 | 
						|
{
 | 
						|
	FactorGraph<GaussianFactor> fgc1 = createMultiConstraintGraph();
 | 
						|
	EXPECT(hasConstraints(fgc1));
 | 
						|
 | 
						|
	FactorGraph<GaussianFactor> fgc2 = createSimpleConstraintGraph() ;
 | 
						|
	EXPECT(hasConstraints(fgc2));
 | 
						|
 | 
						|
	Ordering ordering; ordering += X(1), X(2), L(1);
 | 
						|
	GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 | 
						|
	EXPECT(!hasConstraints(fg));
 | 
						|
}
 | 
						|
 | 
						|
/* ************************************************************************* */
 | 
						|
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 | 
						|
/* ************************************************************************* */
 |