// Create a Kalman smoother for t=1:T and optimize
double timeKalmanSmoother(int T) {
GaussianFactorGraph smoother = createSmoother(T);
clock_t start = clock();
// Keys will come out sorted since keys() returns a set
smoother.optimize(Ordering(smoother.keys()));
clock_t end = clock ();
double dif = (double)(end - start) / CLOCKS_PER_SEC;
return dif;
}
/* ************************************************************************* */
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));
}
