/* ************************************************************************* */ TEST( GaussianBayesTree, balanced_smoother_shortcuts ) { // Create smoother with 7 nodes GaussianFactorGraph smoother = createSmoother(7); // Create the Bayes tree Ordering ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering); // Check the conditional P(Root|Root) GaussianBayesNet empty; GaussianBayesTree::sharedClique R = bayesTree.roots().front(); GaussianBayesNet actual1 = R->shortcut(R); EXPECT(assert_equal(empty,actual1,tol)); // Check the conditional P(C2|Root) GaussianBayesTree::sharedClique C2 = bayesTree[X(3)]; GaussianBayesNet actual2 = C2->shortcut(R); EXPECT(assert_equal(empty,actual2,tol)); // Check the conditional P(C3|Root), which should be equal to P(x2|x4) /** TODO: Note for multifrontal conditional: * p_x2_x4 is now an element conditional of the multifrontal conditional bayesTree[ordering[X(2)]]->conditional() * We don't know yet how to take it out. */ // GaussianConditional::shared_ptr p_x2_x4 = bayesTree[ordering[X(2)]]->conditional(); // p_x2_x4->print("Conditional p_x2_x4: "); // GaussianBayesNet expected3(p_x2_x4); // GaussianISAM::sharedClique C3 = isamTree[ordering[X(1)]]; // GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R); // EXPECT(assert_equal(expected3,actual3,tol)); }
/* ************************************************************************* */ TEST( ISAM, iSAM_smoother ) { Ordering ordering; for (int t = 1; t <= 7; t++) ordering += X(t); // Create smoother with 7 nodes GaussianFactorGraph smoother = createSmoother(7); // run iSAM for every factor GaussianISAM actual; for(boost::shared_ptr<GaussianFactor> factor: smoother) { GaussianFactorGraph factorGraph; factorGraph.push_back(factor); actual.update(factorGraph); } // Create expected Bayes Tree by solving smoother with "natural" ordering GaussianBayesTree expected = *smoother.eliminateMultifrontal(ordering); // Verify sigmas in the bayes tree for(const GaussianBayesTree::sharedClique& clique: expected.nodes() | br::map_values) { GaussianConditional::shared_ptr conditional = clique->conditional(); EXPECT(!conditional->get_model()); } // Check whether BayesTree is correct EXPECT(assert_equal(GaussianFactorGraph(expected).augmentedHessian(), GaussianFactorGraph(actual).augmentedHessian())); // obtain solution VectorValues e; // expected solution for (int t = 1; t <= 7; t++) e.insert(X(t), Vector::Zero(2)); VectorValues optimized = actual.optimize(); // actual solution EXPECT(assert_equal(e, optimized)); }
// Create a planar factor graph and eliminate double timePlanarSmootherEliminate(int N, bool old = true) { GaussianFactorGraph fg = planarGraph(N).get<0>(); clock_t start = clock(); fg.eliminateMultifrontal(); clock_t end = clock (); double dif = (double)(end - start) / CLOCKS_PER_SEC; return dif; }
/* ************************************************************************* */ TEST (Serialization, gaussian_bayes_tree) { const Key x1=1, x2=2, x3=3, x4=4; const Ordering chainOrdering = Ordering(list_of(x2)(x1)(x3)(x4)); const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5); const GaussianFactorGraph chain = list_of (JacobianFactor(x2, (Matrix(1, 1) << 1.), x1, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise)) (JacobianFactor(x2, (Matrix(1, 1) << 1.), x3, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise)) (JacobianFactor(x3, (Matrix(1, 1) << 1.), x4, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise)) (JacobianFactor(x4, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise)); GaussianBayesTree init = *chain.eliminateMultifrontal(chainOrdering); GaussianBayesTree expected = *chain.eliminateMultifrontal(chainOrdering); GaussianBayesTree actual; std::string serialized = serialize(init); deserialize(serialized, actual); EXPECT(assert_equal(expected, actual)); }
/* ************************************************************************* */ TEST( GaussianBayesTree, balanced_smoother_joint ) { // Create smoother with 7 nodes Ordering ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); GaussianFactorGraph smoother = createSmoother(7); // Create the Bayes tree, expected to look like: // x5 x6 x4 // x3 x2 : x4 // x1 : x2 // x7 : x6 GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering); // Conditional density elements reused by both tests const Matrix I = eye(2), A = -0.00429185*I; // Check the joint density P(x1,x7) factored as P(x1|x7)P(x7) GaussianBayesNet expected1 = list_of // Why does the sign get flipped on the prior? (GaussianConditional(X(1), zero(2), I/sigmax7, X(7), A/sigmax7)) (GaussianConditional(X(7), zero(2), -1*I/sigmax7)); GaussianBayesNet actual1 = *bayesTree.jointBayesNet(X(1),X(7)); EXPECT(assert_equal(expected1, actual1, tol)); // // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1) // GaussianBayesNet expected2; // GaussianConditional::shared_ptr // parent2(new GaussianConditional(X(1), zero(2), -1*I/sigmax1, ones(2))); // expected2.push_front(parent2); // push_front(expected2,X(7), zero(2), I/sigmax1, X(1), A/sigmax1, sigma); // GaussianBayesNet actual2 = *bayesTree.jointBayesNet(X(7),X(1)); // EXPECT(assert_equal(expected2,actual2,tol)); // Check the joint density P(x1,x4), i.e. with a root variable double sig14 = 0.784465; Matrix A14 = -0.0769231*I; GaussianBayesNet expected3 = list_of (GaussianConditional(X(1), zero(2), I/sig14, X(4), A14/sig14)) (GaussianConditional(X(4), zero(2), I/sigmax4)); GaussianBayesNet actual3 = *bayesTree.jointBayesNet(X(1),X(4)); EXPECT(assert_equal(expected3,actual3,tol)); // // Check the joint density P(x4,x1), i.e. with a root variable, factored the other way // GaussianBayesNet expected4; // GaussianConditional::shared_ptr // parent4(new GaussianConditional(X(1), zero(2), -1.0*I/sigmax1, ones(2))); // expected4.push_front(parent4); // double sig41 = 0.668096; // Matrix A41 = -0.055794*I; // push_front(expected4,X(4), zero(2), I/sig41, X(1), A41/sig41, sigma); // GaussianBayesNet actual4 = *bayesTree.jointBayesNet(X(4),X(1)); // EXPECT(assert_equal(expected4,actual4,tol)); }
/* ************************************************************************* * Bayes tree for smoother with "nested dissection" ordering: Node[x1] P(x1 | x2) Node[x3] P(x3 | x2 x4) Node[x5] P(x5 | x4 x6) Node[x7] P(x7 | x6) Node[x2] P(x2 | x4) Node[x6] P(x6 | x4) Node[x4] P(x4) becomes C1 x5 x6 x4 C2 x3 x2 : x4 C3 x1 : x2 C4 x7 : x6 ************************************************************************* */ TEST( GaussianBayesTree, balanced_smoother_marginals ) { // Create smoother with 7 nodes GaussianFactorGraph smoother = createSmoother(7); // Create the Bayes tree Ordering ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering); VectorValues actualSolution = bayesTree.optimize(); VectorValues expectedSolution = VectorValues::Zero(actualSolution); EXPECT(assert_equal(expectedSolution,actualSolution,tol)); LONGS_EQUAL(4, (long)bayesTree.size()); double tol=1e-5; // Check marginal on x1 JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), zero(2), sigmax1); JacobianFactor actual1 = *bayesTree.marginalFactor(X(1)); Matrix expectedCovarianceX1 = eye(2,2) * (sigmax1 * sigmax1); Matrix actualCovarianceX1; GaussianFactor::shared_ptr m = bayesTree.marginalFactor(X(1), EliminateCholesky); actualCovarianceX1 = bayesTree.marginalFactor(X(1), EliminateCholesky)->information().inverse(); EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol)); EXPECT(assert_equal(expected1,actual1,tol)); // Check marginal on x2 double sigx2 = 0.68712938; // FIXME: this should be corrected analytically JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), zero(2), sigx2); JacobianFactor actual2 = *bayesTree.marginalFactor(X(2)); EXPECT(assert_equal(expected2,actual2,tol)); // Check marginal on x3 JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), zero(2), sigmax3); JacobianFactor actual3 = *bayesTree.marginalFactor(X(3)); EXPECT(assert_equal(expected3,actual3,tol)); // Check marginal on x4 JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), zero(2), sigmax4); JacobianFactor actual4 = *bayesTree.marginalFactor(X(4)); EXPECT(assert_equal(expected4,actual4,tol)); // Check marginal on x7 (should be equal to x1) JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), zero(2), sigmax7); JacobianFactor actual7 = *bayesTree.marginalFactor(X(7)); EXPECT(assert_equal(expected7,actual7,tol)); }
/* ************************************************************************* * Bayes tree for smoother with "natural" ordering: C1 x6 x7 C2 x5 : x6 C3 x4 : x5 C4 x3 : x4 C5 x2 : x3 C6 x1 : x2 **************************************************************************** */ TEST( GaussianBayesTree, linear_smoother_shortcuts ) { // Create smoother with 7 nodes GaussianFactorGraph smoother = createSmoother(7); GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(); // Create the Bayes tree LONGS_EQUAL(6, (long)bayesTree.size()); // Check the conditional P(Root|Root) GaussianBayesNet empty; GaussianBayesTree::sharedClique R = bayesTree.roots().front(); GaussianBayesNet actual1 = R->shortcut(R); EXPECT(assert_equal(empty,actual1,tol)); // Check the conditional P(C2|Root) GaussianBayesTree::sharedClique C2 = bayesTree[X(5)]; GaussianBayesNet actual2 = C2->shortcut(R); EXPECT(assert_equal(empty,actual2,tol)); // Check the conditional P(C3|Root) double sigma3 = 0.61808; Matrix A56 = (Matrix(2,2) << -0.382022,0.,0.,-0.382022).finished(); GaussianBayesNet expected3; expected3 += GaussianConditional(X(5), zero(2), eye(2)/sigma3, X(6), A56/sigma3); GaussianBayesTree::sharedClique C3 = bayesTree[X(4)]; GaussianBayesNet actual3 = C3->shortcut(R); EXPECT(assert_equal(expected3,actual3,tol)); // Check the conditional P(C4|Root) double sigma4 = 0.661968; Matrix A46 = (Matrix(2,2) << -0.146067,0.,0.,-0.146067).finished(); GaussianBayesNet expected4; expected4 += GaussianConditional(X(4), zero(2), eye(2)/sigma4, X(6), A46/sigma4); GaussianBayesTree::sharedClique C4 = bayesTree[X(3)]; GaussianBayesNet actual4 = C4->shortcut(R); EXPECT(assert_equal(expected4,actual4,tol)); }
/* ************************************************************************* */ 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)); }