diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 45f652d05..57a663e8c 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -36,9 +36,18 @@ using namespace boost::assign; using namespace std; using namespace gtsam; -// static SharedDiagonal -// sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1), sigma_02 = noiseModel::Isotropic::Sigma(2,0.2), -// constraintModel = noiseModel::Constrained::All(2); +typedef boost::tuple BoostTriplet; +bool triplet_equal(BoostTriplet a, BoostTriplet b) { + if (a.get<0>() == b.get<0>() && a.get<1>() == b.get<1>() && + a.get<2>() == b.get<2>()) return true; + + cout << "not equal:" << endl; + cout << "\texpected: " + "(" << a.get<0>() << ", " << a.get<1>() << ") = " << a.get<2>() << endl; + cout << "\tactual: " + "(" << b.get<0>() << ", " << b.get<1>() << ") = " << b.get<2>() << endl; + return false; +} /* ************************************************************************* */ TEST(GaussianFactorGraph, initialization) { @@ -73,37 +82,48 @@ TEST(GaussianFactorGraph, sparseJacobian) { // 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); + const Key x123 = 0, x45 = 1; + gfg.add(x123, (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished(), + Vector2(4, 8), model); + gfg.add(x123, (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished(), + x45, (Matrix(2, 2) << 11, 12, 14, 15.).finished(), + Vector2(13, 16), model); - Matrix actual = gfg.sparseJacobian_(); + // Check version for MATLAB - 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(); - EXPECT(assert_equal(expected, actual)); + // matrix form (matlab) + Matrix expectedMatlab = expectedT.transpose(); + EXPECT(assert_equal(expectedMatlab, gfg.sparseJacobian_())); + + // BoostTriplets + auto boostActual = gfg.sparseJacobian(); + // check the triplets size... + EXPECT_LONGS_EQUAL(16, boostActual.size()); + // check content + for (int i = 0; i < 16; i++) { + EXPECT(triplet_equal( + BoostTriplet(expectedT(i, 0) - 1, expectedT(i, 1) - 1, expectedT(i, 2)), + boostActual.at(i))); + } } /* ************************************************************************* */