/* ************************************************************************* */
TEST( GaussianBayesTree, balanced_smoother_shortcuts )
{
// Create smoother with 7 nodes
GaussianFactorGraph smoother = createSmoother(7);
// Create the Bayes tree
Ordering ordering;
ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
// Check the conditional P(Root|Root)
GaussianBayesNet empty;
GaussianBayesTree::sharedClique R = bayesTree.roots().front();
GaussianBayesNet actual1 = R->shortcut(R);
EXPECT(assert_equal(empty,actual1,tol));
// Check the conditional P(C2|Root)
GaussianBayesTree::sharedClique C2 = bayesTree[X(3)];
GaussianBayesNet actual2 = C2->shortcut(R);
EXPECT(assert_equal(empty,actual2,tol));
// Check the conditional P(C3|Root), which should be equal to P(x2|x4)
/** TODO: Note for multifrontal conditional:
* p_x2_x4 is now an element conditional of the multifrontal conditional bayesTree[ordering[X(2)]]->conditional()
* We don't know yet how to take it out.
*/
// GaussianConditional::shared_ptr p_x2_x4 = bayesTree[ordering[X(2)]]->conditional();
// p_x2_x4->print("Conditional p_x2_x4: ");
// GaussianBayesNet expected3(p_x2_x4);
// GaussianISAM::sharedClique C3 = isamTree[ordering[X(1)]];
// GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R);
// EXPECT(assert_equal(expected3,actual3,tol));
}
/* ************************************************************************* */
TEST( ISAM, iSAM_smoother )
{
Ordering ordering;
for (int t = 1; t <= 7; t++) ordering += X(t);
// Create smoother with 7 nodes
GaussianFactorGraph smoother = createSmoother(7);
// run iSAM for every factor
GaussianISAM actual;
for(boost::shared_ptr<GaussianFactor> factor: smoother) {
GaussianFactorGraph factorGraph;
factorGraph.push_back(factor);
actual.update(factorGraph);
}
// Create expected Bayes Tree by solving smoother with "natural" ordering
GaussianBayesTree expected = *smoother.eliminateMultifrontal(ordering);
// Verify sigmas in the bayes tree
for(const GaussianBayesTree::sharedClique& clique: expected.nodes() | br::map_values) {
GaussianConditional::shared_ptr conditional = clique->conditional();
EXPECT(!conditional->get_model());
}
// Check whether BayesTree is correct
EXPECT(assert_equal(GaussianFactorGraph(expected).augmentedHessian(), GaussianFactorGraph(actual).augmentedHessian()));
// obtain solution
VectorValues e; // expected solution
for (int t = 1; t <= 7; t++) e.insert(X(t), Vector::Zero(2));
VectorValues optimized = actual.optimize(); // actual solution
EXPECT(assert_equal(e, optimized));
}
// Create a planar factor graph and eliminate
double timePlanarSmootherEliminate(int N, bool old = true) {
GaussianFactorGraph fg = planarGraph(N).get<0>();
clock_t start = clock();
fg.eliminateMultifrontal();
clock_t end = clock ();
double dif = (double)(end - start) / CLOCKS_PER_SEC;
return dif;
}
/* ************************************************************************* */
TEST (Serialization, gaussian_bayes_tree) {
const Key x1=1, x2=2, x3=3, x4=4;
const Ordering chainOrdering = Ordering(list_of(x2)(x1)(x3)(x4));
const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5);
const GaussianFactorGraph chain = list_of
(JacobianFactor(x2, (Matrix(1, 1) << 1.), x1, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise))
(JacobianFactor(x2, (Matrix(1, 1) << 1.), x3, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise))
(JacobianFactor(x3, (Matrix(1, 1) << 1.), x4, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise))
(JacobianFactor(x4, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise));
GaussianBayesTree init = *chain.eliminateMultifrontal(chainOrdering);
GaussianBayesTree expected = *chain.eliminateMultifrontal(chainOrdering);
GaussianBayesTree actual;
std::string serialized = serialize(init);
deserialize(serialized, actual);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST( GaussianBayesTree, balanced_smoother_joint )
{
// Create smoother with 7 nodes
Ordering ordering;
ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
GaussianFactorGraph smoother = createSmoother(7);
// Create the Bayes tree, expected to look like:
// x5 x6 x4
// x3 x2 : x4
// x1 : x2
// x7 : x6
GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
// Conditional density elements reused by both tests
const Matrix I = eye(2), A = -0.00429185*I;
// Check the joint density P(x1,x7) factored as P(x1|x7)P(x7)
GaussianBayesNet expected1 = list_of
// Why does the sign get flipped on the prior?
(GaussianConditional(X(1), zero(2), I/sigmax7, X(7), A/sigmax7))
(GaussianConditional(X(7), zero(2), -1*I/sigmax7));
GaussianBayesNet actual1 = *bayesTree.jointBayesNet(X(1),X(7));
EXPECT(assert_equal(expected1, actual1, tol));
// // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1)
// GaussianBayesNet expected2;
// GaussianConditional::shared_ptr
// parent2(new GaussianConditional(X(1), zero(2), -1*I/sigmax1, ones(2)));
// expected2.push_front(parent2);
// push_front(expected2,X(7), zero(2), I/sigmax1, X(1), A/sigmax1, sigma);
// GaussianBayesNet actual2 = *bayesTree.jointBayesNet(X(7),X(1));
// EXPECT(assert_equal(expected2,actual2,tol));
// Check the joint density P(x1,x4), i.e. with a root variable
double sig14 = 0.784465;
Matrix A14 = -0.0769231*I;
GaussianBayesNet expected3 = list_of
(GaussianConditional(X(1), zero(2), I/sig14, X(4), A14/sig14))
(GaussianConditional(X(4), zero(2), I/sigmax4));
GaussianBayesNet actual3 = *bayesTree.jointBayesNet(X(1),X(4));
EXPECT(assert_equal(expected3,actual3,tol));
// // Check the joint density P(x4,x1), i.e. with a root variable, factored the other way
// GaussianBayesNet expected4;
// GaussianConditional::shared_ptr
// parent4(new GaussianConditional(X(1), zero(2), -1.0*I/sigmax1, ones(2)));
// expected4.push_front(parent4);
// double sig41 = 0.668096;
// Matrix A41 = -0.055794*I;
// push_front(expected4,X(4), zero(2), I/sig41, X(1), A41/sig41, sigma);
// GaussianBayesNet actual4 = *bayesTree.jointBayesNet(X(4),X(1));
// EXPECT(assert_equal(expected4,actual4,tol));
}
/* ************************************************************************* *
Bayes tree for smoother with "nested dissection" ordering:
Node[x1] P(x1 | x2)
Node[x3] P(x3 | x2 x4)
Node[x5] P(x5 | x4 x6)
Node[x7] P(x7 | x6)
Node[x2] P(x2 | x4)
Node[x6] P(x6 | x4)
Node[x4] P(x4)
becomes
C1 x5 x6 x4
C2 x3 x2 : x4
C3 x1 : x2
C4 x7 : x6
************************************************************************* */
TEST( GaussianBayesTree, balanced_smoother_marginals )
{
// Create smoother with 7 nodes
GaussianFactorGraph smoother = createSmoother(7);
// Create the Bayes tree
Ordering ordering;
ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
VectorValues actualSolution = bayesTree.optimize();
VectorValues expectedSolution = VectorValues::Zero(actualSolution);
EXPECT(assert_equal(expectedSolution,actualSolution,tol));
LONGS_EQUAL(4, (long)bayesTree.size());
double tol=1e-5;
// Check marginal on x1
JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), zero(2), sigmax1);
JacobianFactor actual1 = *bayesTree.marginalFactor(X(1));
Matrix expectedCovarianceX1 = eye(2,2) * (sigmax1 * sigmax1);
Matrix actualCovarianceX1;
GaussianFactor::shared_ptr m = bayesTree.marginalFactor(X(1), EliminateCholesky);
actualCovarianceX1 = bayesTree.marginalFactor(X(1), EliminateCholesky)->information().inverse();
EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol));
EXPECT(assert_equal(expected1,actual1,tol));
// Check marginal on x2
double sigx2 = 0.68712938; // FIXME: this should be corrected analytically
JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), zero(2), sigx2);
JacobianFactor actual2 = *bayesTree.marginalFactor(X(2));
EXPECT(assert_equal(expected2,actual2,tol));
// Check marginal on x3
JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), zero(2), sigmax3);
JacobianFactor actual3 = *bayesTree.marginalFactor(X(3));
EXPECT(assert_equal(expected3,actual3,tol));
// Check marginal on x4
JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), zero(2), sigmax4);
JacobianFactor actual4 = *bayesTree.marginalFactor(X(4));
EXPECT(assert_equal(expected4,actual4,tol));
// Check marginal on x7 (should be equal to x1)
JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), zero(2), sigmax7);
JacobianFactor actual7 = *bayesTree.marginalFactor(X(7));
EXPECT(assert_equal(expected7,actual7,tol));
}
/* ************************************************************************* *
Bayes tree for smoother with "natural" ordering:
C1 x6 x7
C2 x5 : x6
C3 x4 : x5
C4 x3 : x4
C5 x2 : x3
C6 x1 : x2
**************************************************************************** */
TEST( GaussianBayesTree, linear_smoother_shortcuts )
{
// Create smoother with 7 nodes
GaussianFactorGraph smoother = createSmoother(7);
GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal();
// Create the Bayes tree
LONGS_EQUAL(6, (long)bayesTree.size());
// Check the conditional P(Root|Root)
GaussianBayesNet empty;
GaussianBayesTree::sharedClique R = bayesTree.roots().front();
GaussianBayesNet actual1 = R->shortcut(R);
EXPECT(assert_equal(empty,actual1,tol));
// Check the conditional P(C2|Root)
GaussianBayesTree::sharedClique C2 = bayesTree[X(5)];
GaussianBayesNet actual2 = C2->shortcut(R);
EXPECT(assert_equal(empty,actual2,tol));
// Check the conditional P(C3|Root)
double sigma3 = 0.61808;
Matrix A56 = (Matrix(2,2) << -0.382022,0.,0.,-0.382022).finished();
GaussianBayesNet expected3;
expected3 += GaussianConditional(X(5), zero(2), eye(2)/sigma3, X(6), A56/sigma3);
GaussianBayesTree::sharedClique C3 = bayesTree[X(4)];
GaussianBayesNet actual3 = C3->shortcut(R);
EXPECT(assert_equal(expected3,actual3,tol));
// Check the conditional P(C4|Root)
double sigma4 = 0.661968;
Matrix A46 = (Matrix(2,2) << -0.146067,0.,0.,-0.146067).finished();
GaussianBayesNet expected4;
expected4 += GaussianConditional(X(4), zero(2), eye(2)/sigma4, X(6), A46/sigma4);
GaussianBayesTree::sharedClique C4 = bayesTree[X(3)];
GaussianBayesNet actual4 = C4->shortcut(R);
EXPECT(assert_equal(expected4,actual4,tol));
}
/* ************************************************************************* */
TEST(GaussianBayesTree, shortcut_overlapping_separator)
{
// Test computing shortcuts when the separator overlaps. This previously
// would have highlighted a problem where information was duplicated.
// Create factor graph:
// f(1,2,5)
// f(3,4,5)
// f(5,6)
// f(6,7)
GaussianFactorGraph fg;
noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
fg.add(1, (Matrix(1, 1) << 1.0).finished(), 3, (Matrix(1, 1) << 2.0).finished(), 5, (Matrix(1, 1) << 3.0).finished(), (Vector(1) << 4.0).finished(), model);
fg.add(1, (Matrix(1, 1) << 5.0).finished(), (Vector(1) << 6.0).finished(), model);
fg.add(2, (Matrix(1, 1) << 7.0).finished(), 4, (Matrix(1, 1) << 8.0).finished(), 5, (Matrix(1, 1) << 9.0).finished(), (Vector(1) << 10.0).finished(), model);
fg.add(2, (Matrix(1, 1) << 11.0).finished(), (Vector(1) << 12.0).finished(), model);
fg.add(5, (Matrix(1, 1) << 13.0).finished(), 6, (Matrix(1, 1) << 14.0).finished(), (Vector(1) << 15.0).finished(), model);
fg.add(6, (Matrix(1, 1) << 17.0).finished(), 7, (Matrix(1, 1) << 18.0).finished(), (Vector(1) << 19.0).finished(), model);
fg.add(7, (Matrix(1, 1) << 20.0).finished(), (Vector(1) << 21.0).finished(), model);
// Eliminate into BayesTree
// c(6,7)
// c(5|6)
// c(1,2|5)
// c(3,4|5)
Ordering ordering(fg.keys());
GaussianBayesTree bt = *fg.eliminateMultifrontal(ordering); // eliminate in increasing key order, fg.keys() is sorted.
GaussianFactorGraph joint = *bt.joint(1,2, EliminateQR);
Matrix expectedJointJ = (Matrix(2,3) <<
5, 0, 6,
0, -11, -12
).finished();
Matrix actualJointJ = joint.augmentedJacobian();
EXPECT(assert_equal(expectedJointJ, actualJointJ));
}
