/* ************************************************************************* */
TEST(HessianFactor, combine) {
// update the information matrix with a single jacobian factor
Matrix A0 = Matrix_(2, 2,
11.1803399, 0.0,
0.0, 11.1803399);
Matrix A1 = Matrix_(2, 2,
-2.23606798, 0.0,
0.0, -2.23606798);
Matrix A2 = Matrix_(2, 2,
-8.94427191, 0.0,
0.0, -8.94427191);
Vector b = Vector_(2, 2.23606798,-1.56524758);
SharedDiagonal model = noiseModel::Diagonal::Sigmas(ones(2));
GaussianFactor::shared_ptr f(new JacobianFactor(0, A0, 1, A1, 2, A2, b, model));
FactorGraph<GaussianFactor> factors;
factors.push_back(f);
// Form Ab' * Ab
HessianFactor actual(factors);
Matrix expected = Matrix_(7, 7,
125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000,
0.0, 125.0000, 0.0, -25.0000, 0.0, -100.0000, -17.5000,
-25.0000, 0.0, 5.0000, 0.0, 20.0000, 0.0, -5.0000,
0.0, -25.0000, 0.0, 5.0000, 0.0, 20.0000, 3.5000,
-100.0000, 0.0, 20.0000, 0.0, 80.0000, 0.0, -20.0000,
0.0, -100.0000, 0.0, 20.0000, 0.0, 80.0000, 14.0000,
25.0000, -17.5000, -5.0000, 3.5000, -20.0000, 14.0000, 7.4500);
EXPECT(assert_equal(Matrix(expected.triangularView<Eigen::Upper>()),
Matrix(actual.matrix_.triangularView<Eigen::Upper>()), tol));
}
/* ************************************************************************* */
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));
}