/* ************************************************************************* */ TEST( SubgraphPreconditioner, planarGraph ) { // Check planar graph construction GaussianFactorGraph A; VectorValues xtrue; boost::tie(A, xtrue) = planarGraph(3); LONGS_EQUAL(13,A.size()); LONGS_EQUAL(9,xtrue.size()); DOUBLES_EQUAL(0,error(A,xtrue),1e-9); // check zero error for xtrue // Check that xtrue is optimal GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(A).eliminate(); VectorValues actual = optimize(*R1); CHECK(assert_equal(xtrue,actual)); }
/* ************************************************************************* */ TEST(GaussianFactorGraph, initialization) { // Create empty graph GaussianFactorGraph fg; SharedDiagonal unit2 = noiseModel::Unit::Create(2); fg += JacobianFactor(0, 10*I_2x2, -1.0*Vector::Ones(2), unit2), JacobianFactor(0, -10*I_2x2,1, 10*I_2x2, Vector2(2.0, -1.0), unit2), JacobianFactor(0, -5*I_2x2, 2, 5*I_2x2, Vector2(0.0, 1.0), unit2), JacobianFactor(1, -5*I_2x2, 2, 5*I_2x2, Vector2(-1.0, 1.5), unit2); EXPECT_LONGS_EQUAL(4, (long)fg.size()); // Test sparse, which takes a vector and returns a matrix, used in MATLAB // Note that this the augmented vector and the RHS is in column 7 Matrix expectedIJS = (Matrix(3, 22) << 1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 5., 6., 7., 8., 7., 8., 7., 8., 1., 2., 7., 7., 1., 2., 3., 4., 7., 7., 1., 2., 5., 6., 7., 7., 3., 4., 5., 6., 7., 7., 10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5., 0., 1., -5., -5., 5., 5., -1., 1.5).finished(); Matrix actualIJS = fg.sparseJacobian_(); EQUALITY(expectedIJS, actualIJS); }