diff --git a/base/FastSet.h b/base/FastSet.h index a8f8d367e..6007dae9b 100644 --- a/base/FastSet.h +++ b/base/FastSet.h @@ -31,7 +31,7 @@ namespace gtsam { * percent. */ template -class FastSet: public std::set, boost::fast_pool_allocator > { +class FastSet : public std::set, boost::fast_pool_allocator > { public: diff --git a/inference/BayesTree-inl.h b/inference/BayesTree-inl.h index e698509c3..1ad2ae97f 100644 --- a/inference/BayesTree-inl.h +++ b/inference/BayesTree-inl.h @@ -108,14 +108,7 @@ namespace gtsam { cout << "\n"; BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_) { conditional->print(" " + s + "conditional"); -// cout << " " << conditional->key(); } -// if (!separator_.empty()) { -// cout << " :"; -// BOOST_FOREACH(Index key, separator_) -// cout << " " << key; -// } -// cout << endl; } /* ************************************************************************* */ @@ -345,11 +338,11 @@ namespace gtsam { // for(Index j=0; j(p_S_R).eliminate(); } /* ************************************************************************* */ @@ -374,31 +367,33 @@ namespace gtsam { return *GenericSequentialSolver(p_FSR).joint(keys()); } -// /* ************************************************************************* */ -// // P(C1,C2) = \int_R P(F1|S1) P(S1|R) P(F2|S1) P(S2|R) P(R) -// /* ************************************************************************* */ -// template -// template -// pair, Ordering> -// BayesTree::Clique::joint(shared_ptr C2, shared_ptr R) { -// // For now, assume neither is the root -// -// // Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R) -// sharedBayesNet bn(new BayesNet); -// if (!isRoot()) bn->push_back(*this); // P(F1|S1) -// if (!isRoot()) bn->push_back(shortcut(R)); // P(S1|R) -// if (!C2->isRoot()) bn->push_back(*C2); // P(F2|S2) -// if (!C2->isRoot()) bn->push_back(C2->shortcut(R)); // P(S2|R) -// bn->push_back(*R); // P(R) -// -// // Find the keys of both C1 and C2 -// Ordering keys12 = keys(); -// BOOST_FOREACH(Index key,C2->keys()) keys12.push_back(key); -// keys12.unique(); -// -// // Calculate the marginal -// return make_pair(marginalize(*bn,keys12), keys12); -// } + /* ************************************************************************* */ + // P(C1,C2) = \int_R P(F1|S1) P(S1|R) P(F2|S1) P(S2|R) P(R) + /* ************************************************************************* */ + template + FactorGraph BayesTree::Clique::joint(shared_ptr C2, shared_ptr R) { + // For now, assume neither is the root + + // Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R) + FactorGraph joint; + if (!isRoot()) joint.push_back(*this); // P(F1|S1) + if (!isRoot()) joint.push_back(shortcut(R)); // P(S1|R) + if (!C2->isRoot()) joint.push_back(*C2); // P(F2|S2) + if (!C2->isRoot()) joint.push_back(C2->shortcut(R)); // P(S2|R) + joint.push_back(*R); // P(R) + + // Find the keys of both C1 and C2 + vector keys1(keys()); + vector keys2(C2->keys()); + FastSet keys12; + keys12.insert(keys1.begin(), keys1.end()); + keys12.insert(keys2.begin(), keys2.end()); + + // Calculate the marginal + vector keys12vector; keys12vector.reserve(keys12.size()); + keys12vector.insert(keys12vector.begin(), keys12.begin(), keys12.end()); + return *GenericSequentialSolver(joint).joint(keys12vector); + } /* ************************************************************************* */ template @@ -693,7 +688,7 @@ namespace gtsam { } // Now fill in the nodes index - if(subtree->back()->key() > (nodes_.size() - 1)) + if(nodes_.size() == 0 || subtree->back()->key() > (nodes_.size() - 1)) nodes_.resize(subtree->back()->key() + 1); fillNodesIndex(subtree); } @@ -712,79 +707,45 @@ namespace gtsam { FactorGraph cliqueMarginal = clique->marginal(root_); return GenericSequentialSolver(cliqueMarginal).marginal(key); - -// // Reorder so that only the requested key is not eliminated -// typename FACTORGRAPH::variableindex_type varIndex(cliqueMarginal); -// vector keyAsVector(1); keyAsVector[0] = key; -// Permutation toBack(Permutation::PushToBack(keyAsVector, varIndex.size())); -// Permutation::shared_ptr toBackInverse(toBack.inverse()); -// varIndex.permute(toBack); -// BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, cliqueMarginal) { -// factor->permuteWithInverse(*toBackInverse); -// } -// -// // partially eliminate, remaining factor graph is requested marginal -// SymbolicSequentialSolver::EliminateUntil(cliqueMarginal, varIndex.size()-1, varIndex); -// BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, cliqueMarginal) { -// if(factor) -// factor->permuteWithInverse(toBack); -// } -// return cliqueMarginal; } /* ************************************************************************* */ template typename BayesNet::shared_ptr BayesTree::marginalBayesNet(Index key) const { - // calculate marginal as a factor graph - typename FactorGraph::shared_ptr fg( - new FactorGraph()); - fg->push_back(this->marginal(key)); + // calculate marginal as a factor graph + FactorGraph fg; + fg.push_back(this->marginal(key)); - // eliminate further to Bayes net - return GenericSequentialSolver(*fg).eliminate(); + // eliminate factor graph marginal to a Bayes net + return GenericSequentialSolver(fg).eliminate(); } -// /* ************************************************************************* */ -// // Find two cliques, their joint, then marginalizes -// /* ************************************************************************* */ -// template -// template -// FactorGraph -// BayesTree::joint(Index key1, Index key2) const { -// -// // get clique C1 and C2 -// sharedClique C1 = (*this)[key1], C2 = (*this)[key2]; -// -// // calculate joint -// Ordering ord; -// FactorGraph p_C1C2; -// boost::tie(p_C1C2,ord) = C1->joint(C2,root_); -// -// // create an ordering where both requested keys are not eliminated -// ord.remove(key1); -// ord.remove(key2); -// -// // partially eliminate, remaining factor graph is requested joint -// // TODO, make eliminate functional -// eliminate(p_C1C2,ord); -// return p_C1C2; -// } + /* ************************************************************************* */ + // Find two cliques, their joint, then marginalizes + /* ************************************************************************* */ + template + typename FactorGraph::shared_ptr + BayesTree::joint(Index key1, Index key2) const { -// /* ************************************************************************* */ -// template -// template -// BayesNet -// BayesTree::jointBayesNet(Index key1, Index key2) const { -// -// // calculate marginal as a factor graph -// FactorGraph fg = this->joint(key1,key2); -// -// // eliminate further to Bayes net -// Ordering ordering; -// ordering += key1, key2; -// return eliminate(fg,ordering); -// } + // get clique C1 and C2 + sharedClique C1 = (*this)[key1], C2 = (*this)[key2]; + + // calculate joint + FactorGraph p_C1C2(C1->joint(C2, root_)); + + // eliminate remaining factor graph to get requested joint + vector key12(2); key12[0] = key1; key12[1] = key2; + return GenericSequentialSolver(p_C1C2).joint(key12); + } + + /* ************************************************************************* */ + template + typename BayesNet::shared_ptr BayesTree::jointBayesNet(Index key1, Index key2) const { + + // eliminate factor graph marginal to a Bayes net + return GenericSequentialSolver(*this->joint(key1, key2)).eliminate(); + } /* ************************************************************************* */ template diff --git a/inference/BayesTree.h b/inference/BayesTree.h index ff2a4eac3..220f1b45f 100644 --- a/inference/BayesTree.h +++ b/inference/BayesTree.h @@ -113,9 +113,8 @@ namespace gtsam { /** return the marginal P(C) of the clique */ FactorGraph marginal(shared_ptr root); -// /** return the joint P(C1,C2), where C1==this. TODO: not a method? */ -// template -// std::pair,Ordering> joint(shared_ptr C2, shared_ptr root); + /** return the joint P(C1,C2), where C1==this. TODO: not a method? */ + FactorGraph joint(shared_ptr C2, shared_ptr root); }; // typedef for shared pointers to cliques @@ -261,13 +260,11 @@ namespace gtsam { /** return marginal on any variable, as a Bayes Net */ typename BayesNet::shared_ptr marginalBayesNet(Index key) const; -// /** return joint on two variables */ -// template -// FactorGraph joint(Index key1, Index key2) const; -// -// /** return joint on two variables as a BayesNet */ -// template -// BayesNet jointBayesNet(Index key1, Index key2) const; + /** return joint on two variables */ + typename FactorGraph::shared_ptr joint(Index key1, Index key2) const; + + /** return joint on two variables as a BayesNet */ + typename BayesNet::shared_ptr jointBayesNet(Index key1, Index key2) const; /** * Read only with side effects diff --git a/inference/GenericMultifrontalSolver-inl.h b/inference/GenericMultifrontalSolver-inl.h index c80297682..c2c12c60c 100644 --- a/inference/GenericMultifrontalSolver-inl.h +++ b/inference/GenericMultifrontalSolver-inl.h @@ -26,6 +26,8 @@ #include +using namespace std; + namespace gtsam { /* ************************************************************************* */ @@ -42,6 +44,23 @@ GenericMultifrontalSolver::eliminate() const { return bayesTree; } +/* ************************************************************************* */ +template +typename FactorGraph::shared_ptr +GenericMultifrontalSolver::joint(const std::vector& js) const { + + // We currently have code written only for computing the + + if(js.size() != 2) + throw domain_error( + "*MultifrontalSolver::joint(js) currently can only compute joint marginals\n" + "for exactly two variables. You can call marginal to compute the\n" + "marginal for one variable. *SequentialSolver::joint(js) can compute the\n" + "joint marginal over any number of variables, so use that if necessary.\n"); + + return eliminate()->joint(js[0], js[1]); +} + /* ************************************************************************* */ template typename FACTOR::shared_ptr GenericMultifrontalSolver::marginal(Index j) const { diff --git a/inference/GenericMultifrontalSolver.h b/inference/GenericMultifrontalSolver.h index 9432e5495..91ed7fd23 100644 --- a/inference/GenericMultifrontalSolver.h +++ b/inference/GenericMultifrontalSolver.h @@ -48,6 +48,13 @@ public: */ typename JUNCTIONTREE::BayesTree::shared_ptr eliminate() const; + /** + * Compute the marginal joint over a set of variables, by integrating out + * all of the other variables. This function returns the result as a factor + * graph. + */ + typename FactorGraph::shared_ptr joint(const std::vector& js) const; + /** * Compute the marginal Gaussian density over a variable, by integrating out * all of the other variables. This function returns the result as a factor. diff --git a/inference/GenericSequentialSolver.h b/inference/GenericSequentialSolver.h index a0d406217..4369d449f 100644 --- a/inference/GenericSequentialSolver.h +++ b/inference/GenericSequentialSolver.h @@ -54,12 +54,6 @@ public: */ typename BayesNet::shared_ptr eliminate() const; - /** - * Compute the marginal Gaussian density over a variable, by integrating out - * all of the other variables. This function returns the result as a factor. - */ - typename FACTOR::shared_ptr marginal(Index j) const; - /** * Compute the marginal joint over a set of variables, by integrating out * all of the other variables. This function returns the result as a factor @@ -67,6 +61,12 @@ public: */ typename FactorGraph::shared_ptr joint(const std::vector& js) const; + /** + * Compute the marginal Gaussian density over a variable, by integrating out + * all of the other variables. This function returns the result as a factor. + */ + typename FACTOR::shared_ptr marginal(Index j) const; + }; } diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index d0a747488..c676f1695 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -278,66 +278,66 @@ TEST( BayesTree, balanced_smoother_shortcuts ) //} /* ************************************************************************* */ -// SL-FIX TEST( BayesTree, balanced_smoother_joint ) -//{ -// // Create smoother with 7 nodes -// GaussianFactorGraph smoother = createSmoother(7); -// Ordering ordering; -// ordering += "x1","x3","x5","x7","x2","x6","x4"; -// -// // Create the Bayes tree, expected to look like: -// // x5 x6 x4 -// // x3 x2 : x4 -// // x1 : x2 -// // x7 : x6 -// GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering); -// GaussianISAM bayesTree(chordalBayesNet); -// -// // Conditional density elements reused by both tests -// const Vector sigma = ones(2); -// 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; -// // Why does the sign get flipped on the prior? -// GaussianConditional::shared_ptr -// parent1(new GaussianConditional("x7", zero(2), -1*I/sigmax7, ones(2))); -// expected1.push_front(parent1); -// push_front(expected1,"x1", zero(2), I/sigmax7, "x7", A/sigmax7, sigma); -// GaussianBayesNet actual1 = bayesTree.jointBayesNet("x1","x7"); -// CHECK(assert_equal(expected1,actual1,tol)); -// +TEST( BayesTree, balanced_smoother_joint ) +{ + // Create smoother with 7 nodes + Ordering ordering; + ordering += "x1","x3","x5","x7","x2","x6","x4"; + GaussianFactorGraph smoother = createSmoother(7, ordering).first; + + // Create the Bayes tree, expected to look like: + // x5 x6 x4 + // x3 x2 : x4 + // x1 : x2 + // x7 : x6 + GaussianBayesNet chordalBayesNet = *GaussianSequentialSolver(smoother).eliminate(); + GaussianISAM bayesTree(chordalBayesNet); + + // Conditional density elements reused by both tests + const Vector sigma = ones(2); + 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; + // Why does the sign get flipped on the prior? + GaussianConditional::shared_ptr + parent1(new GaussianConditional(ordering["x7"], zero(2), -1*I/sigmax7, ones(2))); + expected1.push_front(parent1); + push_front(expected1,ordering["x1"], zero(2), I/sigmax7, ordering["x7"], A/sigmax7, sigma); + GaussianBayesNet actual1 = *bayesTree.jointBayesNet(ordering["x1"],ordering["x7"]); + CHECK(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("x1", zero(2), -1*I/sigmax1, ones(2))); +// parent2(new GaussianConditional(ordering["x1"], zero(2), -1*I/sigmax1, ones(2))); // expected2.push_front(parent2); -// push_front(expected2,"x7", zero(2), I/sigmax1, "x1", A/sigmax1, sigma); -// GaussianBayesNet actual2 = bayesTree.jointBayesNet("x7","x1"); +// push_front(expected2,ordering["x7"], zero(2), I/sigmax1, ordering["x1"], A/sigmax1, sigma); +// GaussianBayesNet actual2 = *bayesTree.jointBayesNet(ordering["x7"],ordering["x1"]); // CHECK(assert_equal(expected2,actual2,tol)); -// -// // Check the joint density P(x1,x4), i.e. with a root variable -// GaussianBayesNet expected3; -// GaussianConditional::shared_ptr -// parent3(new GaussianConditional("x4", zero(2), I/sigmax4, ones(2))); -// expected3.push_front(parent3); -// double sig14 = 0.784465; -// Matrix A14 = -0.0769231*I; -// push_front(expected3,"x1", zero(2), I/sig14, "x4", A14/sig14, sigma); -// GaussianBayesNet actual3 = bayesTree.jointBayesNet("x1","x4"); -// CHECK(assert_equal(expected3,actual3,tol)); -// + + // Check the joint density P(x1,x4), i.e. with a root variable + GaussianBayesNet expected3; + GaussianConditional::shared_ptr + parent3(new GaussianConditional(ordering["x4"], zero(2), I/sigmax4, ones(2))); + expected3.push_front(parent3); + double sig14 = 0.784465; + Matrix A14 = -0.0769231*I; + push_front(expected3,ordering["x1"], zero(2), I/sig14, ordering["x4"], A14/sig14, sigma); + GaussianBayesNet actual3 = *bayesTree.jointBayesNet(ordering["x1"],ordering["x4"]); + CHECK(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("x1", zero(2), -1.0*I/sigmax1, ones(2))); +// parent4(new GaussianConditional(ordering["x1"], zero(2), -1.0*I/sigmax1, ones(2))); // expected4.push_front(parent4); // double sig41 = 0.668096; // Matrix A41 = -0.055794*I; -// push_front(expected4,"x4", zero(2), I/sig41, "x1", A41/sig41, sigma); -// GaussianBayesNet actual4 = bayesTree.jointBayesNet("x4","x1"); +// push_front(expected4,ordering["x4"], zero(2), I/sig41, ordering["x1"], A41/sig41, sigma); +// GaussianBayesNet actual4 = *bayesTree.jointBayesNet(ordering["x4"],ordering["x1"]); // CHECK(assert_equal(expected4,actual4,tol)); -//} +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);}