| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | /**
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  |  *  @file   testGaussianFactor.cpp | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  |  *  @brief  Unit tests for Linear Factor | 
					
						
							|  |  |  |  *  @author Christian Potthast | 
					
						
							| 
									
										
										
										
											2009-10-23 01:23:24 +08:00
										 |  |  |  *  @author Frank Dellaert | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  |  **/ | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | #include <iostream>
 | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | #include <boost/tuple/tuple.hpp>
 | 
					
						
							| 
									
										
										
										
											2009-10-31 23:24:22 +08:00
										 |  |  | #include <boost/assign/std/list.hpp> // for operator +=
 | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | #include <boost/assign/std/set.hpp>
 | 
					
						
							| 
									
										
										
										
											2009-11-06 13:43:03 +08:00
										 |  |  | #include <boost/assign/std/map.hpp> // for insert
 | 
					
						
							| 
									
										
										
										
											2009-10-31 23:24:22 +08:00
										 |  |  | using namespace boost::assign; | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | #include <CppUnitLite/TestHarness.h>
 | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | #include "Matrix.h"
 | 
					
						
							| 
									
										
										
										
											2009-11-02 11:50:30 +08:00
										 |  |  | #include "Ordering.h"
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:41:18 +08:00
										 |  |  | #include "GaussianConditional.h"
 | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | #include "smallExample.h"
 | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-10-23 01:23:24 +08:00
										 |  |  | using namespace std; | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | using namespace gtsam; | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | /* ************************************************************************* */ | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | TEST( GaussianFactor, linearFactor ) | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | { | 
					
						
							| 
									
										
										
										
											2009-12-27 20:13:31 +08:00
										 |  |  | 	Matrix I = eye(2); | 
					
						
							|  |  |  | 	Vector b = Vector_(2,0.2,-0.1); | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	double sigma = 0.1; | 
					
						
							| 
									
										
										
										
											2009-12-27 20:13:31 +08:00
										 |  |  | 	GaussianFactor expected("x1", -I, "x2", I, b, sigma); | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// create a small linear factor graph
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactorGraph fg = createGaussianFactorGraph(); | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// get the factor "f2" from the factor graph
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor::shared_ptr lf = fg[1]; | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// check if the two factors are the same
 | 
					
						
							|  |  |  | 	CHECK(assert_equal(expected,*lf)); | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | } | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-12-27 20:13:31 +08:00
										 |  |  | /* ************************************************************************* */ | 
					
						
							|  |  |  | TEST( GaussianFactor, operators ) | 
					
						
							|  |  |  | { | 
					
						
							|  |  |  | 	Matrix I = eye(2); | 
					
						
							|  |  |  | 	Vector b = Vector_(2,0.2,-0.1); | 
					
						
							|  |  |  | 	double sigma = 0.1; | 
					
						
							|  |  |  | 	GaussianFactor lf("x1", -I, "x2", I, b, sigma); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	VectorConfig c; | 
					
						
							|  |  |  | 	c.insert("x1",Vector_(2,10.,20.)); | 
					
						
							|  |  |  | 	c.insert("x2",Vector_(2,30.,60.)); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// test A*x
 | 
					
						
							|  |  |  | 	Vector expectedE = Vector_(2,200.,400.), e = lf*c; | 
					
						
							|  |  |  | 	CHECK(assert_equal(expectedE,e)); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// test A^e
 | 
					
						
							|  |  |  | 	VectorConfig expectedX; | 
					
						
							|  |  |  | 	expectedX.insert("x1",Vector_(2,-2000.,-4000.)); | 
					
						
							|  |  |  | 	expectedX.insert("x2",Vector_(2, 2000., 4000.)); | 
					
						
							|  |  |  | 	CHECK(assert_equal(expectedX,lf^e)); | 
					
						
							|  |  |  | } | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-09-12 11:50:44 +08:00
										 |  |  | /* ************************************************************************* */ | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | TEST( GaussianFactor, keys ) | 
					
						
							| 
									
										
										
										
											2009-09-12 11:50:44 +08:00
										 |  |  | { | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	// get the factor "f2" from the small linear factor graph
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactorGraph fg = createGaussianFactorGraph(); | 
					
						
							|  |  |  | 	GaussianFactor::shared_ptr lf = fg[1]; | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	list<string> expected; | 
					
						
							|  |  |  | 	expected.push_back("x1"); | 
					
						
							|  |  |  | 	expected.push_back("x2"); | 
					
						
							|  |  |  | 	CHECK(lf->keys() == expected); | 
					
						
							| 
									
										
										
										
											2009-09-12 11:50:44 +08:00
										 |  |  | } | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | /* ************************************************************************* */ | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | TEST( GaussianFactor, dimensions ) | 
					
						
							| 
									
										
										
										
											2009-11-12 03:00:39 +08:00
										 |  |  | { | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  |   // get the factor "f2" from the small linear factor graph
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  |   GaussianFactorGraph fg = createGaussianFactorGraph(); | 
					
						
							| 
									
										
										
										
											2009-11-06 13:43:03 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  |   // Check a single factor
 | 
					
						
							|  |  |  |   Dimensions expected; | 
					
						
							|  |  |  |   insert(expected)("x1", 2)("x2", 2); | 
					
						
							|  |  |  |   Dimensions actual = fg[1]->dimensions(); | 
					
						
							|  |  |  |   CHECK(expected==actual); | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | } | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | /* ************************************************************************* */ | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | TEST( GaussianFactor, getDim ) | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | { | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	// get a factor
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactorGraph fg = createGaussianFactorGraph(); | 
					
						
							|  |  |  | 	GaussianFactor::shared_ptr factor = fg[0]; | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	// get the size of a variable
 | 
					
						
							|  |  |  | 	size_t actual = factor->getDim("x1"); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// verify
 | 
					
						
							|  |  |  | 	size_t expected = 2; | 
					
						
							|  |  |  | 	CHECK(actual == expected); | 
					
						
							|  |  |  | } | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | /* ************************************************************************* */ | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | TEST( GaussianFactor, combine ) | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | { | 
					
						
							|  |  |  | 	// create a small linear factor graph
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactorGraph fg = createGaussianFactorGraph(); | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	// get two factors from it and insert the factors into a vector
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	vector<GaussianFactor::shared_ptr> lfg; | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	lfg.push_back(fg[4 - 1]); | 
					
						
							|  |  |  | 	lfg.push_back(fg[2 - 1]); | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	// combine in a factor
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor combined(lfg); | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// sigmas
 | 
					
						
							|  |  |  | 	double sigma2 = 0.1; | 
					
						
							|  |  |  | 	double sigma4 = 0.2; | 
					
						
							|  |  |  | 	Vector sigmas = Vector_(4, sigma4, sigma4, sigma2, sigma2); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// the expected combined linear factor
 | 
					
						
							|  |  |  | 	Matrix Ax2 = Matrix_(4, 2, // x2
 | 
					
						
							|  |  |  | 			-1., 0., | 
					
						
							|  |  |  | 			+0., -1., | 
					
						
							|  |  |  | 			1., 0., | 
					
						
							|  |  |  | 			+0., 1.); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	Matrix Al1 = Matrix_(4, 2,	// l1
 | 
					
						
							|  |  |  | 			1., 0., | 
					
						
							|  |  |  | 			0., 1., | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | 			0., 0., | 
					
						
							|  |  |  | 			0., 0.); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	Matrix Ax1 = Matrix_(4, 2,	// x1
 | 
					
						
							|  |  |  | 			0.00, 0., // f4
 | 
					
						
							|  |  |  | 			0.00, 0., // f4
 | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 			-1., 0., // f2
 | 
					
						
							|  |  |  | 			0.00, -1. // f2
 | 
					
						
							|  |  |  | 	); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// the RHS
 | 
					
						
							|  |  |  | 	Vector b2(4); | 
					
						
							|  |  |  | 	b2(0) = -0.2; | 
					
						
							|  |  |  | 	b2(1) = 0.3; | 
					
						
							|  |  |  | 	b2(2) = 0.2; | 
					
						
							|  |  |  | 	b2(3) = -0.1; | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// use general constructor for making arbitrary factors
 | 
					
						
							|  |  |  | 	vector<pair<string, Matrix> > meas; | 
					
						
							|  |  |  | 	meas.push_back(make_pair("x2", Ax2)); | 
					
						
							|  |  |  | 	meas.push_back(make_pair("l1", Al1)); | 
					
						
							|  |  |  | 	meas.push_back(make_pair("x1", Ax1)); | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor expected(meas, b2, sigmas); | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	CHECK(assert_equal(expected,combined)); | 
					
						
							| 
									
										
										
										
											2009-10-23 01:23:24 +08:00
										 |  |  | } | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | /* ************************************************************************* */ | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | TEST( NonlinearFactorGraph, combine2){ | 
					
						
							|  |  |  | 	double sigma1 = 0.0957; | 
					
						
							| 
									
										
										
										
											2009-10-23 01:23:24 +08:00
										 |  |  | 	Matrix A11(2,2); | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	A11(0,0) = 1; A11(0,1) =  0; | 
					
						
							|  |  |  | 	A11(1,0) = 0;       A11(1,1) = 1; | 
					
						
							| 
									
										
										
										
											2009-10-23 01:23:24 +08:00
										 |  |  | 	Vector b(2); | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	b(0) = 2; b(1) = -1; | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor::shared_ptr f1(new GaussianFactor("x1", A11, b*sigma1, sigma1)); | 
					
						
							| 
									
										
										
										
											2009-10-23 01:23:24 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	double sigma2 = 0.5; | 
					
						
							|  |  |  | 	A11(0,0) = 1; A11(0,1) =  0; | 
					
						
							|  |  |  | 	A11(1,0) = 0; A11(1,1) = -1; | 
					
						
							| 
									
										
										
										
											2009-10-23 01:23:24 +08:00
										 |  |  | 	b(0) = 4 ; b(1) = -5; | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor::shared_ptr f2(new GaussianFactor("x1", A11, b*sigma2, sigma2)); | 
					
						
							| 
									
										
										
										
											2009-10-23 01:23:24 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	double sigma3 = 0.25; | 
					
						
							|  |  |  | 	A11(0,0) = 1; A11(0,1) =  0; | 
					
						
							|  |  |  | 	A11(1,0) = 0; A11(1,1) = -1; | 
					
						
							| 
									
										
										
										
											2009-10-23 01:23:24 +08:00
										 |  |  | 	b(0) = 3 ; b(1) = -88; | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor::shared_ptr f3(new GaussianFactor("x1", A11, b*sigma3, sigma3)); | 
					
						
							| 
									
										
										
										
											2009-10-23 01:23:24 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	// TODO: find a real sigma value for this example
 | 
					
						
							|  |  |  | 	double sigma4 = 0.1; | 
					
						
							| 
									
										
										
										
											2009-10-23 01:23:24 +08:00
										 |  |  | 	A11(0,0) = 6; A11(0,1) =  0; | 
					
						
							|  |  |  | 	A11(1,0) = 0; A11(1,1) = 7; | 
					
						
							|  |  |  | 	b(0) = 5 ; b(1) = -6; | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor::shared_ptr f4(new GaussianFactor("x1", A11*sigma4, b*sigma4, sigma4)); | 
					
						
							| 
									
										
										
										
											2009-10-23 01:23:24 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	vector<GaussianFactor::shared_ptr> lfg; | 
					
						
							| 
									
										
										
										
											2009-10-23 01:23:24 +08:00
										 |  |  | 	lfg.push_back(f1); | 
					
						
							|  |  |  | 	lfg.push_back(f2); | 
					
						
							|  |  |  | 	lfg.push_back(f3); | 
					
						
							|  |  |  | 	lfg.push_back(f4); | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor combined(lfg); | 
					
						
							| 
									
										
										
										
											2009-10-23 01:23:24 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	Vector sigmas = Vector_(8, sigma1, sigma1, sigma2, sigma2, sigma3, sigma3, sigma4, sigma4); | 
					
						
							| 
									
										
										
										
											2009-10-23 01:23:24 +08:00
										 |  |  | 	Matrix A22(8,2); | 
					
						
							| 
									
										
										
										
											2009-12-27 20:13:31 +08:00
										 |  |  | 	A22(0,0) = 1;   A22(0,1) =  0; | 
					
						
							|  |  |  | 	A22(1,0) = 0;   A22(1,1) = 1; | 
					
						
							|  |  |  | 	A22(2,0) = 1;   A22(2,1) =  0; | 
					
						
							|  |  |  | 	A22(3,0) = 0;   A22(3,1) = -1; | 
					
						
							|  |  |  | 	A22(4,0) = 1;   A22(4,1) =  0; | 
					
						
							|  |  |  | 	A22(5,0) = 0;   A22(5,1) = -1; | 
					
						
							|  |  |  | 	A22(6,0) = 0.6; A22(6,1) =  0; | 
					
						
							|  |  |  | 	A22(7,0) = 0;   A22(7,1) =  0.7; | 
					
						
							| 
									
										
										
										
											2009-10-23 01:23:24 +08:00
										 |  |  | 	Vector exb(8); | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	exb(0) = 2*sigma1 ; exb(1) = -1*sigma1;  exb(2) = 4*sigma2 ; exb(3) = -5*sigma2; | 
					
						
							|  |  |  | 	exb(4) = 3*sigma3 ; exb(5) = -88*sigma3; exb(6) = 5*sigma4 ; exb(7) = -6*sigma4; | 
					
						
							| 
									
										
										
										
											2009-10-23 01:23:24 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	vector<pair<string, Matrix> > meas; | 
					
						
							|  |  |  | 	meas.push_back(make_pair("x1", A22)); | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor expected(meas, exb, sigmas); | 
					
						
							| 
									
										
										
										
											2009-10-23 01:23:24 +08:00
										 |  |  | 	CHECK(assert_equal(expected,combined)); | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | } | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | /* ************************************************************************* */ | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | TEST( GaussianFactor, linearFactorN){ | 
					
						
							|  |  |  |   vector<GaussianFactor::shared_ptr> f; | 
					
						
							|  |  |  |   f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", Matrix_(2,2, | 
					
						
							| 
									
										
										
										
											2009-10-22 23:52:39 +08:00
										 |  |  |       1.0, 0.0, | 
					
						
							|  |  |  |       0.0, 1.0), | 
					
						
							|  |  |  |       Vector_(2, | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  |       10.0, 5.0), 1))); | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  |   f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", Matrix_(2,2, | 
					
						
							| 
									
										
										
										
											2009-10-22 23:52:39 +08:00
										 |  |  |       -10.0, 0.0, | 
					
						
							|  |  |  |       0.0, -10.0), | 
					
						
							|  |  |  |       "x2", Matrix_(2,2, | 
					
						
							|  |  |  |       10.0, 0.0, | 
					
						
							|  |  |  |       0.0, 10.0), | 
					
						
							|  |  |  |       Vector_(2, | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  |       1.0, -2.0), 1))); | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  |   f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x2", Matrix_(2,2, | 
					
						
							| 
									
										
										
										
											2009-10-22 23:52:39 +08:00
										 |  |  |       -10.0, 0.0, | 
					
						
							|  |  |  |       0.0, -10.0), | 
					
						
							|  |  |  |       "x3", Matrix_(2,2, | 
					
						
							|  |  |  |       10.0, 0.0, | 
					
						
							|  |  |  |       0.0, 10.0), | 
					
						
							|  |  |  |       Vector_(2, | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  |       1.5, -1.5), 1))); | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  |   f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x3", Matrix_(2,2, | 
					
						
							| 
									
										
										
										
											2009-10-22 23:52:39 +08:00
										 |  |  |       -10.0, 0.0, | 
					
						
							|  |  |  |       0.0, -10.0), | 
					
						
							|  |  |  |       "x4", Matrix_(2,2, | 
					
						
							|  |  |  |       10.0, 0.0, | 
					
						
							|  |  |  |       0.0, 10.0), | 
					
						
							|  |  |  |       Vector_(2, | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  |       2.0, -1.0), 1))); | 
					
						
							| 
									
										
										
										
											2009-10-22 23:52:39 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  |   GaussianFactor combinedFactor(f); | 
					
						
							| 
									
										
										
										
											2009-10-22 23:52:39 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-10-27 23:13:13 +08:00
										 |  |  |   vector<pair<string, Matrix> > combinedMeasurement; | 
					
						
							|  |  |  |   combinedMeasurement.push_back(make_pair("x1", Matrix_(8,2, | 
					
						
							| 
									
										
										
										
											2009-12-27 20:13:31 +08:00
										 |  |  |       1.0,  0.0, | 
					
						
							|  |  |  |       0.0,  1.0, | 
					
						
							|  |  |  |     -10.0,  0.0, | 
					
						
							|  |  |  |       0.0,-10.0, | 
					
						
							|  |  |  |       0.0,  0.0, | 
					
						
							|  |  |  |       0.0,  0.0, | 
					
						
							|  |  |  |       0.0,  0.0, | 
					
						
							|  |  |  |       0.0,  0.0))); | 
					
						
							| 
									
										
										
										
											2009-10-27 23:13:13 +08:00
										 |  |  |   combinedMeasurement.push_back(make_pair("x2", Matrix_(8,2, | 
					
						
							| 
									
										
										
										
											2009-12-27 20:13:31 +08:00
										 |  |  |       0.0,  0.0, | 
					
						
							|  |  |  |       0.0,  0.0, | 
					
						
							|  |  |  |      10.0,  0.0, | 
					
						
							| 
									
										
										
										
											2009-10-22 23:52:39 +08:00
										 |  |  |       0.0, 10.0, | 
					
						
							| 
									
										
										
										
											2009-12-27 20:13:31 +08:00
										 |  |  |     -10.0,  0.0, | 
					
						
							|  |  |  |       0.0,-10.0, | 
					
						
							|  |  |  |       0.0,  0.0, | 
					
						
							|  |  |  |       0.0,  0.0))); | 
					
						
							| 
									
										
										
										
											2009-10-27 23:13:13 +08:00
										 |  |  |   combinedMeasurement.push_back(make_pair("x3", Matrix_(8,2, | 
					
						
							| 
									
										
										
										
											2009-12-27 20:13:31 +08:00
										 |  |  |       0.0,  0.0, | 
					
						
							|  |  |  |       0.0,  0.0, | 
					
						
							|  |  |  |       0.0,  0.0, | 
					
						
							|  |  |  |       0.0,  0.0, | 
					
						
							|  |  |  |      10.0,  0.0, | 
					
						
							| 
									
										
										
										
											2009-10-22 23:52:39 +08:00
										 |  |  |       0.0, 10.0, | 
					
						
							| 
									
										
										
										
											2009-12-27 20:13:31 +08:00
										 |  |  |     -10.0,  0.0, | 
					
						
							|  |  |  |       0.0,-10.0))); | 
					
						
							| 
									
										
										
										
											2009-10-27 23:13:13 +08:00
										 |  |  |   combinedMeasurement.push_back(make_pair("x4", Matrix_(8,2, | 
					
						
							| 
									
										
										
										
											2009-10-22 23:52:39 +08:00
										 |  |  |       0.0, 0.0, | 
					
						
							|  |  |  |       0.0, 0.0, | 
					
						
							|  |  |  |       0.0, 0.0, | 
					
						
							|  |  |  |       0.0, 0.0, | 
					
						
							|  |  |  |       0.0, 0.0, | 
					
						
							|  |  |  |       0.0, 0.0, | 
					
						
							| 
									
										
										
										
											2009-12-27 20:13:31 +08:00
										 |  |  |      10.0, 0.0, | 
					
						
							|  |  |  |       0.0,10.0))); | 
					
						
							| 
									
										
										
										
											2009-10-22 23:52:39 +08:00
										 |  |  |   Vector b = Vector_(8, | 
					
						
							|  |  |  |       10.0, 5.0, 1.0, -2.0, 1.5, -1.5, 2.0, -1.0); | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  |   GaussianFactor expected(combinedMeasurement, b, 1.); | 
					
						
							| 
									
										
										
										
											2009-10-27 23:13:13 +08:00
										 |  |  |   CHECK(combinedFactor.equals(expected)); | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | } | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | /* ************************************************************************* */ | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | TEST( GaussianFactor, error ) | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | { | 
					
						
							|  |  |  | 	// create a small linear factor graph
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactorGraph fg = createGaussianFactorGraph(); | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	// get the first factor from the factor graph
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor::shared_ptr lf = fg[0]; | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	// check the error of the first factor with noisy config
 | 
					
						
							|  |  |  | 	VectorConfig cfg = createZeroDelta(); | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	// calculate the error from the factor "f1"
 | 
					
						
							|  |  |  | 	// note the error is the same as in testNonlinearFactor
 | 
					
						
							|  |  |  | 	double actual = lf->error(cfg); | 
					
						
							|  |  |  | 	DOUBLES_EQUAL( 1.0, actual, 0.00000001 ); | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | } | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | /* ************************************************************************* */ | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | TEST( GaussianFactor, eliminate ) | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | { | 
					
						
							|  |  |  | 	// create a small linear factor graph
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactorGraph fg = createGaussianFactorGraph(); | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	// get two factors from it and insert the factors into a vector
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	vector<GaussianFactor::shared_ptr> lfg; | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	lfg.push_back(fg[4 - 1]); | 
					
						
							|  |  |  | 	lfg.push_back(fg[2 - 1]); | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	// combine in a factor
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor combined(lfg); | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	// eliminate the combined factor
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:41:18 +08:00
										 |  |  | 	GaussianConditional::shared_ptr actualCG; | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor::shared_ptr actualLF; | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	boost::tie(actualCG,actualLF) = combined.eliminate("x2"); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// create expected Conditional Gaussian
 | 
					
						
							|  |  |  | 	Matrix R11 = Matrix_(2,2, | 
					
						
							|  |  |  | 			1.0, 0.0, | 
					
						
							|  |  |  | 			0.0, 1.0 | 
					
						
							|  |  |  | 	); | 
					
						
							|  |  |  | 	Matrix S12 = Matrix_(2,2, | 
					
						
							|  |  |  | 			-0.2, 0.0, | 
					
						
							|  |  |  | 			+0.0,-0.2 | 
					
						
							|  |  |  | 	); | 
					
						
							|  |  |  | 	Matrix S13 = Matrix_(2,2, | 
					
						
							|  |  |  | 			-0.8, 0.0, | 
					
						
							|  |  |  | 			+0.0,-0.8 | 
					
						
							|  |  |  | 	); | 
					
						
							|  |  |  | 	Vector d(2); d(0) = 0.2; d(1) = -0.14; | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 14:59:59 +08:00
										 |  |  | 	Vector sigmas(2); | 
					
						
							|  |  |  | 	sigmas(0) = 1/sqrt(125.0); | 
					
						
							|  |  |  | 	sigmas(1) = 1/sqrt(125.0); | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// Check the conditional Gaussian
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:41:18 +08:00
										 |  |  | 	GaussianConditional expectedCG("x2", d,R11,"l1",S12,"x1",S13,sigmas); | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// the expected linear factor
 | 
					
						
							|  |  |  | 	double sigma = 0.2236; | 
					
						
							|  |  |  | 	Matrix Bl1 = Matrix_(2,2, | 
					
						
							|  |  |  | 			// l1
 | 
					
						
							|  |  |  | 			1.00, 0.00, | 
					
						
							|  |  |  | 			0.00, 1.00 | 
					
						
							|  |  |  | 	); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	Matrix Bx1 = Matrix_(2,2, | 
					
						
							|  |  |  | 			// x1
 | 
					
						
							|  |  |  | 			-1.00,  0.00, | 
					
						
							|  |  |  | 			+0.00, -1.00 | 
					
						
							|  |  |  | 	); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// the RHS
 | 
					
						
							|  |  |  | 	Vector b1(2); b1(0) = 0.0; b1(1) = 0.2; | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor expectedLF("l1", Bl1, "x1", Bx1, b1, sigma); | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// check if the result matches
 | 
					
						
							|  |  |  | 	CHECK(assert_equal(expectedCG,*actualCG,1e-4)); | 
					
						
							|  |  |  | 	CHECK(assert_equal(expectedLF,*actualLF,1e-5)); | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | } | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | /* ************************************************************************* */ | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | TEST( GaussianFactor, eliminate2 ) | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | { | 
					
						
							|  |  |  | 	// sigmas
 | 
					
						
							|  |  |  | 	double sigma1 = 0.2; | 
					
						
							|  |  |  | 	double sigma2 = 0.1; | 
					
						
							|  |  |  | 	Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// the combined linear factor
 | 
					
						
							|  |  |  | 	Matrix Ax2 = Matrix_(4,2, | 
					
						
							|  |  |  | 			// x2
 | 
					
						
							|  |  |  | 			-1., 0., | 
					
						
							|  |  |  | 			+0.,-1., | 
					
						
							|  |  |  | 			1., 0., | 
					
						
							|  |  |  | 			+0.,1. | 
					
						
							|  |  |  | 	); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	Matrix Al1x1 = Matrix_(4,4, | 
					
						
							|  |  |  | 			// l1   x1
 | 
					
						
							|  |  |  | 			1., 0., 0.00,  0., // f4
 | 
					
						
							|  |  |  | 			0., 1., 0.00,  0., // f4
 | 
					
						
							|  |  |  | 			0., 0., -1.,  0., // f2
 | 
					
						
							|  |  |  | 			0., 0., 0.00,-1.  // f2
 | 
					
						
							|  |  |  | 	); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// the RHS
 | 
					
						
							|  |  |  | 	Vector b2(4); | 
					
						
							|  |  |  | 	b2(0) = -0.2; | 
					
						
							|  |  |  | 	b2(1) =  0.3; | 
					
						
							|  |  |  | 	b2(2) =  0.2; | 
					
						
							|  |  |  | 	b2(3) = -0.1; | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	vector<pair<string, Matrix> > meas; | 
					
						
							|  |  |  | 	meas.push_back(make_pair("x2", Ax2)); | 
					
						
							|  |  |  | 	meas.push_back(make_pair("l1x1", Al1x1)); | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor combined(meas, b2, sigmas); | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// eliminate the combined factor
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:41:18 +08:00
										 |  |  | 	GaussianConditional::shared_ptr actualCG; | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor::shared_ptr actualLF; | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | 	boost::tie(actualCG,actualLF) = combined.eliminate("x2"); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// create expected Conditional Gaussian
 | 
					
						
							|  |  |  | 	Matrix R11 = Matrix_(2,2, | 
					
						
							|  |  |  | 			1.00,  0.00, | 
					
						
							|  |  |  | 			0.00,  1.00 | 
					
						
							|  |  |  | 	); | 
					
						
							|  |  |  | 	Matrix S12 = Matrix_(2,4, | 
					
						
							|  |  |  | 			-0.20, 0.00,-0.80, 0.00, | 
					
						
							|  |  |  | 			+0.00,-0.20,+0.00,-0.80 | 
					
						
							|  |  |  | 	); | 
					
						
							|  |  |  | 	Vector d(2); d(0) = 0.2; d(1) = -0.14; | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	Vector x2Sigmas(2); | 
					
						
							|  |  |  | 	x2Sigmas(0) = 0.0894427; | 
					
						
							|  |  |  | 	x2Sigmas(1) = 0.0894427; | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:41:18 +08:00
										 |  |  | 	GaussianConditional expectedCG("x2",d,R11,"l1x1",S12,x2Sigmas); | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// the expected linear factor
 | 
					
						
							|  |  |  | 	double sigma = 0.2236; | 
					
						
							|  |  |  | 	Matrix Bl1x1 = Matrix_(2,4, | 
					
						
							|  |  |  | 			// l1          x1
 | 
					
						
							|  |  |  | 			1.00, 0.00, -1.00,  0.00, | 
					
						
							|  |  |  | 			0.00, 1.00, +0.00, -1.00 | 
					
						
							|  |  |  | 	); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// the RHS
 | 
					
						
							|  |  |  | 	Vector b1(2); b1(0) = 0.0; b1(1) = 0.894427; | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor expectedLF("l1x1", Bl1x1, b1*sigma, sigma); | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// check if the result matches
 | 
					
						
							|  |  |  | 	CHECK(assert_equal(expectedCG,*actualCG,1e-4)); | 
					
						
							|  |  |  | 	CHECK(assert_equal(expectedLF,*actualLF,1e-5)); | 
					
						
							|  |  |  | } | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | /* ************************************************************************* */ | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | TEST( GaussianFactor, default_error ) | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | { | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor f; | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | 	VectorConfig c; | 
					
						
							|  |  |  | 	double actual = f.error(c); | 
					
						
							|  |  |  | 	CHECK(actual==0.0); | 
					
						
							|  |  |  | } | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | //* ************************************************************************* */
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | TEST( GaussianFactor, eliminate_empty ) | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | { | 
					
						
							|  |  |  | 	// create an empty factor
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor f; | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// eliminate the empty factor
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:41:18 +08:00
										 |  |  | 	GaussianConditional::shared_ptr actualCG; | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor::shared_ptr actualLF; | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | 	boost::tie(actualCG,actualLF) = f.eliminate("x2"); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// expected Conditional Gaussian is just a parent-less node with P(x)=1
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:41:18 +08:00
										 |  |  | 	GaussianConditional expectedCG("x2"); | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// expected remaining factor is still empty :-)
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor expectedLF; | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// check if the result matches
 | 
					
						
							|  |  |  | 	CHECK(actualCG->equals(expectedCG)); | 
					
						
							|  |  |  | 	CHECK(actualLF->equals(expectedLF)); | 
					
						
							|  |  |  | } | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | //* ************************************************************************* */
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | TEST( GaussianFactor, empty ) | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | { | 
					
						
							|  |  |  | 	// create an empty factor
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor f; | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | 	CHECK(f.empty()==true); | 
					
						
							|  |  |  | } | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | /* ************************************************************************* */ | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | TEST( GaussianFactor, matrix ) | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | { | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	// create a small linear factor graph
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactorGraph fg = createGaussianFactorGraph(); | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	// get the factor "f2" from the factor graph
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor::shared_ptr lf = fg[1]; | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 23:08:58 +08:00
										 |  |  | 	// render with a given ordering
 | 
					
						
							|  |  |  | 	Ordering ord; | 
					
						
							|  |  |  | 	ord += "x1","x2"; | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	Matrix A; Vector b; | 
					
						
							|  |  |  | 	boost::tie(A,b) = lf->matrix(ord); | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	Matrix A1 = Matrix_(2,4, | 
					
						
							|  |  |  | 			-10.0,  0.0, 10.0,  0.0, | 
					
						
							|  |  |  | 			000.0,-10.0,  0.0, 10.0 ); | 
					
						
							|  |  |  | 	Vector b1 = Vector_(2, 2.0, -1.0); | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 04:59:16 +08:00
										 |  |  | 	EQUALITY(A,A1); | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | 	EQUALITY(b,b1); | 
					
						
							|  |  |  | } | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 23:08:58 +08:00
										 |  |  | /* ************************************************************************* */ | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | TEST( GaussianFactor, matrix_aug ) | 
					
						
							| 
									
										
										
										
											2009-11-05 23:08:58 +08:00
										 |  |  | { | 
					
						
							|  |  |  | 	// create a small linear factor graph
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactorGraph fg = createGaussianFactorGraph(); | 
					
						
							| 
									
										
										
										
											2009-11-05 23:08:58 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// get the factor "f2" from the factor graph
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor::shared_ptr lf = fg[1]; | 
					
						
							| 
									
										
										
										
											2009-11-05 23:08:58 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// render with a given ordering
 | 
					
						
							|  |  |  | 	Ordering ord; | 
					
						
							|  |  |  | 	ord += "x1","x2"; | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	Matrix Ab; | 
					
						
							|  |  |  | 	Ab = lf->matrix_augmented(ord); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	Matrix Ab1 = Matrix_(2,5, | 
					
						
							| 
									
										
										
										
											2009-11-10 05:34:20 +08:00
										 |  |  | 			-1.0,  0.0, 1.0,  0.0,  0.2, | 
					
						
							|  |  |  | 			00.0,- 1.0, 0.0,  1.0, -0.1 ); | 
					
						
							| 
									
										
										
										
											2009-11-05 23:08:58 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	EQUALITY(Ab,Ab1); | 
					
						
							|  |  |  | } | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-06 13:43:03 +08:00
										 |  |  | /* ************************************************************************* */ | 
					
						
							|  |  |  | // small aux. function to print out lists of anything
 | 
					
						
							|  |  |  | template<class T> | 
					
						
							|  |  |  | void print(const list<T>& i) { | 
					
						
							|  |  |  | 	copy(i.begin(), i.end(), ostream_iterator<T> (cout, ",")); | 
					
						
							|  |  |  | 	cout << endl; | 
					
						
							|  |  |  | } | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | /* ************************************************************************* */ | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | TEST( GaussianFactor, sparse ) | 
					
						
							| 
									
										
										
										
											2009-11-06 13:43:03 +08:00
										 |  |  | { | 
					
						
							|  |  |  | 	// create a small linear factor graph
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactorGraph fg = createGaussianFactorGraph(); | 
					
						
							| 
									
										
										
										
											2009-11-06 13:43:03 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// get the factor "f2" from the factor graph
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor::shared_ptr lf = fg[1]; | 
					
						
							| 
									
										
										
										
											2009-11-06 13:43:03 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// render with a given ordering
 | 
					
						
							|  |  |  | 	Ordering ord; | 
					
						
							|  |  |  | 	ord += "x1","x2"; | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	list<int> i,j; | 
					
						
							|  |  |  | 	list<double> s; | 
					
						
							| 
									
										
										
										
											2009-12-12 14:18:29 +08:00
										 |  |  | 	boost::tie(i,j,s) = lf->sparse(fg.columnIndices(ord)); | 
					
						
							| 
									
										
										
										
											2009-11-06 13:43:03 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	list<int> i1,j1; | 
					
						
							|  |  |  | 	i1 += 1,2,1,2; | 
					
						
							|  |  |  | 	j1 += 1,2,3,4; | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	list<double> s1; | 
					
						
							|  |  |  | 	s1 += -10,-10,10,10; | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	CHECK(i==i1); | 
					
						
							|  |  |  | 	CHECK(j==j1); | 
					
						
							|  |  |  | 	CHECK(s==s1); | 
					
						
							|  |  |  | } | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | /* ************************************************************************* */ | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | TEST( GaussianFactor, sparse2 ) | 
					
						
							| 
									
										
										
										
											2009-11-06 13:43:03 +08:00
										 |  |  | { | 
					
						
							|  |  |  | 	// create a small linear factor graph
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactorGraph fg = createGaussianFactorGraph(); | 
					
						
							| 
									
										
										
										
											2009-11-06 13:43:03 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// get the factor "f2" from the factor graph
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor::shared_ptr lf = fg[1]; | 
					
						
							| 
									
										
										
										
											2009-11-06 13:43:03 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// render with a given ordering
 | 
					
						
							|  |  |  | 	Ordering ord; | 
					
						
							|  |  |  | 	ord += "x2","l1","x1"; | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	list<int> i,j; | 
					
						
							|  |  |  | 	list<double> s; | 
					
						
							| 
									
										
										
										
											2009-12-12 14:18:29 +08:00
										 |  |  | 	boost::tie(i,j,s) = lf->sparse(fg.columnIndices(ord)); | 
					
						
							| 
									
										
										
										
											2009-11-06 13:43:03 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	list<int> i1,j1; | 
					
						
							|  |  |  | 	i1 += 1,2,1,2; | 
					
						
							| 
									
										
										
										
											2009-12-12 14:18:29 +08:00
										 |  |  | 	j1 += 5,6,1,2; | 
					
						
							| 
									
										
										
										
											2009-11-06 13:43:03 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	list<double> s1; | 
					
						
							| 
									
										
										
										
											2009-12-12 14:18:29 +08:00
										 |  |  | 	s1 += -10,-10,10,10; | 
					
						
							| 
									
										
										
										
											2009-11-06 13:43:03 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	CHECK(i==i1); | 
					
						
							|  |  |  | 	CHECK(j==j1); | 
					
						
							|  |  |  | 	CHECK(s==s1); | 
					
						
							|  |  |  | } | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | /* ************************************************************************* */ | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | TEST( GaussianFactor, size ) | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | { | 
					
						
							|  |  |  | 	// create a linear factor graph
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactorGraph fg = createGaussianFactorGraph(); | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// get some factors from the graph
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	boost::shared_ptr<GaussianFactor> factor1 = fg[0]; | 
					
						
							|  |  |  | 	boost::shared_ptr<GaussianFactor> factor2 = fg[1]; | 
					
						
							|  |  |  | 	boost::shared_ptr<GaussianFactor> factor3 = fg[2]; | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	CHECK(factor1->size() == 1); | 
					
						
							|  |  |  | 	CHECK(factor2->size() == 2); | 
					
						
							|  |  |  | 	CHECK(factor3->size() == 2); | 
					
						
							|  |  |  | } | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | /* ************************************************************************* */ | 
					
						
							| 
									
										
										
										
											2009-11-13 00:41:18 +08:00
										 |  |  | TEST( GaussianFactor, CONSTRUCTOR_GaussianConditional ) | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | { | 
					
						
							|  |  |  | 	Matrix R11 = Matrix_(2,2, | 
					
						
							|  |  |  | 			1.00,  0.00, | 
					
						
							|  |  |  | 			0.00,  1.00 | 
					
						
							|  |  |  | 	); | 
					
						
							|  |  |  | 	Matrix S12 = Matrix_(2,2, | 
					
						
							|  |  |  | 			-0.200001, 0.00, | 
					
						
							|  |  |  | 			+0.00,-0.200001 | 
					
						
							|  |  |  | 	); | 
					
						
							|  |  |  | 	Vector d(2); d(0) = 2.23607; d(1) = -1.56525; | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	Vector sigmas(2); | 
					
						
							|  |  |  | 	sigmas(0) = 0.29907; | 
					
						
							|  |  |  | 	sigmas(1) = 0.29907; | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:41:18 +08:00
										 |  |  | 	GaussianConditional::shared_ptr CG(new GaussianConditional("x2",d,R11,"l1x1",S12,sigmas)); | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor actualLF(CG); | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | 	//  actualLF.print();
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor expectedLF("x2",R11,"l1x1",S12,d, sigmas(0)); | 
					
						
							| 
									
										
										
										
											2009-11-05 21:52:12 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	CHECK(assert_equal(expectedLF,actualLF,1e-5)); | 
					
						
							|  |  |  | } | 
					
						
							| 
									
										
										
										
											2009-11-10 05:34:20 +08:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-12-12 02:03:43 +08:00
										 |  |  | /* ************************************************************************* */ | 
					
						
							|  |  |  | TEST( GaussianFactor, alphaFactor ) | 
					
						
							|  |  |  | { | 
					
						
							|  |  |  | 	GaussianFactorGraph fg = createGaussianFactorGraph(); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// get alphafactor for first factor in fg at zero, in gradient direction
 | 
					
						
							|  |  |  |   VectorConfig x = createZeroDelta(); | 
					
						
							|  |  |  | 	VectorConfig d = fg.gradient(x); | 
					
						
							|  |  |  | 	GaussianFactor::shared_ptr factor = fg[0]; | 
					
						
							|  |  |  | 	GaussianFactor::shared_ptr actual = factor->alphaFactor(x,d); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// calculate expected
 | 
					
						
							|  |  |  | 	Matrix A = Matrix_(2,1,30.0,5.0); | 
					
						
							|  |  |  | 	Vector b = Vector_(2,-0.1,-0.1); | 
					
						
							|  |  |  | 	Vector sigmas = Vector_(2,0.1,0.1); | 
					
						
							|  |  |  | 	GaussianFactor expected("alpha",A,b,sigmas); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	CHECK(assert_equal(expected,*actual)); | 
					
						
							|  |  |  | } | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-10 05:34:20 +08:00
										 |  |  | /* ************************************************************************* */ | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | TEST ( GaussianFactor, constraint_eliminate1 ) | 
					
						
							| 
									
										
										
										
											2009-11-10 05:34:20 +08:00
										 |  |  | { | 
					
						
							| 
									
										
										
										
											2009-11-10 12:36:07 +08:00
										 |  |  | 	// construct a linear constraint
 | 
					
						
							|  |  |  | 	Vector v(2); v(0)=1.2; v(1)=3.4; | 
					
						
							|  |  |  | 	string key = "x0"; | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor lc(key, eye(2), v, 0.0); | 
					
						
							| 
									
										
										
										
											2009-11-10 12:36:07 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// eliminate it
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:41:18 +08:00
										 |  |  | 	GaussianConditional::shared_ptr actualCG; | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor::shared_ptr actualLF; | 
					
						
							| 
									
										
										
										
											2009-11-10 12:36:07 +08:00
										 |  |  | 	boost::tie(actualCG,actualLF) = lc.eliminate("x0"); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// verify linear factor
 | 
					
						
							|  |  |  | 	CHECK(actualLF->size() == 0); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// verify conditional Gaussian
 | 
					
						
							|  |  |  | 	Vector sigmas = Vector_(2, 0.0, 0.0); | 
					
						
							| 
									
										
										
										
											2009-11-13 00:41:18 +08:00
										 |  |  | 	GaussianConditional expCG("x0", v, eye(2), sigmas); | 
					
						
							| 
									
										
										
										
											2009-11-10 12:36:07 +08:00
										 |  |  | 	CHECK(assert_equal(expCG, *actualCG)); | 
					
						
							| 
									
										
										
										
											2009-11-10 05:34:20 +08:00
										 |  |  | } | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | /* ************************************************************************* */ | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | TEST ( GaussianFactor, constraint_eliminate2 ) | 
					
						
							| 
									
										
										
										
											2009-11-12 03:00:39 +08:00
										 |  |  | { | 
					
						
							|  |  |  | 	// Construct a linear constraint
 | 
					
						
							|  |  |  | 	// RHS
 | 
					
						
							|  |  |  | 	Vector b(2); b(0)=3.0; b(1)=4.0; | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// A1 - invertible
 | 
					
						
							|  |  |  | 	Matrix A1(2,2); | 
					
						
							|  |  |  | 	A1(0,0) = 1.0 ; A1(0,1) = 2.0; | 
					
						
							|  |  |  | 	A1(1,0) = 2.0 ; A1(1,1) = 1.0; | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// A2 - not invertible
 | 
					
						
							|  |  |  | 	Matrix A2(2,2); | 
					
						
							|  |  |  | 	A2(0,0) = 1.0 ; A2(0,1) = 2.0; | 
					
						
							|  |  |  | 	A2(1,0) = 2.0 ; A2(1,1) = 4.0; | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor lc("x", A1, "y", A2, b, 0.0); | 
					
						
							| 
									
										
										
										
											2009-11-12 03:00:39 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// eliminate x and verify results
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:41:18 +08:00
										 |  |  | 	GaussianConditional::shared_ptr actualCG; | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor::shared_ptr actualLF; | 
					
						
							| 
									
										
										
										
											2009-11-12 03:00:39 +08:00
										 |  |  | 	boost::tie(actualCG, actualLF) = lc.eliminate("x"); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// LF should be null
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor expectedLF; | 
					
						
							| 
									
										
										
										
											2009-11-12 03:00:39 +08:00
										 |  |  | 	CHECK(assert_equal(*actualLF, expectedLF)); | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// verify CG
 | 
					
						
							|  |  |  | 	Matrix R = Matrix_(2, 2, | 
					
						
							|  |  |  | 			1.0,    2.0, | 
					
						
							|  |  |  | 			0.0,    1.0); | 
					
						
							|  |  |  | 	Matrix S = Matrix_(2,2, | 
					
						
							|  |  |  | 			1.0,    2.0, | 
					
						
							|  |  |  | 			0.0,    0.0); | 
					
						
							|  |  |  | 	Vector d = Vector_(2, 3.0, 0.6666); | 
					
						
							| 
									
										
										
										
											2009-11-13 00:41:18 +08:00
										 |  |  | 	GaussianConditional expectedCG("x", d, R, "y", S, zero(2)); | 
					
						
							| 
									
										
										
										
											2009-11-12 03:00:39 +08:00
										 |  |  | 	CHECK(assert_equal(expectedCG, *actualCG, 1e-4)); | 
					
						
							|  |  |  | } | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-13 14:17:19 +08:00
										 |  |  | /* ************************************************************************* *
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | TEST ( GaussianFactor, constraint_eliminate3 ) | 
					
						
							| 
									
										
										
										
											2009-11-12 03:00:39 +08:00
										 |  |  | { | 
					
						
							| 
									
										
										
										
											2009-11-13 14:17:19 +08:00
										 |  |  | 	// This test shows that ordering matters if there are non-invertible
 | 
					
						
							| 
									
										
										
										
											2009-11-12 03:00:39 +08:00
										 |  |  | 	// blocks, as this example can be eliminated if x is first, but not
 | 
					
						
							|  |  |  | 	// if y is first.
 | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// Construct a linear constraint
 | 
					
						
							|  |  |  | 	// RHS
 | 
					
						
							|  |  |  | 	Vector b(2); b(0)=3.0; b(1)=4.0; | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// A1 - invertible
 | 
					
						
							|  |  |  | 	Matrix A1(2,2); | 
					
						
							|  |  |  | 	A1(0,0) = 1.0 ; A1(0,1) = 2.0; | 
					
						
							|  |  |  | 	A1(1,0) = 2.0 ; A1(1,1) = 1.0; | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | 	// A2 - not invertible
 | 
					
						
							|  |  |  | 	Matrix A2(2,2); | 
					
						
							|  |  |  | 	A2(0,0) = 1.0 ; A2(0,1) = 2.0; | 
					
						
							|  |  |  | 	A2(1,0) = 2.0 ; A2(1,1) = 4.0; | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor lc("x", A1, "y", A2, b, 0.0); | 
					
						
							| 
									
										
										
										
											2009-11-12 03:00:39 +08:00
										 |  |  | 
 | 
					
						
							|  |  |  | 	// eliminate y from original graph
 | 
					
						
							|  |  |  | 	// NOTE: this will throw an exception, as
 | 
					
						
							|  |  |  | 	// the leading matrix is rank deficient
 | 
					
						
							| 
									
										
										
										
											2009-11-13 00:41:18 +08:00
										 |  |  | 	GaussianConditional::shared_ptr actualCG; | 
					
						
							| 
									
										
										
										
											2009-11-13 00:16:32 +08:00
										 |  |  | 	GaussianFactor::shared_ptr actualLF; | 
					
						
							| 
									
										
										
										
											2009-11-12 03:00:39 +08:00
										 |  |  | 	try { | 
					
						
							|  |  |  | 		boost::tie(actualCG, actualLF) = lc.eliminate("y"); | 
					
						
							|  |  |  | 		CHECK(false); | 
					
						
							|  |  |  | 	} catch (domain_error) { | 
					
						
							|  |  |  | 		CHECK(true); | 
					
						
							|  |  |  | 	} | 
					
						
							|  |  |  | } | 
					
						
							| 
									
										
										
										
											2009-08-22 06:23:24 +08:00
										 |  |  | /* ************************************************************************* */ | 
					
						
							|  |  |  | int main() { TestResult tr; return TestRegistry::runAllTests(tr);} | 
					
						
							|  |  |  | /* ************************************************************************* */ |