131 lines
3.3 KiB
C++
131 lines
3.3 KiB
C++
/**
|
|
* @file testConstrainedConditionalGaussian.cpp
|
|
* @brief tests for constrained conditional gaussians
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
#include "ConstrainedConditionalGaussian.h"
|
|
|
|
using namespace gtsam;
|
|
|
|
/* ************************************************************************* */
|
|
TEST (ConstrainedConditionalGaussian, basic_unary1 )
|
|
{
|
|
Vector v(2); v(0)=1.0; v(1)=2.0;
|
|
|
|
// check unary constructor that doesn't require an R matrix
|
|
// assumed identity matrix
|
|
ConstrainedConditionalGaussian unary(v);
|
|
FGConfig fg;
|
|
fg.insert("x1", v);
|
|
|
|
CHECK(assert_equal(v, unary.solve(fg)));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST (ConstrainedConditionalGaussian, basic_unary2 )
|
|
{
|
|
Vector v(2); v(0)=1.0; v(1)=2.0;
|
|
|
|
// check unary constructor that makes use of a A matrix
|
|
Matrix A = eye(2) * 10;
|
|
|
|
ConstrainedConditionalGaussian unary(10*v, A);
|
|
FGConfig fg;
|
|
fg.insert("x1", v);
|
|
|
|
CHECK(assert_equal(v, unary.solve(fg)));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST (ConstrainedConditionalGaussian, basic_unary3 )
|
|
{
|
|
Vector v(2); v(0)=1.0; v(1)=2.0;
|
|
|
|
// check unary constructor that makes use of a A matrix
|
|
// where A^(-1) exists, but A is not triangular
|
|
Matrix A(2,2);
|
|
A(0,0) = 1.0 ; A(0,1) = 2.0;
|
|
A(1,0) = 2.0 ; A(1,1) = 1.0;
|
|
|
|
Vector rhs = A*v;
|
|
ConstrainedConditionalGaussian unary(rhs, A);
|
|
FGConfig fg;
|
|
fg.insert("x1", v);
|
|
|
|
CHECK(assert_equal(v, unary.solve(fg)));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST (ConstrainedConditionalGaussian, basic_binary1 )
|
|
{
|
|
// tests x = (A1^(-1))*(b - A2y) case, where A1 is invertible
|
|
// A1 is not already triangular, however
|
|
|
|
// 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 - should still work
|
|
Matrix A2(2,2);
|
|
A2(0,0) = 1.0 ; A2(0,1) = 2.0;
|
|
A2(1,0) = 2.0 ; A2(1,1) = 4.0;
|
|
|
|
Vector y = Vector_(2, 1.0, 2.0);
|
|
|
|
FGConfig fg;
|
|
fg.insert("x1", y);
|
|
|
|
Vector expected = Vector_(2, -3.3333, 0.6667);
|
|
|
|
ConstrainedConditionalGaussian binary(b, A1, "x1", A2);
|
|
|
|
CHECK(assert_equal(expected, binary.solve(fg), 1e-4));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST (ConstrainedConditionalGaussian, basic_ternary1 )
|
|
{
|
|
// tests x = (A1^(-1))*(b - A2*y - A3*z) case, where A1 is invertible
|
|
// A1 is not already triangular, however
|
|
|
|
// 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 - should still work
|
|
Matrix A2(2,2);
|
|
A2(0,0) = 1.0 ; A2(0,1) = 2.0;
|
|
A2(1,0) = 2.0 ; A2(1,1) = 4.0;
|
|
|
|
Matrix A3 = eye(2)*10;
|
|
|
|
Vector y = Vector_(2, 1.0, 2.0);
|
|
Vector z = Vector_(2, 1.0, -1.0);
|
|
|
|
FGConfig fg;
|
|
fg.insert("x1", y);
|
|
fg.insert("x2", z);
|
|
|
|
Vector expected = Vector_(2, 6.6667, -9.3333);
|
|
|
|
ConstrainedConditionalGaussian ternary(b, A1, "x1", A2, "x2", A3);
|
|
|
|
CHECK(assert_equal(expected, ternary.solve(fg), 1e-4));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
|
/* ************************************************************************* */
|
|
|