Working recursive (but not cached) shortcut.
Frank Dellaert
parent
d1b8925617
commit
58007a8167
|
|
@ -43,20 +43,68 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional>
|
||||
template<class Factor>
|
||||
typename BayesTree<Conditional>::sharedBayesNet
|
||||
BayesTree<Conditional>::Clique::shortcut() {
|
||||
BayesTree<Conditional>::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<Conditional>);
|
||||
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<Conditional>);
|
||||
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<Factor> 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<Factor> p_Sp_R(*parent_->shortcut<Factor>(R));
|
||||
//p_Sp_R.print("p_Sp_R");
|
||||
|
||||
// now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R)
|
||||
FactorGraph<Factor> 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<Factor,Conditional>(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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@ namespace gtsam {
|
|||
*/
|
||||
struct Clique: public BayesNet<Conditional> {
|
||||
|
||||
typedef boost::shared_ptr<Clique> shared_ptr;
|
||||
typedef typename boost::shared_ptr<Clique> shared_ptr;
|
||||
shared_ptr parent_;
|
||||
std::list<shared_ptr> children_;
|
||||
std::list<std::string> 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<class Factor>
|
||||
sharedBayesNet shortcut(shared_ptr R);
|
||||
};
|
||||
|
||||
typedef boost::shared_ptr<Clique> 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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<ConditionalGaussian> 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<ConditionalGaussian> empty;
|
||||
Gaussian::sharedClique R = bayesTree.root();
|
||||
Gaussian::sharedBayesNet actual1 = R->shortcut<LinearFactor>(R);
|
||||
CHECK(assert_equal(empty,*actual1,1e-4));
|
||||
|
||||
// Check the conditional P(C2|Root)
|
||||
Gaussian::sharedClique C2 = bayesTree["x5"];
|
||||
Gaussian::sharedBayesNet actual2 = C2->shortcut<LinearFactor>(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<ConditionalGaussian> expected3; expected3.push_back(cg3);
|
||||
Gaussian::sharedClique C3 = bayesTree["x4"];
|
||||
Gaussian::sharedBayesNet actual3 = C3->shortcut<LinearFactor>(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<ConditionalGaussian> expected4; expected4.push_back(cg4);
|
||||
Gaussian::sharedClique C4 = bayesTree["x3"];
|
||||
Gaussian::sharedBayesNet actual4 = C4->shortcut<LinearFactor>(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<VectorConfig> 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<VectorConfig> 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<ConditionalGaussian> expected_root;
|
||||
//BayesNet<ConditionalGaussian> 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<VectorConfig> actualSolution = chordalBayesNet->optimize();
|
||||
|
||||
// Create the Bayes tree
|
||||
Gaussian bayesTree(*chordalBayesNet);
|
||||
|
||||
// Check the conditional P(Root|Root)
|
||||
BayesNet<ConditionalGaussian> empty;
|
||||
Gaussian::sharedClique R = bayesTree.root();
|
||||
Gaussian::sharedBayesNet actual1 = R->shortcut<LinearFactor>(R);
|
||||
CHECK(assert_equal(empty,*actual1,1e-4));
|
||||
|
||||
// Check the conditional P(C2|Root)
|
||||
Gaussian::sharedClique C2 = bayesTree["x3"];
|
||||
Gaussian::sharedBayesNet actual2 = C2->shortcut<LinearFactor>(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<ConditionalGaussian> expected3; expected3.push_back(p_x2_x4);
|
||||
Gaussian::sharedClique C3 = bayesTree["x1"];
|
||||
Gaussian::sharedBayesNet actual3 = C3->shortcut<LinearFactor>(R);
|
||||
CHECK(assert_equal(expected3,*actual3,1e-4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue