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