/* ************************************************************************* */ TEST(GaussianFactorGraph, matrices) { // 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 Matrix A00 = (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished(); Matrix A10 = (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished(); Matrix A11 = (Matrix(2, 2) << 11, 12, 14, 15).finished(); GaussianFactorGraph gfg; SharedDiagonal model = noiseModel::Unit::Create(2); gfg.add(0, A00, Vector2(4., 8.), model); gfg.add(0, A10, 1, A11, Vector2(13., 16.), model); Matrix Ab(4, 6); Ab << 1, 2, 3, 0, 0, 4, 5, 6, 7, 0, 0, 8, 9, 10, 0, 11, 12, 13, 0, 0, 0, 14, 15, 16; // augmented versions EXPECT(assert_equal(Ab, gfg.augmentedJacobian())); EXPECT(assert_equal(Ab.transpose() * Ab, gfg.augmentedHessian())); // jacobian Matrix A = Ab.leftCols(Ab.cols() - 1); Vector b = Ab.col(Ab.cols() - 1); Matrix actualA; Vector actualb; boost::tie(actualA, actualb) = gfg.jacobian(); EXPECT(assert_equal(A, actualA)); EXPECT(assert_equal(b, actualb)); // hessian Matrix L = A.transpose() * A; Vector eta = A.transpose() * b; Matrix actualL; Vector actualeta; boost::tie(actualL, actualeta) = gfg.hessian(); EXPECT(assert_equal(L, actualL)); EXPECT(assert_equal(eta, actualeta)); // hessianBlockDiagonal VectorValues expectLdiagonal; // Make explicit that diagonal is sum-squares of columns expectLdiagonal.insert(0, Vector3(1 + 25 + 81, 4 + 36 + 100, 9 + 49)); expectLdiagonal.insert(1, Vector2(121 + 196, 144 + 225)); EXPECT(assert_equal(expectLdiagonal, gfg.hessianDiagonal())); // hessianBlockDiagonal map<Key, Matrix> actualBD = gfg.hessianBlockDiagonal(); LONGS_EQUAL(2, actualBD.size()); EXPECT(assert_equal(A00.transpose() * A00 + A10.transpose() * A10, actualBD[0])); EXPECT(assert_equal(A11.transpose() * A11, actualBD[1])); }
/* ************************************************************************* */ TEST(GaussianBayesTree, shortcut_overlapping_separator) { // Test computing shortcuts when the separator overlaps. This previously // would have highlighted a problem where information was duplicated. // Create factor graph: // f(1,2,5) // f(3,4,5) // f(5,6) // f(6,7) GaussianFactorGraph fg; noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); fg.add(1, (Matrix(1, 1) << 1.0).finished(), 3, (Matrix(1, 1) << 2.0).finished(), 5, (Matrix(1, 1) << 3.0).finished(), (Vector(1) << 4.0).finished(), model); fg.add(1, (Matrix(1, 1) << 5.0).finished(), (Vector(1) << 6.0).finished(), model); fg.add(2, (Matrix(1, 1) << 7.0).finished(), 4, (Matrix(1, 1) << 8.0).finished(), 5, (Matrix(1, 1) << 9.0).finished(), (Vector(1) << 10.0).finished(), model); fg.add(2, (Matrix(1, 1) << 11.0).finished(), (Vector(1) << 12.0).finished(), model); fg.add(5, (Matrix(1, 1) << 13.0).finished(), 6, (Matrix(1, 1) << 14.0).finished(), (Vector(1) << 15.0).finished(), model); fg.add(6, (Matrix(1, 1) << 17.0).finished(), 7, (Matrix(1, 1) << 18.0).finished(), (Vector(1) << 19.0).finished(), model); fg.add(7, (Matrix(1, 1) << 20.0).finished(), (Vector(1) << 21.0).finished(), model); // Eliminate into BayesTree // c(6,7) // c(5|6) // c(1,2|5) // c(3,4|5) Ordering ordering(fg.keys()); GaussianBayesTree bt = *fg.eliminateMultifrontal(ordering); // eliminate in increasing key order, fg.keys() is sorted. GaussianFactorGraph joint = *bt.joint(1,2, EliminateQR); Matrix expectedJointJ = (Matrix(2,3) << 5, 0, 6, 0, -11, -12 ).finished(); Matrix actualJointJ = joint.augmentedJacobian(); EXPECT(assert_equal(expectedJointJ, actualJointJ)); }