diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h index ba776cb9a..1943cef4d 100644 --- a/cpp/BayesTree-inl.h +++ b/cpp/BayesTree-inl.h @@ -43,20 +43,68 @@ namespace gtsam { /* ************************************************************************* */ template + template typename BayesTree::sharedBayesNet - BayesTree::Clique::shortcut() { + BayesTree::Clique::shortcut(shared_ptr R) { // The shortcut density is a conditional P(S|R) of the separator of this // clique on the root. We can compute it recursively from the parent shortcut - // P(S_p|R) as \int P(F_p|S_p) P(S_p|R), where F_p are the frontal nodes in p + // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p - // The base case is when we are the root or the parent is the root, - // in which case we return an empty Bayes net - sharedBayesNet p_S_R(new BayesNet); - if (parent_==NULL || parent_->parent_==NULL) return p_S_R; + // A first base case is when this clique or its parent is the root, + // in which case we return an empty Bayes net. + if (R.get()==this || parent_==R) { + sharedBayesNet empty(new BayesNet); + return empty; + } - // If not the base case, calculate the parent shortcut P(S_p|R) - sharedBayesNet p_Sp_R = parent_->shortcut(); + // The parent clique has a Conditional for each frontal node in Fp + // so we can obtain P(Fp|Sp) in factor graph form + FactorGraph p_Fp_Sp(*parent_); + //p_Fp_Sp.print("p_Fp_Sp"); + // If not the base case, obtain the parent shortcut P(Sp|R) as factors + FactorGraph p_Sp_R(*parent_->shortcut(R)); + //p_Sp_R.print("p_Sp_R"); + + // now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R) + FactorGraph p_Cp_R = combine(p_Fp_Sp, p_Sp_R); + + // Eliminate into a Bayes net with ordering designed to integrate out + // any variables not in *our* separator. Variables to integrate out must be + // eliminated first hence the desired ordering is [Cp\S S]. + // However, an added wrinkle is that Cp might overlap with the root. + // Keys corresponding to the root should not be added to the ordering at all. + + // Get the key list Cp=Fp+Sp, which will form the basis for the integrands + Ordering integrands; + { + Ordering Fp = parent_->ordering(), Sp = parent_->separator_; + integrands.splice(integrands.end(),Fp); + integrands.splice(integrands.end(),Sp); + } + + // Start ordering with the separator + Ordering ordering = separator_; + + // remove any variables in the root, after this integrands = Cp\R, ordering = S\R + BOOST_FOREACH(string key, R->ordering()) { + integrands.remove(key); + ordering.remove(key); + } + + // remove any variables in the separator, after this integrands = Cp\R\S + BOOST_FOREACH(string key, separator_) integrands.remove(key); + + // form the ordering as [Cp\R\S S\R] + BOOST_REVERSE_FOREACH(string key, integrands) ordering.push_front(key); + + // eliminate to get marginal + sharedBayesNet p_S_R = _eliminate(p_Cp_R,ordering); + + // remove all integrands + BOOST_FOREACH(string key, integrands) p_S_R->pop_front(); + + // return the parent shortcut P(Sp|R) return p_S_R; } diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h index f898bf4a9..a1442d157 100644 --- a/cpp/BayesTree.h +++ b/cpp/BayesTree.h @@ -38,7 +38,7 @@ namespace gtsam { */ struct Clique: public BayesNet { - typedef boost::shared_ptr shared_ptr; + typedef typename boost::shared_ptr shared_ptr; shared_ptr parent_; std::list children_; std::list separator_; /** separator keys */ @@ -46,19 +46,23 @@ namespace gtsam { //* Constructor */ Clique(const sharedConditional& conditional); + /** print this node */ + void print(const std::string& s = "Bayes tree node") const; + /** The size *includes* the separator */ size_t size() const { return this->conditionals_.size() + separator_.size(); } - /** print this node */ - void print(const std::string& s = "Bayes tree node") const; + /** is this the root of a Bayes tree ? */ + inline bool isRoot() const { return parent_==NULL;} /** print this node and entire subtree below it */ void printTree(const std::string& indent) const; /** return the conditional P(S|Root) on the separator given the root */ - sharedBayesNet shortcut(); + template + sharedBayesNet shortcut(shared_ptr R); }; typedef boost::shared_ptr sharedClique; @@ -119,7 +123,7 @@ namespace gtsam { sharedClique operator[](const std::string& key) const { typename Nodes::const_iterator it = nodes_.find(key); if (it == nodes_.end()) throw(std::invalid_argument( - "BayesTree::operator['" + key + "'): key not found")); + "BayesTree::operator['" + key + "']: key not found")); sharedClique clique = it->second; return clique; } diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp index ca4bc399e..7a3da89dc 100644 --- a/cpp/testBayesTree.cpp +++ b/cpp/testBayesTree.cpp @@ -101,21 +101,54 @@ TEST( BayesTree, smoother ) Gaussian bayesTree(*chordalBayesNet); LONGS_EQUAL(7,bayesTree.size()); - // Get the conditional P(S6|Root) - Vector sigma1 = repeat(2, 0.786153); - ConditionalGaussian::shared_ptr cg(new ConditionalGaussian("x2", zero(2), eye(2), sigma1)); - BayesNet expected; expected.push_back(cg); - Gaussian::sharedClique C6 = bayesTree["x1"]; - Gaussian::sharedBayesNet actual = C6->shortcut(); - //CHECK(assert_equal(expected,*actual,1e-4)); + // Check the conditional P(Root|Root) + BayesNet empty; + Gaussian::sharedClique R = bayesTree.root(); + Gaussian::sharedBayesNet actual1 = R->shortcut(R); + CHECK(assert_equal(empty,*actual1,1e-4)); + + // Check the conditional P(C2|Root) + Gaussian::sharedClique C2 = bayesTree["x5"]; + Gaussian::sharedBayesNet actual2 = C2->shortcut(R); + CHECK(assert_equal(empty,*actual2,1e-4)); + + // Check the conditional P(C3|Root) + Vector sigma3 = repeat(2, 0.61808); + Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022); + ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian("x5", zero(2), eye(2), "x6", A56, sigma3)); + BayesNet expected3; expected3.push_back(cg3); + Gaussian::sharedClique C3 = bayesTree["x4"]; + Gaussian::sharedBayesNet actual3 = C3->shortcut(R); + CHECK(assert_equal(expected3,*actual3,1e-4)); + + // Check the conditional P(C4|Root) + Vector sigma4 = repeat(2, 0.661968); + Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067); + ConditionalGaussian::shared_ptr cg4(new ConditionalGaussian("x4", zero(2), eye(2), "x6", A46, sigma4)); + BayesNet expected4; expected4.push_back(cg4); + Gaussian::sharedClique C4 = bayesTree["x3"]; + Gaussian::sharedBayesNet actual4 = C4->shortcut(R); + CHECK(assert_equal(expected4,*actual4,1e-4)); } /* ************************************************************************* * Bayes tree for smoother with "nested dissection" ordering: - x5 x6 x4 - x3 x2 : x4 - x1 : x2 - x7 : x6 + + 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( BayesTree, balanced_smoother_marginals ) { @@ -126,27 +159,22 @@ TEST( BayesTree, balanced_smoother_marginals ) // eliminate using a "nested dissection" ordering GaussianBayesNet::shared_ptr chordalBayesNet = smoother.eliminate(ordering); - boost::shared_ptr actualSolution = chordalBayesNet->optimize(); + +// SymbolicBayesNet symbolic(*chordalBayesNet); +// symbolic.print("chordalBayesNet"); VectorConfig expectedSolution; Vector delta = zero(2); - expectedSolution.insert("x1",delta); - expectedSolution.insert("x2",delta); - expectedSolution.insert("x3",delta); - expectedSolution.insert("x4",delta); - expectedSolution.insert("x5",delta); - expectedSolution.insert("x6",delta); - expectedSolution.insert("x7",delta); + BOOST_FOREACH(string key, ordering) + expectedSolution.insert(key,delta); + boost::shared_ptr actualSolution = chordalBayesNet->optimize(); CHECK(assert_equal(expectedSolution,*actualSolution,1e-4)); // Create the Bayes tree Gaussian bayesTree(*chordalBayesNet); LONGS_EQUAL(7,bayesTree.size()); - // Check root clique - //BayesNet expected_root; - //BayesNet actual_root = bayesTree.root(); - //CHECK(assert_equal(expected_root,actual_root)); + // Marginals // Marginal will always be axis-parallel Gaussian on delta=(0,0) Matrix R = eye(2); @@ -170,6 +198,40 @@ TEST( BayesTree, balanced_smoother_marginals ) CHECK(assert_equal(expected3,*actual3,1e-4)); } +/* ************************************************************************* */ +TEST( BayesTree, balanced_smoother_shortcuts ) +{ + // Create smoother with 7 nodes + LinearFactorGraph smoother = createSmoother(7); + Ordering ordering; + ordering += "x1","x3","x5","x7","x2","x6","x4"; + + // eliminate using a "nested dissection" ordering + GaussianBayesNet::shared_ptr chordalBayesNet = smoother.eliminate(ordering); + boost::shared_ptr actualSolution = chordalBayesNet->optimize(); + + // Create the Bayes tree + Gaussian bayesTree(*chordalBayesNet); + + // Check the conditional P(Root|Root) + BayesNet empty; + Gaussian::sharedClique R = bayesTree.root(); + Gaussian::sharedBayesNet actual1 = R->shortcut(R); + CHECK(assert_equal(empty,*actual1,1e-4)); + + // Check the conditional P(C2|Root) + Gaussian::sharedClique C2 = bayesTree["x3"]; + Gaussian::sharedBayesNet actual2 = C2->shortcut(R); + CHECK(assert_equal(empty,*actual2,1e-4)); + + // Check the conditional P(C3|Root), which should be equal to P(x2|x4) + ConditionalGaussian::shared_ptr p_x2_x4 = (*chordalBayesNet)["x2"]; + BayesNet expected3; expected3.push_back(p_x2_x4); + Gaussian::sharedClique C3 = bayesTree["x1"]; + Gaussian::sharedBayesNet actual3 = C3->shortcut(R); + CHECK(assert_equal(expected3,*actual3,1e-4)); +} + /* ************************************************************************* */ int main() { TestResult tr;