/* ************************************************************************* */ 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)); }