Added push_fron convenience method to add ConditionalGaussians into a Bayes net with much less clutter. Modernized some very old tests in the process.
Frank Dellaert
parent
f677341108
commit
1ae6bb4030
|
|
@ -43,6 +43,20 @@ GaussianBayesNet simpleGaussian(const string& key, const Vector& mu, double sigm
|
|||
return bn;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void push_front(GaussianBayesNet& bn, const string& key, Vector d, Matrix R,
|
||||
const string& name1, Matrix S, Vector sigmas) {
|
||||
ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key, d, R, name1, S, sigmas));
|
||||
bn.push_front(cg);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void push_front(GaussianBayesNet& bn, const string& key, Vector d, Matrix R,
|
||||
const string& name1, Matrix S, const string& name2, Matrix T, Vector sigmas) {
|
||||
ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key, d, R, name1, S, name2, T, sigmas));
|
||||
bn.push_front(cg);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorConfig optimize(const GaussianBayesNet& bn)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -25,6 +25,20 @@ namespace gtsam {
|
|||
/** Create a simple Gaussian on a single multivariate variable */
|
||||
GaussianBayesNet simpleGaussian(const std::string& key, const Vector& mu, double sigma=1.0);
|
||||
|
||||
/**
|
||||
* Add a conditional node with one parent
|
||||
* |Rx+Sy-d|
|
||||
*/
|
||||
void push_front(GaussianBayesNet& bn, const std::string& key, Vector d, Matrix R,
|
||||
const std::string& name1, Matrix S, Vector sigmas);
|
||||
|
||||
/**
|
||||
* Add a conditional node with two parents
|
||||
* |Rx+Sy+Tz-d|
|
||||
*/
|
||||
void push_front(GaussianBayesNet& bn, const std::string& key, Vector d, Matrix R,
|
||||
const std::string& name1, Matrix S, const std::string& name2, Matrix T, Vector sigmas);
|
||||
|
||||
/**
|
||||
* optimize, i.e. return x = inv(R)*d
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -121,8 +121,8 @@ TEST( BayesTree, linear_smoother_shortcuts )
|
|||
// Check the conditional P(C3|Root)
|
||||
Vector sigma3 = repeat(2, 0.61808);
|
||||
Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022);
|
||||
ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian("x5", zero(2), eye(2), "x6", A56, sigma3));
|
||||
GaussianBayesNet expected3; expected3.push_back(cg3);
|
||||
GaussianBayesNet expected3;
|
||||
push_front(expected3,"x5", zero(2), eye(2), "x6", A56, sigma3);
|
||||
Gaussian::sharedClique C3 = bayesTree["x4"];
|
||||
GaussianBayesNet actual3 = C3->shortcut<LinearFactor>(R);
|
||||
CHECK(assert_equal(expected3,actual3,1e-4));
|
||||
|
|
@ -130,8 +130,8 @@ TEST( BayesTree, linear_smoother_shortcuts )
|
|||
// Check the conditional P(C4|Root)
|
||||
Vector sigma4 = repeat(2, 0.661968);
|
||||
Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067);
|
||||
ConditionalGaussian::shared_ptr cg4(new ConditionalGaussian("x4", zero(2), eye(2), "x6", A46, sigma4));
|
||||
GaussianBayesNet expected4; expected4.push_back(cg4);
|
||||
GaussianBayesNet expected4;
|
||||
push_front(expected4,"x4", zero(2), eye(2), "x6", A46, sigma4);
|
||||
Gaussian::sharedClique C4 = bayesTree["x3"];
|
||||
GaussianBayesNet actual4 = C4->shortcut<LinearFactor>(R);
|
||||
CHECK(assert_equal(expected4,actual4,1e-4));
|
||||
|
|
@ -249,8 +249,7 @@ TEST( BayesTree, balanced_smoother_clique_marginals )
|
|||
GaussianBayesNet expected = simpleGaussian("x2",zero(2),sigmax2);
|
||||
Vector sigma = repeat(2, 0.707107);
|
||||
Matrix A12 = (-0.5)*eye(2);
|
||||
ConditionalGaussian::shared_ptr cg(new ConditionalGaussian("x1", zero(2), eye(2), "x2", A12, sigma));
|
||||
expected.push_front(cg);
|
||||
push_front(expected,"x1", zero(2), eye(2), "x2", A12, sigma);
|
||||
Gaussian::sharedClique R = bayesTree.root(), C3 = bayesTree["x1"];
|
||||
FactorGraph<LinearFactor> marginal = C3->marginal<LinearFactor>(R);
|
||||
GaussianBayesNet actual = eliminate<LinearFactor,ConditionalGaussian>(marginal,C3->keys());
|
||||
|
|
@ -275,15 +274,13 @@ TEST( BayesTree, balanced_smoother_joint )
|
|||
|
||||
// Check the joint density P(x1,x7) factored as P(x1|x7)P(x7)
|
||||
GaussianBayesNet expected1 = simpleGaussian("x7", zero(2), sigmax7);
|
||||
ConditionalGaussian::shared_ptr cg1(new ConditionalGaussian("x1", zero(2), eye(2), "x7", A, sigma));
|
||||
expected1.push_front(cg1);
|
||||
push_front(expected1,"x1", zero(2), eye(2), "x7", A, sigma);
|
||||
GaussianBayesNet actual1 = bayesTree.jointBayesNet<LinearFactor>("x1","x7");
|
||||
CHECK(assert_equal(expected1,actual1,1e-4));
|
||||
|
||||
// Check the joint density P(x7,x1) factored as P(x7|x1)P(x1)
|
||||
GaussianBayesNet expected2 = simpleGaussian("x1", zero(2), sigmax1);
|
||||
ConditionalGaussian::shared_ptr cg2(new ConditionalGaussian("x7", zero(2), eye(2), "x1", A, sigma));
|
||||
expected2.push_front(cg2);
|
||||
push_front(expected2,"x7", zero(2), eye(2), "x1", A, sigma);
|
||||
GaussianBayesNet actual2 = bayesTree.jointBayesNet<LinearFactor>("x7","x1");
|
||||
CHECK(assert_equal(expected2,actual2,1e-4));
|
||||
|
||||
|
|
@ -291,8 +288,7 @@ TEST( BayesTree, balanced_smoother_joint )
|
|||
GaussianBayesNet expected3 = simpleGaussian("x4", zero(2), sigmax4);
|
||||
Vector sigma14 = repeat(2, 0.784465);
|
||||
Matrix A14 = (-0.0769231)*eye(2);
|
||||
ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian("x1", zero(2), eye(2), "x4", A14, sigma14));
|
||||
expected3.push_front(cg3);
|
||||
push_front(expected3,"x1", zero(2), eye(2), "x4", A14, sigma14);
|
||||
GaussianBayesNet actual3 = bayesTree.jointBayesNet<LinearFactor>("x1","x4");
|
||||
CHECK(assert_equal(expected3,actual3,1e-4));
|
||||
|
||||
|
|
@ -300,8 +296,7 @@ TEST( BayesTree, balanced_smoother_joint )
|
|||
GaussianBayesNet expected4 = simpleGaussian("x1", zero(2), sigmax1);
|
||||
Vector sigma41 = repeat(2, 0.668096);
|
||||
Matrix A41 = (-0.055794)*eye(2);
|
||||
ConditionalGaussian::shared_ptr cg4(new ConditionalGaussian("x4", zero(2), eye(2), "x1", A41, sigma41));
|
||||
expected4.push_front(cg4);
|
||||
push_front(expected4,"x4", zero(2), eye(2), "x1", A41, sigma41);
|
||||
GaussianBayesNet actual4 = bayesTree.jointBayesNet<LinearFactor>("x4","x1");
|
||||
CHECK(assert_equal(expected4,actual4,1e-4));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -193,21 +193,8 @@ TEST( LinearFactorGraph, eliminateOne_x1 )
|
|||
ConditionalGaussian::shared_ptr actual = fg.eliminateOne("x1");
|
||||
|
||||
// create expected Conditional Gaussian
|
||||
Matrix R11 = Matrix_(2,2,
|
||||
1.0, 0.0,
|
||||
0.0, 1.0
|
||||
);
|
||||
Matrix S12 = Matrix_(2,2,
|
||||
-0.111111, 0.00,
|
||||
+0.00,-0.111111
|
||||
);
|
||||
Matrix S13 = Matrix_(2,2,
|
||||
-0.444444, 0.00,
|
||||
+0.00,-0.444444
|
||||
);
|
||||
Vector d(2); d(0) = -0.133333; d(1) = -0.0222222;
|
||||
Vector sigma(2); sigma(0) = 1./15; sigma(1) = 1./15;
|
||||
|
||||
Matrix I = eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
|
||||
Vector d = Vector_(2, -0.133333, -0.0222222), sigma = repeat(2, 1./15);
|
||||
ConditionalGaussian expected("x1",d,R11,"l1",S12,"x2",S13,sigma);
|
||||
|
||||
CHECK(assert_equal(expected,*actual,tol));
|
||||
|
|
@ -221,21 +208,8 @@ TEST( LinearFactorGraph, eliminateOne_x2 )
|
|||
ConditionalGaussian::shared_ptr actual = fg.eliminateOne("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;
|
||||
Vector sigma(2); sigma(0) = 0.0894427; sigma(1) = 0.0894427;
|
||||
|
||||
Matrix I = eye(2), R11 = I, S12 = -0.2*I, S13 = -0.8*I;
|
||||
Vector d = Vector_(2, 0.2, -0.14), sigma = repeat(2, 0.0894427);
|
||||
ConditionalGaussian expected("x2",d,R11,"l1",S12,"x1",S13,sigma);
|
||||
|
||||
CHECK(assert_equal(expected,*actual,tol));
|
||||
|
|
@ -248,21 +222,8 @@ TEST( LinearFactorGraph, eliminateOne_l1 )
|
|||
ConditionalGaussian::shared_ptr actual = fg.eliminateOne("l1");
|
||||
|
||||
// create expected Conditional Gaussian
|
||||
Matrix R11 = Matrix_(2,2,
|
||||
1.0, 0.0,
|
||||
0.0, 1.0
|
||||
);
|
||||
Matrix S12 = Matrix_(2,2,
|
||||
-0.5, 0.0,
|
||||
+0.0,-0.5
|
||||
);
|
||||
Matrix S13 = Matrix_(2,2,
|
||||
-0.5, 0.0,
|
||||
+0.0,-0.5
|
||||
);
|
||||
Vector d(2); d(0) = -0.1; d(1) = 0.25;
|
||||
Vector sigma(2); sigma(0) = 0.141421; sigma(1) = 0.141421;
|
||||
|
||||
Matrix I = eye(2), R11 = I, S12 = -0.5*I, S13 = -0.5*I;
|
||||
Vector d = Vector_(2, -0.1, 0.25), sigma = repeat(2, 0.141421);
|
||||
ConditionalGaussian expected("l1",d,R11,"x1",S12,"x2",S13,sigma);
|
||||
|
||||
CHECK(assert_equal(expected,*actual,tol));
|
||||
|
|
@ -272,50 +233,22 @@ TEST( LinearFactorGraph, eliminateOne_l1 )
|
|||
TEST( LinearFactorGraph, eliminateAll )
|
||||
{
|
||||
// create expected Chordal bayes Net
|
||||
double data1[] = { 1.0, 0.0,
|
||||
0.0, 1.0};
|
||||
Matrix R1 = Matrix_(2,2, data1);
|
||||
Vector d1(2); d1(0) = -0.1; d1(1) = -0.1;
|
||||
Vector sigma1(2); sigma1(0) = 0.1; sigma1(1) = 0.1;
|
||||
Matrix I = eye(2);
|
||||
|
||||
ConditionalGaussian::shared_ptr cg1(new ConditionalGaussian("x1",d1, R1, sigma1));
|
||||
Vector d1 = Vector_(2, -0.1,-0.1);
|
||||
GaussianBayesNet expected = simpleGaussian("x1",d1,0.1);
|
||||
|
||||
double data21[] = { 1.0, 0.0,
|
||||
0.0, 1.0};
|
||||
Matrix R2 = Matrix_(2,2, data21);
|
||||
double data22[] = { -1.0, 0.0,
|
||||
0.0, -1.0};
|
||||
Matrix A1 = Matrix_(2,2, data22);
|
||||
Vector d2(2); d2(0) = 0.0; d2(1) = 0.2;
|
||||
Vector sigma2(2); sigma2(0) = 0.149071; sigma2(1) = 0.149071;
|
||||
Vector d2 = Vector_(2, 0.0, 0.2), sigma2 = repeat(2,0.149071);
|
||||
push_front(expected,"l1",d2, I,"x1", (-1)*I,sigma2);
|
||||
|
||||
ConditionalGaussian::shared_ptr cg2(new ConditionalGaussian("l1",d2, R2,"x1", A1,sigma2));
|
||||
|
||||
double data31[] = { 1.0, 0.0,
|
||||
0.0, 1.0};
|
||||
Matrix R3 = Matrix_(2,2, data31);
|
||||
double data32[] = { -0.2, 0.0,
|
||||
0.0, -0.2};
|
||||
Matrix A21 = Matrix_(2,2, data32);
|
||||
double data33[] = { -0.8, 0.0,
|
||||
0.0, -0.8};
|
||||
Matrix A22 = Matrix_(2,2, data33);
|
||||
|
||||
Vector d3(2); d3(0) = 0.2; d3(1) = -0.14;
|
||||
Vector sigma3(2); sigma3(0) = 0.0894427; sigma3(1) = 0.0894427;
|
||||
|
||||
ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian("x2",d3, R3,"l1", A21, "x1", A22, sigma3));
|
||||
|
||||
GaussianBayesNet expected;
|
||||
expected.push_back(cg3);
|
||||
expected.push_back(cg2);
|
||||
expected.push_back(cg1);
|
||||
Vector d3 = Vector_(2, 0.2, -0.14), sigma3 = repeat(2,0.0894427);
|
||||
push_front(expected,"x2",d3, I,"l1", (-0.2)*I, "x1", (-0.8)*I, sigma3);
|
||||
|
||||
// Check one ordering
|
||||
LinearFactorGraph fg1 = createLinearFactorGraph();
|
||||
Ordering ord1;
|
||||
ord1 += "x2","l1","x1";
|
||||
GaussianBayesNet actual = fg1.eliminate(ord1);
|
||||
Ordering ordering;
|
||||
ordering += "x2","l1","x1";
|
||||
GaussianBayesNet actual = fg1.eliminate(ordering);
|
||||
CHECK(assert_equal(expected,actual,tol));
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue