/* ************************************************************************* */
TEST(HessianFactor, CombineAndEliminate2) {
Matrix A01 = I_3x3;
Vector3 b0(1.5, 1.5, 1.5);
Vector3 s0(1.6, 1.6, 1.6);
Matrix A10 = 2.0 * I_3x3;
Matrix A11 = -2.0 * I_3x3;
Vector3 b1(2.5, 2.5, 2.5);
Vector3 s1(2.6, 2.6, 2.6);
Matrix A21 = 3.0 * I_3x3;
Vector3 b2(3.5, 3.5, 3.5);
Vector3 s2(3.6, 3.6, 3.6);
GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
Matrix93 A0, A1;
A0 << A10, Z_3x3, Z_3x3;
A1 << A11, A01, A21;
Vector9 b, sigmas;
b << b1, b0, b2;
sigmas << s1, s0, s2;
// create a full, uneliminated version of the factor
JacobianFactor jacobian(0, A0, 1, A1, b,
noiseModel::Diagonal::Sigmas(sigmas, true));
// Make sure combining works
HessianFactor hessian(gfg);
EXPECT(assert_equal(HessianFactor(jacobian), hessian, 1e-6));
EXPECT(
assert_equal(jacobian.augmentedInformation(),
hessian.augmentedInformation(), 1e-9));
// perform elimination on jacobian
Ordering ordering = list_of(0);
GaussianConditional::shared_ptr expectedConditional;
JacobianFactor::shared_ptr expectedFactor;
boost::tie(expectedConditional, expectedFactor) = //
jacobian.eliminate(ordering);
// Eliminate
GaussianConditional::shared_ptr actualConditional;
HessianFactor::shared_ptr actualHessian;
boost::tie(actualConditional, actualHessian) = //
EliminateCholesky(gfg, ordering);
EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
VectorValues v;
v.insert(1, Vector3(1, 2, 3));
EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9);
EXPECT(
assert_equal(expectedFactor->augmentedInformation(),
actualHessian->augmentedInformation(), 1e-9));
EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6));
}
/* ************************************************************************* */
TEST(HessianFactor, CombineAndEliminate)
{
Matrix A01 = (Matrix(3,3) <<
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
Vector b0 = (Vector(3) << 1.5, 1.5, 1.5);
Vector s0 = (Vector(3) << 1.6, 1.6, 1.6);
Matrix A10 = (Matrix(3,3) <<
2.0, 0.0, 0.0,
0.0, 2.0, 0.0,
0.0, 0.0, 2.0);
Matrix A11 = (Matrix(3,3) <<
-2.0, 0.0, 0.0,
0.0, -2.0, 0.0,
0.0, 0.0, -2.0);
Vector b1 = (Vector(3) << 2.5, 2.5, 2.5);
Vector s1 = (Vector(3) << 2.6, 2.6, 2.6);
Matrix A21 = (Matrix(3,3) <<
3.0, 0.0, 0.0,
0.0, 3.0, 0.0,
0.0, 0.0, 3.0);
Vector b2 = (Vector(3) << 3.5, 3.5, 3.5);
Vector s2 = (Vector(3) << 3.6, 3.6, 3.6);
GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
Vector b = gtsam::concatVectors(3, &b1, &b0, &b2);
Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2);
// create a full, uneliminated version of the factor
JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
// perform elimination on jacobian
GaussianConditional::shared_ptr expectedConditional;
JacobianFactor::shared_ptr expectedRemainingFactor;
boost::tie(expectedConditional, expectedRemainingFactor) = expectedFactor.eliminate(Ordering(list_of(0)));
// Eliminate
GaussianConditional::shared_ptr actualConditional;
HessianFactor::shared_ptr actualCholeskyFactor;
boost::tie(actualConditional, actualCholeskyFactor) = EliminateCholesky(gfg, Ordering(list_of(0)));
EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
EXPECT(assert_equal(HessianFactor(*expectedRemainingFactor), *actualCholeskyFactor, 1e-6));
}
/* ************************************************************************* */
TEST(GaussianFactorGraph, matrices) {
// Create factor graph:
// x1 x2 x3 x4 x5 b
// 1 2 3 0 0 4
// 5 6 7 0 0 8
// 9 10 0 11 12 13
// 0 0 0 14 15 16
Matrix A00 = (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished();
Matrix A10 = (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished();
Matrix A11 = (Matrix(2, 2) << 11, 12, 14, 15).finished();
GaussianFactorGraph gfg;
SharedDiagonal model = noiseModel::Unit::Create(2);
gfg.add(0, A00, Vector2(4., 8.), model);
gfg.add(0, A10, 1, A11, Vector2(13., 16.), model);
Matrix Ab(4, 6);
Ab << 1, 2, 3, 0, 0, 4, 5, 6, 7, 0, 0, 8, 9, 10, 0, 11, 12, 13, 0, 0, 0, 14, 15, 16;
// augmented versions
EXPECT(assert_equal(Ab, gfg.augmentedJacobian()));
EXPECT(assert_equal(Ab.transpose() * Ab, gfg.augmentedHessian()));
// jacobian
Matrix A = Ab.leftCols(Ab.cols() - 1);
Vector b = Ab.col(Ab.cols() - 1);
Matrix actualA;
Vector actualb;
boost::tie(actualA, actualb) = gfg.jacobian();
EXPECT(assert_equal(A, actualA));
EXPECT(assert_equal(b, actualb));
// hessian
Matrix L = A.transpose() * A;
Vector eta = A.transpose() * b;
Matrix actualL;
Vector actualeta;
boost::tie(actualL, actualeta) = gfg.hessian();
EXPECT(assert_equal(L, actualL));
EXPECT(assert_equal(eta, actualeta));
// hessianBlockDiagonal
VectorValues expectLdiagonal; // Make explicit that diagonal is sum-squares of columns
expectLdiagonal.insert(0, Vector3(1 + 25 + 81, 4 + 36 + 100, 9 + 49));
expectLdiagonal.insert(1, Vector2(121 + 196, 144 + 225));
EXPECT(assert_equal(expectLdiagonal, gfg.hessianDiagonal()));
// hessianBlockDiagonal
map<Key, Matrix> actualBD = gfg.hessianBlockDiagonal();
LONGS_EQUAL(2, actualBD.size());
EXPECT(assert_equal(A00.transpose() * A00 + A10.transpose() * A10, actualBD[0]));
EXPECT(assert_equal(A11.transpose() * A11, actualBD[1]));
}
/* ************************************************************************* */
KalmanFilter::State KalmanFilter::init(const Vector& x0,
const SharedDiagonal& P0) {
// Create a factor graph f(x0), eliminate it into P(x0)
GaussianFactorGraph factorGraph;
factorGraph.add(0, I_, x0, P0); // |x-x0|^2_diagSigma
return solve(factorGraph, useQR());
}
/* ************************************************************************* */
TEST(GaussianFactorGraph, sparseJacobian) {
// Create factor graph:
// x1 x2 x3 x4 x5 b
// 1 2 3 0 0 4
// 5 6 7 0 0 8
// 9 10 0 11 12 13
// 0 0 0 14 15 16
// Expected - NOTE that we transpose this!
Matrix expectedT = (Matrix(16, 3) <<
1., 1., 2.,
1., 2., 4.,
1., 3., 6.,
2., 1.,10.,
2., 2.,12.,
2., 3.,14.,
1., 6., 8.,
2., 6.,16.,
3., 1.,18.,
3., 2.,20.,
3., 4.,22.,
3., 5.,24.,
4., 4.,28.,
4., 5.,30.,
3., 6.,26.,
4., 6.,32.).finished();
Matrix expected = expectedT.transpose();
GaussianFactorGraph gfg;
SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5);
gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.).finished(), Vector2(4., 8.), model);
gfg.add(0, (Matrix(2, 3) << 9., 10., 0., 0., 0., 0.).finished(), 1,
(Matrix(2, 2) << 11., 12., 14., 15.).finished(), Vector2(13., 16.), model);
Matrix actual = gfg.sparseJacobian_();
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(HessianFactor, CombineAndEliminate1) {
Matrix3 A01 = 3.0 * I_3x3;
Vector3 b0(1, 0, 0);
Matrix3 A21 = 4.0 * I_3x3;
Vector3 b2 = Vector3::Zero();
GaussianFactorGraph gfg;
gfg.add(1, A01, b0);
gfg.add(1, A21, b2);
Matrix63 A1;
A1 << A01, A21;
Vector6 b;
b << b0, b2;
// create a full, uneliminated version of the factor
JacobianFactor jacobian(1, A1, b);
// Make sure combining works
HessianFactor hessian(gfg);
VectorValues v;
v.insert(1, Vector3(1, 0, 0));
EXPECT_DOUBLES_EQUAL(jacobian.error(v), hessian.error(v), 1e-9);
EXPECT(assert_equal(HessianFactor(jacobian), hessian, 1e-6));
EXPECT(assert_equal(25.0 * I_3x3, hessian.information(), 1e-9));
EXPECT(
assert_equal(jacobian.augmentedInformation(),
hessian.augmentedInformation(), 1e-9));
// perform elimination on jacobian
Ordering ordering = list_of(1);
GaussianConditional::shared_ptr expectedConditional;
JacobianFactor::shared_ptr expectedFactor;
boost::tie(expectedConditional, expectedFactor) = //
jacobian.eliminate(ordering);
// Eliminate
GaussianConditional::shared_ptr actualConditional;
HessianFactor::shared_ptr actualHessian;
boost::tie(actualConditional, actualHessian) = //
EliminateCholesky(gfg, ordering);
EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9);
EXPECT(
assert_equal(expectedFactor->augmentedInformation(),
actualHessian->augmentedInformation(), 1e-9));
EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6));
}
/* ************************************************************************* */
TEST(GaussianFactorGraph, eliminate_empty) {
// eliminate an empty factor
GaussianFactorGraph gfg;
gfg.add(JacobianFactor());
GaussianBayesNet::shared_ptr actualBN;
GaussianFactorGraph::shared_ptr remainingGFG;
boost::tie(actualBN, remainingGFG) = gfg.eliminatePartialSequential(Ordering());
// expected Bayes net is empty
GaussianBayesNet expectedBN;
// expected remaining graph should be the same as the original, still containing the empty factor
GaussianFactorGraph expectedLF = gfg;
// check if the result matches
EXPECT(assert_equal(*actualBN, expectedBN));
EXPECT(assert_equal(*remainingGFG, expectedLF));
}
/* ************************************************************************* */
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));
}
/* ************************************************************************* */
TEST_UNSAFE(HessianFactor, CombineAndEliminate)
{
Matrix A01 = Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
Vector b0 = Vector_(3, 1.5, 1.5, 1.5);
Vector s0 = Vector_(3, 1.6, 1.6, 1.6);
Matrix A10 = Matrix_(3,3,
2.0, 0.0, 0.0,
0.0, 2.0, 0.0,
0.0, 0.0, 2.0);
Matrix A11 = Matrix_(3,3,
-2.0, 0.0, 0.0,
0.0, -2.0, 0.0,
0.0, 0.0, -2.0);
Vector b1 = Vector_(3, 2.5, 2.5, 2.5);
Vector s1 = Vector_(3, 2.6, 2.6, 2.6);
Matrix A21 = Matrix_(3,3,
3.0, 0.0, 0.0,
0.0, 3.0, 0.0,
0.0, 0.0, 3.0);
Vector b2 = Vector_(3, 3.5, 3.5, 3.5);
Vector s2 = Vector_(3, 3.6, 3.6, 3.6);
GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
Vector b = gtsam::concatVectors(3, &b1, &b0, &b2);
Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2);
// create a full, uneliminated version of the factor
JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
// perform elimination
GaussianConditional::shared_ptr expectedBN = expectedFactor.eliminate(1);
// create expected Hessian after elimination
HessianFactor expectedCholeskyFactor(expectedFactor);
// Convert all factors to hessians
FactorGraph<HessianFactor> hessians;
BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, gfg) {
if(boost::shared_ptr<HessianFactor> hf = boost::dynamic_pointer_cast<HessianFactor>(factor))
hessians.push_back(hf);
else if(boost::shared_ptr<JacobianFactor> jf = boost::dynamic_pointer_cast<JacobianFactor>(factor))
hessians.push_back(boost::make_shared<HessianFactor>(*jf));
else
CHECK(false);
}
// Eliminate
GaussianFactorGraph::EliminationResult actualCholesky = EliminateCholesky(gfg, 1);
HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast<HessianFactor>(actualCholesky.second);
EXPECT(assert_equal(*expectedBN, *actualCholesky.first, 1e-6));
EXPECT(assert_equal(expectedCholeskyFactor, *actualFactor, 1e-6));
}
